LCOV - code coverage report
Current view: top level - Codec - mathutils.h (source / functions) Hit Total Coverage
Test: coverage.info Lines: 46 48 95.8 %
Date: 2019-11-25 17:12:20 Functions: 3 3 100.0 %

          Line data    Source code
       1             : /*
       2             :  * Copyright (c) 2017, Alliance for Open Media. All rights reserved
       3             :  *
       4             :  * This source code is subject to the terms of the BSD 2 Clause License and
       5             :  * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License
       6             :  * was not distributed with this source code in the LICENSE file, you can
       7             :  * obtain it at www.aomedia.org/license/software. If the Alliance for Open
       8             :  * Media Patent License 1.0 was not distributed with this source code in the
       9             :  * PATENTS file, you can obtain it at www.aomedia.org/license/patent.
      10             :  */
      11             : 
      12             : #ifndef AOM_AV1_ENCODER_MATHUTILS_H_
      13             : #define AOM_AV1_ENCODER_MATHUTILS_H_
      14             : 
      15             : #include <memory.h>
      16             : #include <math.h>
      17             : #include <stdio.h>
      18             : #include <stdlib.h>
      19             : #include <assert.h>
      20             : 
      21             : static const double TINY_NEAR_ZERO = 1.0E-16;
      22             : 
      23             : #define PI 3.141592653589793238462643383279502884
      24             : 
      25             : // Solves Ax = b, where x and b are column vectors of size nx1 and A is nxn
      26        4487 : static INLINE int32_t linsolve(int32_t n, double *A, int32_t stride, double *b, double *x) {
      27             :     int32_t i, j, k;
      28             :     double c;
      29             :     // Forward elimination
      30       17948 :     for (k = 0; k < n - 1; k++) {
      31             :         // Bring the largest magnitude to the diagonal position
      32       40383 :         for (i = n - 1; i > k; i--) {
      33       26922 :             if (fabs(A[(i - 1) * stride + k]) < fabs(A[i * stride + k])) {
      34       70090 :                 for (j = 0; j < n; j++) {
      35       56072 :                     c = A[i * stride + j];
      36       56072 :                     A[i * stride + j] = A[(i - 1) * stride + j];
      37       56072 :                     A[(i - 1) * stride + j] = c;
      38             :                 }
      39       14018 :                 c = b[i];
      40       14018 :                 b[i] = b[i - 1];
      41       14018 :                 b[i - 1] = c;
      42             :             }
      43             :         }
      44       40383 :         for (i = k; i < n - 1; i++) {
      45       26922 :             if (fabs(A[k * stride + k]) < TINY_NEAR_ZERO) return 0;
      46       26922 :             c = A[(i + 1) * stride + k] / A[k * stride + k];
      47      134610 :             for (j = 0; j < n; j++) A[(i + 1) * stride + j] -= c * A[k * stride + j];
      48       26922 :             b[i + 1] -= c * b[k];
      49             :         }
      50             :     }
      51             :     // Backward substitution
      52       22435 :     for (i = n - 1; i >= 0; i--) {
      53       17948 :         if (fabs(A[i * stride + i]) < TINY_NEAR_ZERO) return 0;
      54       17948 :         c = 0;
      55       44870 :         for (j = i + 1; j <= n - 1; j++) c += A[i * stride + j] * x[j];
      56       17948 :         x[i] = (b[i] - c) / A[i * stride + i];
      57             :     }
      58             : 
      59        4487 :     return 1;
      60             : }
      61             : 
      62             : ////////////////////////////////////////////////////////////////////////////////
      63             : // Least-squares
      64             : // Solves for n-dim x in a least squares sense to minimize |Ax - b|^2
      65             : // The solution is simply x = (A'A)^-1 A'b or simply the solution for
      66             : // the system: A'A x = A'b
      67        4487 : static INLINE int32_t least_squares(int32_t n, double *A, int32_t rows, int32_t stride,
      68             :     double *b, double *scratch, double *x) {
      69             :     int32_t i, j, k;
      70        4487 :     double *scratch_ = NULL;
      71             :     double *AtA, *Atb;
      72        4487 :     if (!scratch) {
      73           0 :         scratch_ = (double *)malloc(sizeof(*scratch) * n * (n + 1));
      74           0 :         scratch = scratch_;
      75             :     }
      76        4487 :     AtA = scratch;
      77        4487 :     Atb = scratch + n * n;
      78             : 
      79       22435 :     for (i = 0; i < n; ++i) {
      80       62818 :         for (j = i; j < n; ++j) {
      81       44870 :             AtA[i * n + j] = 0.0;
      82    94479000 :             for (k = 0; k < rows; ++k)
      83    94434100 :                 AtA[i * n + j] += A[k * stride + i] * A[k * stride + j];
      84       44870 :             AtA[j * n + i] = AtA[i * n + j];
      85             :         }
      86       17948 :         Atb[i] = 0;
      87    37791600 :         for (k = 0; k < rows; ++k) Atb[i] += A[k * stride + i] * b[k];
      88             :     }
      89        4487 :     int32_t ret = linsolve(n, AtA, n, Atb, x);
      90        4487 :     if (scratch_) free(scratch_);
      91        4487 :     return ret;
      92             : }
      93             : 
      94             : // Matrix multiply
      95        8974 : static INLINE void multiply_mat(const double *m1, const double *m2, double *res,
      96             :     const int32_t m1_rows, const int32_t inner_dim,
      97             :     const int32_t m2_cols) {
      98             :     double sum;
      99             : 
     100             :     int32_t row, col, inner;
     101       35896 :     for (row = 0; row < m1_rows; ++row) {
     102      107688 :         for (col = 0; col < m2_cols; ++col) {
     103       80766 :             sum = 0;
     104      323064 :             for (inner = 0; inner < inner_dim; ++inner)
     105      242298 :                 sum += m1[row * inner_dim + inner] * m2[inner * m2_cols + col];
     106       80766 :             *(res++) = sum;
     107             :         }
     108             :     }
     109        8974 : }
     110             : 
     111             : //
     112             : // The functions below are needed only for homography computation
     113             : // Remove if the homography models are not used.
     114             : //
     115             : ///////////////////////////////////////////////////////////////////////////////
     116             : // svdcmp
     117             : // Adopted from Numerical Recipes in C
     118             : 
     119             : static INLINE double sign(double a, double b) {
     120             :     return ((b) >= 0 ? fabs(a) : -fabs(a));
     121             : }
     122             : 
     123             : static INLINE double pythag(double a, double b) {
     124             :     double ct;
     125             :     const double absa = fabs(a);
     126             :     const double absb = fabs(b);
     127             : 
     128             :     if (absa > absb) {
     129             :         ct = absb / absa;
     130             :         return absa * sqrt(1.0 + ct * ct);
     131             :     }
     132             :     else {
     133             :         ct = absa / absb;
     134             :         return (absb == 0) ? 0 : absb * sqrt(1.0 + ct * ct);
     135             :     }
     136             : }
     137             : 
     138             : static INLINE int32_t svdcmp(double **u, int32_t m, int32_t n, double w[], double **v) {
     139             :     const int32_t max_its = 30;
     140             :     int32_t flag, i, its, j, jj, k, l, nm;
     141             :     double anorm, c, f, g, h, s, scale, x, y, z;
     142             :     double *rv1 = (double *)malloc(sizeof(*rv1) * (n + 1));
     143             :     g = scale = anorm = 0.0;
     144             :     for (i = 0; i < n; i++) {
     145             :         l = i + 1;
     146             :         rv1[i] = scale * g;
     147             :         g = s = scale = 0.0;
     148             :         if (i < m) {
     149             :             for (k = i; k < m; k++) scale += fabs(u[k][i]);
     150             :             if (scale != 0.) {
     151             :                 for (k = i; k < m; k++) {
     152             :                     u[k][i] /= scale;
     153             :                     s += u[k][i] * u[k][i];
     154             :                 }
     155             :                 f = u[i][i];
     156             :                 g = -sign(sqrt(s), f);
     157             :                 h = f * g - s;
     158             :                 u[i][i] = f - g;
     159             :                 for (j = l; j < n; j++) {
     160             :                     for (s = 0.0, k = i; k < m; k++) s += u[k][i] * u[k][j];
     161             :                     f = s / h;
     162             :                     for (k = i; k < m; k++) u[k][j] += f * u[k][i];
     163             :                 }
     164             :                 for (k = i; k < m; k++) u[k][i] *= scale;
     165             :             }
     166             :         }
     167             :         w[i] = scale * g;
     168             :         g = s = scale = 0.0;
     169             :         if (i < m && i != n - 1) {
     170             :             for (k = l; k < n; k++) scale += fabs(u[i][k]);
     171             :             if (scale != 0.) {
     172             :                 for (k = l; k < n; k++) {
     173             :                     u[i][k] /= scale;
     174             :                     s += u[i][k] * u[i][k];
     175             :                 }
     176             :                 f = u[i][l];
     177             :                 g = -sign(sqrt(s), f);
     178             :                 h = f * g - s;
     179             :                 u[i][l] = f - g;
     180             :                 for (k = l; k < n; k++) rv1[k] = u[i][k] / h;
     181             :                 for (j = l; j < m; j++) {
     182             :                     for (s = 0.0, k = l; k < n; k++) s += u[j][k] * u[i][k];
     183             :                     for (k = l; k < n; k++) u[j][k] += s * rv1[k];
     184             :                 }
     185             :                 for (k = l; k < n; k++) u[i][k] *= scale;
     186             :             }
     187             :         }
     188             :         anorm = fmax(anorm, (fabs(w[i]) + fabs(rv1[i])));
     189             :     }
     190             : 
     191             :     for (i = n - 1; i >= 0; i--) {
     192             :         if (i < n - 1) {
     193             :             if (g != 0.) {
     194             :                 for (j = l; j < n; j++) v[j][i] = (u[i][j] / u[i][l]) / g;
     195             :                 for (j = l; j < n; j++) {
     196             :                     for (s = 0.0, k = l; k < n; k++) s += u[i][k] * v[k][j];
     197             :                     for (k = l; k < n; k++) v[k][j] += s * v[k][i];
     198             :                 }
     199             :             }
     200             :             for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
     201             :         }
     202             :         v[i][i] = 1.0;
     203             :         g = rv1[i];
     204             :         l = i;
     205             :     }
     206             :     for (i = AOMMIN(m, n) - 1; i >= 0; i--) {
     207             :         l = i + 1;
     208             :         g = w[i];
     209             :         for (j = l; j < n; j++) u[i][j] = 0.0;
     210             :         if (g != 0.) {
     211             :             g = 1.0 / g;
     212             :             for (j = l; j < n; j++) {
     213             :                 for (s = 0.0, k = l; k < m; k++) s += u[k][i] * u[k][j];
     214             :                 f = (s / u[i][i]) * g;
     215             :                 for (k = i; k < m; k++) u[k][j] += f * u[k][i];
     216             :             }
     217             :             for (j = i; j < m; j++) u[j][i] *= g;
     218             :         }
     219             :         else
     220             :             for (j = i; j < m; j++) u[j][i] = 0.0;
     221             :         ++u[i][i];
     222             :     }
     223             :     for (k = n - 1; k >= 0; k--) {
     224             :         for (its = 0; its < max_its; its++) {
     225             :             flag = 1;
     226             :             for (l = k; l >= 0; l--) {
     227             :                 nm = l - 1;
     228             :                 if ((double)(fabs(rv1[l]) + anorm) == anorm || nm < 0) {
     229             :                     flag = 0;
     230             :                     break;
     231             :                 }
     232             :                 if ((double)(fabs(w[nm]) + anorm) == anorm) break;
     233             :             }
     234             :             if (flag) {
     235             :                 c = 0.0;
     236             :                 s = 1.0;
     237             :                 for (i = l; i <= k; i++) {
     238             :                     f = s * rv1[i];
     239             :                     rv1[i] = c * rv1[i];
     240             :                     if ((double)(fabs(f) + anorm) == anorm) break;
     241             :                     g = w[i];
     242             :                     h = pythag(f, g);
     243             :                     w[i] = h;
     244             :                     h = 1.0 / h;
     245             :                     c = g * h;
     246             :                     s = -f * h;
     247             :                     for (j = 0; j < m; j++) {
     248             :                         y = u[j][nm];
     249             :                         z = u[j][i];
     250             :                         u[j][nm] = y * c + z * s;
     251             :                         u[j][i] = z * c - y * s;
     252             :                     }
     253             :                 }
     254             :             }
     255             :             z = w[k];
     256             :             if (l == k) {
     257             :                 if (z < 0.0) {
     258             :                     w[k] = -z;
     259             :                     for (j = 0; j < n; j++) v[j][k] = -v[j][k];
     260             :                 }
     261             :                 break;
     262             :             }
     263             :             if (its == max_its - 1) {
     264             :                 free(rv1);
     265             :                 return 1;
     266             :             }
     267             :             assert(k > 0);
     268             :             x = w[l];
     269             :             nm = k - 1;
     270             :             y = w[nm];
     271             :             g = rv1[nm];
     272             :             h = rv1[k];
     273             :             f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
     274             :             g = pythag(f, 1.0);
     275             :             f = ((x - z) * (x + z) + h * ((y / (f + sign(g, f))) - h)) / x;
     276             :             c = s = 1.0;
     277             :             for (j = l; j <= nm; j++) {
     278             :                 i = j + 1;
     279             :                 g = rv1[i];
     280             :                 y = w[i];
     281             :                 h = s * g;
     282             :                 g = c * g;
     283             :                 z = pythag(f, h);
     284             :                 rv1[j] = z;
     285             :                 c = f / z;
     286             :                 s = h / z;
     287             :                 f = x * c + g * s;
     288             :                 g = g * c - x * s;
     289             :                 h = y * s;
     290             :                 y *= c;
     291             :                 for (jj = 0; jj < n; jj++) {
     292             :                     x = v[jj][j];
     293             :                     z = v[jj][i];
     294             :                     v[jj][j] = x * c + z * s;
     295             :                     v[jj][i] = z * c - x * s;
     296             :                 }
     297             :                 z = pythag(f, h);
     298             :                 w[j] = z;
     299             :                 if (z != 0.) {
     300             :                     z = 1.0 / z;
     301             :                     c = f * z;
     302             :                     s = h * z;
     303             :                 }
     304             :                 f = c * g + s * y;
     305             :                 x = c * y - s * g;
     306             :                 for (jj = 0; jj < m; jj++) {
     307             :                     y = u[jj][j];
     308             :                     z = u[jj][i];
     309             :                     u[jj][j] = y * c + z * s;
     310             :                     u[jj][i] = z * c - y * s;
     311             :                 }
     312             :             }
     313             :             rv1[l] = 0.0;
     314             :             rv1[k] = f;
     315             :             w[k] = x;
     316             :         }
     317             :     }
     318             :     free(rv1);
     319             :     return 0;
     320             : }
     321             : 
     322             : static INLINE int32_t SVD(double *U, double *W, double *V, double *matx, int32_t M,
     323             :     int32_t N) {
     324             :     // Assumes allocation for U is MxN
     325             :     double **nrU = (double **)malloc((M) * sizeof(*nrU));
     326             :     double **nrV = (double **)malloc((N) * sizeof(*nrV));
     327             :     int32_t problem, i;
     328             : 
     329             :     problem = !(nrU && nrV);
     330             :     if (!problem) {
     331             :         for (i = 0; i < M; i++)
     332             :             nrU[i] = &U[i * N];
     333             :         for (i = 0; i < N; i++)
     334             :             nrV[i] = &V[i * N];
     335             :     }
     336             :     else {
     337             :         if (nrU) free(nrU);
     338             :         if (nrV) free(nrV);
     339             :         return 1;
     340             :     }
     341             : 
     342             :     /* copy from given matx into nrU */
     343             :     for (i = 0; i < M; i++)
     344             :         memcpy(&(nrU[i][0]), matx + N * i, N * sizeof(*matx));
     345             :     /* HERE IT IS: do SVD */
     346             :     if (svdcmp(nrU, M, N, W, nrV)) {
     347             :         free(nrU);
     348             :         free(nrV);
     349             :         return 1;
     350             :     }
     351             : 
     352             :     /* free Numerical Recipes arrays */
     353             :     free(nrU);
     354             :     free(nrV);
     355             : 
     356             :     return 0;
     357             : }
     358             : 
     359             : #endif  // AOM_AV1_ENCODER_MATHUTILS_H_

Generated by: LCOV version 1.14