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