Removed extra traces of svd that were accidently left in previously
Richard Roberts
parent
e1b0f7b238
commit
141a23ae6a
|
|
@ -14,7 +14,7 @@ check_PROGRAMS =
|
||||||
# base Math
|
# base Math
|
||||||
|
|
||||||
headers += FixedVector.h types.h blockMatrices.h Matrix-inl.h
|
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
|
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix
|
||||||
|
|
||||||
if USE_LAPACK
|
if USE_LAPACK
|
||||||
|
|
|
||||||
|
|
@ -47,7 +47,6 @@ extern "C" {
|
||||||
|
|
||||||
#include <gtsam/base/Matrix-inl.h>
|
#include <gtsam/base/Matrix-inl.h>
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/base/svdcmp.h>
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
namespace ublas = boost::numeric::ublas;
|
namespace ublas = boost::numeric::ublas;
|
||||||
|
|
|
||||||
|
|
@ -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 <stdexcept>
|
|
||||||
#include <math.h> /* for 'fabs' */
|
|
||||||
#include <iostream>
|
|
||||||
#include <vector>
|
|
||||||
#include <gtsam/base/Matrix.h>
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
|
|
@ -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);
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue