Redo / refactor affine and rot-zoom least squares
Use a simpler least-squares function for affine and rotzoom
model estimation, instead of computing the pseudo inverse.
Also refactors the code into a separate mathutils.h file.
The SVD code is currently used only for estimation of the
homography models which can be removed when we remove the
homography models.
Coding efficiency change is in noise range, with the small
difference coming from numerical precision issues.
Change-Id: I0a9eb79495911cea21a7945b397d596e22a2a186
diff --git a/av1/av1_cx.mk b/av1/av1_cx.mk
index 0a0d770..f61d0aa 100644
--- a/av1/av1_cx.mk
+++ b/av1/av1_cx.mk
@@ -38,6 +38,7 @@
AV1_CX_SRCS-yes += encoder/ethread.c
AV1_CX_SRCS-yes += encoder/extend.c
AV1_CX_SRCS-yes += encoder/firstpass.c
+AV1_CX_SRCS-yes += encoder/mathutils.h
AV1_CX_SRCS-$(CONFIG_GLOBAL_MOTION) += ../third_party/fastfeat/fast.h
AV1_CX_SRCS-$(CONFIG_GLOBAL_MOTION) += ../third_party/fastfeat/nonmax.c
AV1_CX_SRCS-$(CONFIG_GLOBAL_MOTION) += ../third_party/fastfeat/fast_9.c
diff --git a/av1/encoder/mathutils.h b/av1/encoder/mathutils.h
new file mode 100644
index 0000000..23243dd
--- /dev/null
+++ b/av1/encoder/mathutils.h
@@ -0,0 +1,354 @@
+/*
+ * Copyright (c) 2017, Alliance for Open Media. All rights reserved
+ *
+ * This source code is subject to the terms of the BSD 2 Clause License and
+ * the Alliance for Open Media Patent License 1.0. If the BSD 2 Clause License
+ * was not distributed with this source code in the LICENSE file, you can
+ * obtain it at www.aomedia.org/license/software. If the Alliance for Open
+ * Media Patent License 1.0 was not distributed with this source code in the
+ * PATENTS file, you can obtain it at www.aomedia.org/license/patent.
+ */
+
+#include <memory.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+
+static const double TINY_NEAR_ZERO = 1.0E-16;
+
+// Solves Ax = b, where x and b are column vectors of size nx1 and A is nxn
+static INLINE int linsolve(int n, double *A, int stride, double *b, double *x) {
+ int i, j, k;
+ double c;
+ // Forward elimination
+ for (k = 0; k < n - 1; k++) {
+ // Bring the largest magitude to the diagonal position
+ for (i = n - 1; i > k; i--) {
+ if (fabs(A[(i - 1) * stride + k]) < fabs(A[i * stride + k])) {
+ for (j = 0; j < n; j++) {
+ c = A[i * stride + j];
+ A[i * stride + j] = A[(i - 1) * stride + j];
+ A[(i - 1) * stride + j] = c;
+ }
+ c = b[i];
+ b[i] = b[i - 1];
+ b[i - 1] = c;
+ }
+ }
+ for (i = k; i < n - 1; i++) {
+ if (fabs(A[k * stride + k]) < TINY_NEAR_ZERO) return 0;
+ c = A[(i + 1) * stride + k] / A[k * stride + k];
+ for (j = 0; j < n; j++) A[(i + 1) * stride + j] -= c * A[k * stride + j];
+ b[i + 1] -= c * b[k];
+ }
+ }
+ // Backward substitution
+ for (i = n - 1; i >= 0; i--) {
+ if (fabs(A[i * stride + i]) < TINY_NEAR_ZERO) return 0;
+ c = 0;
+ for (j = i + 1; j <= n - 1; j++) c += A[i * stride + j] * x[j];
+ x[i] = (b[i] - c) / A[i * stride + i];
+ }
+
+ return 1;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Least-squares
+// Solves for n-dim x in a least squares sense to minimize |Ax - b|^2
+// The solution is simply x = (A'A)^-1 A'b or simply the solution for
+// the system: A'A x = A'b
+static INLINE int least_squares(int n, double *A, int rows, int stride,
+ double *b, double *scratch, double *x) {
+ int i, j, k;
+ double *scratch_ = NULL;
+ double *AtA, *Atb;
+ if (!scratch) {
+ scratch_ = (double *)aom_malloc(sizeof(*scratch) * n * (n + 1));
+ scratch = scratch_;
+ }
+ AtA = scratch;
+ Atb = scratch + n * n;
+
+ for (i = 0; i < n; ++i) {
+ for (j = i; j < n; ++j) {
+ AtA[i * n + j] = 0.0;
+ for (k = 0; k < rows; ++k)
+ AtA[i * n + j] += A[k * stride + i] * A[k * stride + j];
+ AtA[j * n + i] = AtA[i * n + j];
+ }
+ Atb[i] = 0;
+ for (k = 0; k < rows; ++k) Atb[i] += A[k * stride + i] * b[k];
+ }
+ int ret = linsolve(n, AtA, n, Atb, x);
+ if (scratch_) aom_free(scratch_);
+ return ret;
+}
+
+// Matrix multiply
+static INLINE void multiply_mat(const double *m1, const double *m2, double *res,
+ const int m1_rows, const int inner_dim,
+ const int m2_cols) {
+ double sum;
+
+ int row, col, inner;
+ for (row = 0; row < m1_rows; ++row) {
+ for (col = 0; col < m2_cols; ++col) {
+ sum = 0;
+ for (inner = 0; inner < inner_dim; ++inner)
+ sum += m1[row * inner_dim + inner] * m2[inner * m2_cols + col];
+ *(res++) = sum;
+ }
+ }
+}
+
+//
+// The functions below are needed only for homography computation
+// Remove if the homography models are not used.
+//
+///////////////////////////////////////////////////////////////////////////////
+// svdcmp
+// Adopted from Numerical Recipes in C
+
+static INLINE double sign(double a, double b) {
+ return ((b) >= 0 ? fabs(a) : -fabs(a));
+}
+
+static INLINE double pythag(double a, double b) {
+ double ct;
+ const double absa = fabs(a);
+ const double absb = fabs(b);
+
+ if (absa > absb) {
+ ct = absb / absa;
+ return absa * sqrt(1.0 + ct * ct);
+ } else {
+ ct = absa / absb;
+ return (absb == 0) ? 0 : absb * sqrt(1.0 + ct * ct);
+ }
+}
+
+static INLINE int svdcmp(double **u, int m, int n, double w[], double **v) {
+ const int max_its = 30;
+ int flag, i, its, j, jj, k, l, nm;
+ double anorm, c, f, g, h, s, scale, x, y, z;
+ double *rv1 = (double *)aom_malloc(sizeof(*rv1) * (n + 1));
+ g = scale = anorm = 0.0;
+ for (i = 0; i < n; i++) {
+ l = i + 1;
+ rv1[i] = scale * g;
+ g = s = scale = 0.0;
+ if (i < m) {
+ for (k = i; k < m; k++) scale += fabs(u[k][i]);
+ if (scale != 0.) {
+ for (k = i; k < m; k++) {
+ u[k][i] /= scale;
+ s += u[k][i] * u[k][i];
+ }
+ f = u[i][i];
+ g = -sign(sqrt(s), f);
+ h = f * g - s;
+ u[i][i] = f - g;
+ for (j = l; j < n; j++) {
+ for (s = 0.0, k = i; k < m; k++) s += u[k][i] * u[k][j];
+ f = s / h;
+ for (k = i; k < m; k++) u[k][j] += f * u[k][i];
+ }
+ for (k = i; k < m; k++) u[k][i] *= scale;
+ }
+ }
+ w[i] = scale * g;
+ g = s = scale = 0.0;
+ if (i < m && i != n - 1) {
+ for (k = l; k < n; k++) scale += fabs(u[i][k]);
+ if (scale != 0.) {
+ for (k = l; k < n; k++) {
+ u[i][k] /= scale;
+ s += u[i][k] * u[i][k];
+ }
+ f = u[i][l];
+ g = -sign(sqrt(s), f);
+ h = f * g - s;
+ u[i][l] = f - g;
+ for (k = l; k < n; k++) rv1[k] = u[i][k] / h;
+ for (j = l; j < m; j++) {
+ for (s = 0.0, k = l; k < n; k++) s += u[j][k] * u[i][k];
+ for (k = l; k < n; k++) u[j][k] += s * rv1[k];
+ }
+ for (k = l; k < n; k++) u[i][k] *= scale;
+ }
+ }
+ anorm = fmax(anorm, (fabs(w[i]) + fabs(rv1[i])));
+ }
+
+ for (i = n - 1; i >= 0; i--) {
+ if (i < n - 1) {
+ if (g != 0.) {
+ for (j = l; j < n; j++) v[j][i] = (u[i][j] / u[i][l]) / g;
+ for (j = l; j < n; j++) {
+ for (s = 0.0, k = l; k < n; k++) s += u[i][k] * v[k][j];
+ for (k = l; k < n; k++) v[k][j] += s * v[k][i];
+ }
+ }
+ for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
+ }
+ v[i][i] = 1.0;
+ g = rv1[i];
+ l = i;
+ }
+ for (i = AOMMIN(m, n) - 1; i >= 0; i--) {
+ l = i + 1;
+ g = w[i];
+ for (j = l; j < n; j++) u[i][j] = 0.0;
+ if (g != 0.) {
+ g = 1.0 / g;
+ for (j = l; j < n; j++) {
+ for (s = 0.0, k = l; k < m; k++) s += u[k][i] * u[k][j];
+ f = (s / u[i][i]) * g;
+ for (k = i; k < m; k++) u[k][j] += f * u[k][i];
+ }
+ for (j = i; j < m; j++) u[j][i] *= g;
+ } else {
+ for (j = i; j < m; j++) u[j][i] = 0.0;
+ }
+ ++u[i][i];
+ }
+ for (k = n - 1; k >= 0; k--) {
+ for (its = 0; its < max_its; its++) {
+ flag = 1;
+ for (l = k; l >= 0; l--) {
+ nm = l - 1;
+ if ((double)(fabs(rv1[l]) + anorm) == anorm || nm < 0) {
+ flag = 0;
+ break;
+ }
+ if ((double)(fabs(w[nm]) + anorm) == anorm) break;
+ }
+ if (flag) {
+ c = 0.0;
+ s = 1.0;
+ for (i = l; i <= k; i++) {
+ f = s * rv1[i];
+ rv1[i] = c * rv1[i];
+ if ((double)(fabs(f) + anorm) == anorm) break;
+ g = w[i];
+ h = pythag(f, g);
+ w[i] = h;
+ h = 1.0 / h;
+ c = g * h;
+ s = -f * h;
+ for (j = 0; j < m; j++) {
+ y = u[j][nm];
+ z = u[j][i];
+ u[j][nm] = y * c + z * s;
+ u[j][i] = z * c - y * s;
+ }
+ }
+ }
+ z = w[k];
+ if (l == k) {
+ if (z < 0.0) {
+ w[k] = -z;
+ for (j = 0; j < n; j++) v[j][k] = -v[j][k];
+ }
+ break;
+ }
+ if (its == max_its - 1) {
+ aom_free(rv1);
+ return 1;
+ }
+ assert(k > 0);
+ x = w[l];
+ nm = k - 1;
+ y = w[nm];
+ g = rv1[nm];
+ h = rv1[k];
+ f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
+ g = pythag(f, 1.0);
+ f = ((x - z) * (x + z) + h * ((y / (f + sign(g, f))) - h)) / x;
+ c = s = 1.0;
+ for (j = l; j <= nm; j++) {
+ i = j + 1;
+ g = rv1[i];
+ y = w[i];
+ h = s * g;
+ g = c * g;
+ z = pythag(f, h);
+ rv1[j] = z;
+ c = f / z;
+ s = h / z;
+ f = x * c + g * s;
+ g = g * c - x * s;
+ h = y * s;
+ y *= c;
+ for (jj = 0; jj < n; jj++) {
+ x = v[jj][j];
+ z = v[jj][i];
+ v[jj][j] = x * c + z * s;
+ v[jj][i] = z * c - x * s;
+ }
+ z = pythag(f, h);
+ w[j] = z;
+ if (z != 0.) {
+ z = 1.0 / z;
+ c = f * z;
+ s = h * z;
+ }
+ f = c * g + s * y;
+ x = c * y - s * g;
+ for (jj = 0; jj < m; jj++) {
+ y = u[jj][j];
+ z = u[jj][i];
+ u[jj][j] = y * c + z * s;
+ u[jj][i] = z * c - y * s;
+ }
+ }
+ rv1[l] = 0.0;
+ rv1[k] = f;
+ w[k] = x;
+ }
+ }
+ aom_free(rv1);
+ return 0;
+}
+
+static INLINE int SVD(double *U, double *W, double *V, double *matx, int M,
+ int N) {
+ // Assumes allocation for U is MxN
+ double **nrU = (double **)aom_malloc((M) * sizeof(*nrU));
+ double **nrV = (double **)aom_malloc((N) * sizeof(*nrV));
+ int problem, i;
+
+ problem = !(nrU && nrV);
+ if (!problem) {
+ for (i = 0; i < M; i++) {
+ nrU[i] = &U[i * N];
+ }
+ for (i = 0; i < N; i++) {
+ nrV[i] = &V[i * N];
+ }
+ } else {
+ if (nrU) aom_free(nrU);
+ if (nrV) aom_free(nrV);
+ return 1;
+ }
+
+ /* copy from given matx into nrU */
+ for (i = 0; i < M; i++) {
+ memcpy(&(nrU[i][0]), matx + N * i, N * sizeof(*matx));
+ }
+
+ /* HERE IT IS: do SVD */
+ if (svdcmp(nrU, M, N, W, nrV)) {
+ aom_free(nrU);
+ aom_free(nrV);
+ return 1;
+ }
+
+ /* aom_free Numerical Recipes arrays */
+ aom_free(nrU);
+ aom_free(nrV);
+
+ return 0;
+}
diff --git a/av1/encoder/pickrst.c b/av1/encoder/pickrst.c
index 21410e0..0cd72c0 100644
--- a/av1/encoder/pickrst.c
+++ b/av1/encoder/pickrst.c
@@ -31,6 +31,7 @@
#include "av1/encoder/encoder.h"
#include "av1/encoder/picklpf.h"
#include "av1/encoder/pickrst.h"
+#include "av1/encoder/mathutils.h"
// When set to RESTORE_WIENER or RESTORE_SGRPROJ only those are allowed.
// When set to RESTORE_NONE (0) we allow switchable.
@@ -329,8 +330,10 @@
#if CONFIG_HIGHBITDEPTH
}
#endif
+ aom_clear_system_state();
get_proj_subspace(src8, width, height, src_stride, dat8, dat_stride,
bit_depth, flt1, width, flt2, width, exq);
+ aom_clear_system_state();
encode_xq(exq, exqd);
err =
get_pixel_proj_error(src8, width, height, src_stride, dat8, dat_stride,
@@ -560,46 +563,6 @@
}
#endif // CONFIG_HIGHBITDEPTH
-// Solves Ax = b, where x and b are column vectors
-static int linsolve(int n, double *A, int stride, double *b, double *x) {
- int i, j, k;
- double c;
-
- aom_clear_system_state();
-
- // Forward elimination
- for (k = 0; k < n - 1; k++) {
- // Bring the largest magitude to the diagonal position
- for (i = n - 1; i > k; i--) {
- if (fabs(A[(i - 1) * stride + k]) < fabs(A[i * stride + k])) {
- for (j = 0; j < n; j++) {
- c = A[i * stride + j];
- A[i * stride + j] = A[(i - 1) * stride + j];
- A[(i - 1) * stride + j] = c;
- }
- c = b[i];
- b[i] = b[i - 1];
- b[i - 1] = c;
- }
- }
- for (i = k; i < n - 1; i++) {
- if (fabs(A[k * stride + k]) < 1e-10) return 0;
- c = A[(i + 1) * stride + k] / A[k * stride + k];
- for (j = 0; j < n; j++) A[(i + 1) * stride + j] -= c * A[k * stride + j];
- b[i + 1] -= c * b[k];
- }
- }
- // Backward substitution
- for (i = n - 1; i >= 0; i--) {
- if (fabs(A[i * stride + i]) < 1e-10) return 0;
- c = 0;
- for (j = i + 1; j <= n - 1; j++) c += A[i * stride + j] * x[j];
- x[i] = (b[i] - c) / A[i * stride + i];
- }
-
- return 1;
-}
-
static INLINE int wrap_index(int i) {
return (i >= WIENER_HALFWIN1 ? WIENER_WIN - 1 - i : i);
}
@@ -910,6 +873,7 @@
type[tile_idx] = RESTORE_NONE;
continue;
}
+ aom_clear_system_state();
rsi[plane].restoration_type[tile_idx] = RESTORE_WIENER;
err = try_restoration_tile(src, cpi, rsi, 1 << plane, partial_frame,
@@ -1052,6 +1016,7 @@
type[tile_idx] = RESTORE_NONE;
continue;
}
+ aom_clear_system_state();
rsi->restoration_type[tile_idx] = RESTORE_WIENER;
err = try_restoration_tile(src, cpi, rsi, 1, partial_frame, tile_idx, 0, 0,
diff --git a/av1/encoder/ransac.c b/av1/encoder/ransac.c
index f7188f1..b9b1d3b 100644
--- a/av1/encoder/ransac.c
+++ b/av1/encoder/ransac.c
@@ -16,6 +16,7 @@
#include <assert.h>
#include "av1/encoder/ransac.h"
+#include "av1/encoder/mathutils.h"
#define MAX_MINPTS 4
#define MAX_DEGENERATE_ITER 10
@@ -132,309 +133,6 @@
}
}
-///////////////////////////////////////////////////////////////////////////////
-// svdcmp
-// Adopted from Numerical Recipes in C
-
-static const double TINY_NEAR_ZERO = 1.0E-12;
-
-static INLINE double sign(double a, double b) {
- return ((b) >= 0 ? fabs(a) : -fabs(a));
-}
-
-static INLINE double pythag(double a, double b) {
- double ct;
- const double absa = fabs(a);
- const double absb = fabs(b);
-
- if (absa > absb) {
- ct = absb / absa;
- return absa * sqrt(1.0 + ct * ct);
- } else {
- ct = absa / absb;
- return (absb == 0) ? 0 : absb * sqrt(1.0 + ct * ct);
- }
-}
-
-static void multiply_mat(const double *m1, const double *m2, double *res,
- const int m1_rows, const int inner_dim,
- const int m2_cols) {
- double sum;
-
- int row, col, inner;
- for (row = 0; row < m1_rows; ++row) {
- for (col = 0; col < m2_cols; ++col) {
- sum = 0;
- for (inner = 0; inner < inner_dim; ++inner)
- sum += m1[row * inner_dim + inner] * m2[inner * m2_cols + col];
- *(res++) = sum;
- }
- }
-}
-
-static int svdcmp(double **u, int m, int n, double w[], double **v) {
- const int max_its = 30;
- int flag, i, its, j, jj, k, l, nm;
- double anorm, c, f, g, h, s, scale, x, y, z;
- double *rv1 = (double *)aom_malloc(sizeof(*rv1) * (n + 1));
- g = scale = anorm = 0.0;
- for (i = 0; i < n; i++) {
- l = i + 1;
- rv1[i] = scale * g;
- g = s = scale = 0.0;
- if (i < m) {
- for (k = i; k < m; k++) scale += fabs(u[k][i]);
- if (scale != 0.) {
- for (k = i; k < m; k++) {
- u[k][i] /= scale;
- s += u[k][i] * u[k][i];
- }
- f = u[i][i];
- g = -sign(sqrt(s), f);
- h = f * g - s;
- u[i][i] = f - g;
- for (j = l; j < n; j++) {
- for (s = 0.0, k = i; k < m; k++) s += u[k][i] * u[k][j];
- f = s / h;
- for (k = i; k < m; k++) u[k][j] += f * u[k][i];
- }
- for (k = i; k < m; k++) u[k][i] *= scale;
- }
- }
- w[i] = scale * g;
- g = s = scale = 0.0;
- if (i < m && i != n - 1) {
- for (k = l; k < n; k++) scale += fabs(u[i][k]);
- if (scale != 0.) {
- for (k = l; k < n; k++) {
- u[i][k] /= scale;
- s += u[i][k] * u[i][k];
- }
- f = u[i][l];
- g = -sign(sqrt(s), f);
- h = f * g - s;
- u[i][l] = f - g;
- for (k = l; k < n; k++) rv1[k] = u[i][k] / h;
- for (j = l; j < m; j++) {
- for (s = 0.0, k = l; k < n; k++) s += u[j][k] * u[i][k];
- for (k = l; k < n; k++) u[j][k] += s * rv1[k];
- }
- for (k = l; k < n; k++) u[i][k] *= scale;
- }
- }
- anorm = fmax(anorm, (fabs(w[i]) + fabs(rv1[i])));
- }
-
- for (i = n - 1; i >= 0; i--) {
- if (i < n - 1) {
- if (g != 0.) {
- for (j = l; j < n; j++) v[j][i] = (u[i][j] / u[i][l]) / g;
- for (j = l; j < n; j++) {
- for (s = 0.0, k = l; k < n; k++) s += u[i][k] * v[k][j];
- for (k = l; k < n; k++) v[k][j] += s * v[k][i];
- }
- }
- for (j = l; j < n; j++) v[i][j] = v[j][i] = 0.0;
- }
- v[i][i] = 1.0;
- g = rv1[i];
- l = i;
- }
- for (i = AOMMIN(m, n) - 1; i >= 0; i--) {
- l = i + 1;
- g = w[i];
- for (j = l; j < n; j++) u[i][j] = 0.0;
- if (g != 0.) {
- g = 1.0 / g;
- for (j = l; j < n; j++) {
- for (s = 0.0, k = l; k < m; k++) s += u[k][i] * u[k][j];
- f = (s / u[i][i]) * g;
- for (k = i; k < m; k++) u[k][j] += f * u[k][i];
- }
- for (j = i; j < m; j++) u[j][i] *= g;
- } else {
- for (j = i; j < m; j++) u[j][i] = 0.0;
- }
- ++u[i][i];
- }
- for (k = n - 1; k >= 0; k--) {
- for (its = 0; its < max_its; its++) {
- flag = 1;
- for (l = k; l >= 0; l--) {
- nm = l - 1;
- if ((double)(fabs(rv1[l]) + anorm) == anorm || nm < 0) {
- flag = 0;
- break;
- }
- if ((double)(fabs(w[nm]) + anorm) == anorm) break;
- }
- if (flag) {
- c = 0.0;
- s = 1.0;
- for (i = l; i <= k; i++) {
- f = s * rv1[i];
- rv1[i] = c * rv1[i];
- if ((double)(fabs(f) + anorm) == anorm) break;
- g = w[i];
- h = pythag(f, g);
- w[i] = h;
- h = 1.0 / h;
- c = g * h;
- s = -f * h;
- for (j = 0; j < m; j++) {
- y = u[j][nm];
- z = u[j][i];
- u[j][nm] = y * c + z * s;
- u[j][i] = z * c - y * s;
- }
- }
- }
- z = w[k];
- if (l == k) {
- if (z < 0.0) {
- w[k] = -z;
- for (j = 0; j < n; j++) v[j][k] = -v[j][k];
- }
- break;
- }
- if (its == max_its - 1) {
- aom_free(rv1);
- return 1;
- }
- assert(k > 0);
- x = w[l];
- nm = k - 1;
- y = w[nm];
- g = rv1[nm];
- h = rv1[k];
- f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
- g = pythag(f, 1.0);
- f = ((x - z) * (x + z) + h * ((y / (f + sign(g, f))) - h)) / x;
- c = s = 1.0;
- for (j = l; j <= nm; j++) {
- i = j + 1;
- g = rv1[i];
- y = w[i];
- h = s * g;
- g = c * g;
- z = pythag(f, h);
- rv1[j] = z;
- c = f / z;
- s = h / z;
- f = x * c + g * s;
- g = g * c - x * s;
- h = y * s;
- y *= c;
- for (jj = 0; jj < n; jj++) {
- x = v[jj][j];
- z = v[jj][i];
- v[jj][j] = x * c + z * s;
- v[jj][i] = z * c - x * s;
- }
- z = pythag(f, h);
- w[j] = z;
- if (z != 0.) {
- z = 1.0 / z;
- c = f * z;
- s = h * z;
- }
- f = c * g + s * y;
- x = c * y - s * g;
- for (jj = 0; jj < m; jj++) {
- y = u[jj][j];
- z = u[jj][i];
- u[jj][j] = y * c + z * s;
- u[jj][i] = z * c - y * s;
- }
- }
- rv1[l] = 0.0;
- rv1[k] = f;
- w[k] = x;
- }
- }
- aom_free(rv1);
- return 0;
-}
-
-static int SVD(double *U, double *W, double *V, double *matx, int M, int N) {
- // Assumes allocation for U is MxN
- double **nrU = (double **)aom_malloc((M) * sizeof(*nrU));
- double **nrV = (double **)aom_malloc((N) * sizeof(*nrV));
- int problem, i;
-
- problem = !(nrU && nrV);
- if (!problem) {
- for (i = 0; i < M; i++) {
- nrU[i] = &U[i * N];
- }
- for (i = 0; i < N; i++) {
- nrV[i] = &V[i * N];
- }
- } else {
- if (nrU) aom_free(nrU);
- if (nrV) aom_free(nrV);
- return 1;
- }
-
- /* copy from given matx into nrU */
- for (i = 0; i < M; i++) {
- memcpy(&(nrU[i][0]), matx + N * i, N * sizeof(*matx));
- }
-
- /* HERE IT IS: do SVD */
- if (svdcmp(nrU, M, N, W, nrV)) {
- aom_free(nrU);
- aom_free(nrV);
- return 1;
- }
-
- /* aom_free Numerical Recipes arrays */
- aom_free(nrU);
- aom_free(nrV);
-
- return 0;
-}
-
-int pseudo_inverse(double *inv, double *matx, const int M, const int N) {
- double ans;
- int i, j, k;
- double *const U = (double *)aom_malloc(M * N * sizeof(*matx));
- double *const W = (double *)aom_malloc(N * sizeof(*matx));
- double *const V = (double *)aom_malloc(N * N * sizeof(*matx));
-
- if (!(U && W && V)) {
- return 1;
- }
- if (SVD(U, W, V, matx, M, N)) {
- aom_free(U);
- aom_free(W);
- aom_free(V);
- return 1;
- }
- for (i = 0; i < N; i++) {
- if (fabs(W[i]) < TINY_NEAR_ZERO) {
- aom_free(U);
- aom_free(W);
- aom_free(V);
- return 1;
- }
- }
-
- for (i = 0; i < N; i++) {
- for (j = 0; j < M; j++) {
- ans = 0;
- for (k = 0; k < N; k++) {
- ans += V[k + N * i] * U[k + N * j] / W[k];
- }
- inv[j + M * i] = ans;
- }
- }
- aom_free(U);
- aom_free(W);
- aom_free(V);
- return 0;
-}
-
static void normalize_homography(double *pts, int n, double *T) {
double *p = pts;
double mean[2] = { 0, 0 };
@@ -596,7 +294,7 @@
static int find_rotzoom(int np, double *pts1, double *pts2, double *mat) {
const int np2 = np * 2;
- double *a = (double *)aom_malloc(sizeof(*a) * np2 * 9);
+ double *a = (double *)aom_malloc(sizeof(*a) * np2 * 10);
double *b = a + np2 * 4;
double *temp = b + np2;
int i;
@@ -624,11 +322,10 @@
b[2 * i] = dx;
b[2 * i + 1] = dy;
}
- if (pseudo_inverse(temp, a, np2, 4)) {
+ if (!least_squares(4, a, np2, 4, b, temp, mat)) {
aom_free(a);
return 1;
}
- multiply_mat(temp, b, mat, 4, np2, 1);
denormalize_rotzoom_reorder(mat, T1, T2);
aom_free(a);
return 0;
@@ -636,7 +333,7 @@
static int find_affine(int np, double *pts1, double *pts2, double *mat) {
const int np2 = np * 2;
- double *a = (double *)aom_malloc(sizeof(*a) * np2 * 13);
+ double *a = (double *)aom_malloc(sizeof(*a) * np2 * 14);
double *b = a + np2 * 6;
double *temp = b + np2;
int i;
@@ -668,11 +365,10 @@
b[2 * i] = dx;
b[2 * i + 1] = dy;
}
- if (pseudo_inverse(temp, a, np2, 6)) {
+ if (!least_squares(6, a, np2, 6, b, temp, mat)) {
aom_free(a);
return 1;
}
- multiply_mat(temp, b, mat, 6, np2, 1);
denormalize_affine_reorder(mat, T1, T2);
aom_free(a);
return 0;
