diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am index 57098af9d..48777a829 100644 --- a/gtsam/base/Makefile.am +++ b/gtsam/base/Makefile.am @@ -14,7 +14,7 @@ check_PROGRAMS = # base Math headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h -sources += Vector.cpp svdcmp.cpp Matrix.cpp +sources += Vector.cpp Matrix.cpp check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix if USE_LAPACK diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index a27f3bf82..e3f05a8d6 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -47,7 +47,6 @@ extern "C" { #include #include -#include using namespace std; namespace ublas = boost::numeric::ublas; diff --git a/gtsam/base/svdcmp.cpp b/gtsam/base/svdcmp.cpp deleted file mode 100644 index fd793ad0f..000000000 --- a/gtsam/base/svdcmp.cpp +++ /dev/null @@ -1,295 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file svdcmp.cpp - * @brief SVD decomposition adapted from NRC - * @author Alireza Fathi - * @author Frank Dellaert - */ -#include -#include /* for 'fabs' */ -#include -#include -#include - -using namespace std; - -#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) -static double sqrarg; -#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg) -static double maxarg1, maxarg2; -#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\ - (maxarg1) : (maxarg2)) -static int iminarg1, iminarg2; -#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\ - (iminarg1) : (iminarg2)) - - -/* ************************************************************************* */ -double pythag(double a, double b) { - double absa = 0.0, absb = 0.0; - absa = fabs(a); - absb = fabs(b); - if (absa > absb) - return absa * sqrt(1.0 + SQR(absb/absa)); - else { - if (absb == 0.0) - return 0.0; - else - return (absb * sqrt(1.0 + SQR(absa/absb))); - } -} - -/* ************************************************************************* */ -void svdcmp(double **a, int m, int n, double w[], double **v, bool sort) { - int flag, i, its, j, jj, k, l, nm; - double anorm, c, f, g, h, s, scale, x, y, z; - - //vector sizes: - // w[n] - q-1 passed in - // a[m] - u-1 passed in - // v[n] - v-1 passed in - - //Current progress on verifying array bounds: - // rv1 references have been fixed - - double *rv1 = new double[n]; - - g = 0.0; - scale = 0.0; - anorm = 0.0; - for (i = 1; i <= n; i++) { - l = i + 1; - rv1[i - 1] = scale * g; - g = s = scale = 0.0; - if (i <= m) { - for (k = i; k <= m; k++) - scale += fabs(a[k][i]); - if (scale) { - for (k = i; k <= m; k++) { - a[k][i] /= scale; - s += a[k][i] * a[k][i]; - } - f = a[i][i]; - g = -SIGN(sqrt(s),f); - h = f * g - s; - a[i][i] = f - g; - for (j = l; j <= n; j++) { - for (s = 0.0, k = i; k <= m; k++) - s += a[k][i] * a[k][j]; - f = s / h; - for (k = i; k <= m; k++) - a[k][j] += f * a[k][i]; - } - for (k = i; k <= m; k++) - a[k][i] *= scale; - } - } - w[i] = scale * g; - g = s = scale = 0.0; - if (i <= m && i != n) { - for (k = l; k <= n; k++) - scale += fabs(a[i][k]); - if (scale) { - for (k = l; k <= n; k++) { - a[i][k] /= scale; - s += a[i][k] * a[i][k]; - } - f = a[i][l]; - g = -SIGN(sqrt(s),f); - h = f * g - s; - a[i][l] = f - g; - for (k = l; k <= n; k++) { - rv1[k - 1] = a[i][k] / h; - } - - for (j = l; j <= m; j++) { - for (s = 0.0, k = l; k <= n; k++) - s += a[j][k] * a[i][k]; - for (k = l; k <= n; k++) { - a[j][k] += s * rv1[k - 1]; - } - } - for (k = l; k <= n; k++) - a[i][k] *= scale; - } - } - anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1]))); - - } - for (i = n; i >= 1; i--) { - if (i < n) { - if (g) { - for (j = l; j <= n; j++) - v[j][i] = (a[i][j] / a[i][l]) / g; - for (j = l; j <= n; j++) { - for (s = 0.0, k = l; k <= n; k++) - s += a[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 - 1]; - l = i; - } - for (i = IMIN(m,n); i >= 1; i--) { - l = i + 1; - g = w[i]; - for (j = l; j <= n; j++) - a[i][j] = 0.0; - if (g) { - g = 1.0 / g; - for (j = l; j <= n; j++) { - for (s = 0.0, k = l; k <= m; k++) - s += a[k][i] * a[k][j]; - f = (s / a[i][i]) * g; - for (k = i; k <= m; k++) - a[k][j] += f * a[k][i]; - } - for (j = i; j <= m; j++) - a[j][i] *= g; - } else - for (j = i; j <= m; j++) - a[j][i] = 0.0; - ++a[i][i]; - } - for (k = n; k >= 1; k--) { - for (its = 1; its <= 30; its++) { - flag = 1; - for (l = k; l >= 1; l--) { - nm = l - 1; - if ((double) (fabs(rv1[l - 1]) + anorm) == anorm) { - 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 - 1]; - rv1[i - 1] = c * rv1[i - 1]; - 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 = 1; j <= m; j++) { - y = a[j][nm]; - z = a[j][i]; - a[j][nm] = y * c + z * s; - a[j][i] = z * c - y * s; - } - } - } - z = w[k]; - if (l == k) { - if (z < 0.0) { - w[k] = -z; - for (j = 1; j <= n; j++) - v[j][k] = -v[j][k]; - } - break; - } - if (its == 30) - throw(std::domain_error( - "no convergence in 30 svdcmp iterations")); - x = w[l]; - nm = k - 1; - y = w[nm]; - g = rv1[nm - 1]; - h = rv1[k - 1]; - 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 - 1]; - y = w[i]; - h = s * g; - g = c * g; - z = pythag(f, h); - rv1[j - 1] = z; - c = f / z; - s = h / z; - f = x * c + g * s; - g = g * c - x * s; - h = y * s; - y *= c; - for (jj = 1; 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) { - z = 1.0 / z; - c = f * z; - s = h * z; - } - f = c * g + s * y; - x = c * y - s * g; - for (jj = 1; jj <= m; jj++) { - y = a[jj][j]; - z = a[jj][i]; - a[jj][j] = y * c + z * s; - a[jj][i] = z * c - y * s; - } - } - rv1[l - 1] = 0.0; - rv1[k - 1] = f; - w[k] = x; - } - } - - if (sort) { - for (int i1 = 1; i1 <= n; i1++) { - for (int i2 = i1+1; i2 <= n; i2++) { - if (w[i1] < w[i2]) { - // sort S - double temp = w[i1]; - w[i1] = w[i2]; - w[i2] = temp; - // sort V - for (int j = 1; j <= n; j++) { - double temp1 = v[j][i1]; - v[j][i1] = v[j][i2]; - v[j][i2] = temp1; - } - // sort U - for (int j = 1; j <= m; j++) { - double temp1 = a[j][i1]; - a[j][i1] = a[j][i2]; - a[j][i2] = temp1; - } - } - } - } - } - - delete[] rv1; - -} - -/* ************************************************************************* */ diff --git a/gtsam/base/svdcmp.h b/gtsam/base/svdcmp.h deleted file mode 100644 index a0b5f486d..000000000 --- a/gtsam/base/svdcmp.h +++ /dev/null @@ -1,25 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file svdcmp.h - * @brief SVD decomposition adapted from NRC - * @author Alireza Fathi - * @author Frank Dellaert - */ - -// \callgraph - -#pragma once - -/** SVD decomposition */ -void svdcmp(double **a, int m, int n, double w[], double **v, bool sort = true); -