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