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_
|