12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added default bool option to svd to sort the singular values and V. the default is true so pass false to avoid sorting

release/4.3a0
Manohar Paluri 2010-02-14 04:54:39 +00:00
parent f9c2000847
commit 693e13ef88
5 changed files with 741 additions and 664 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cpp/Matrix.cpp
View File

@ -866,7 +866,7 @@ Matrix cholesky_inverse(const Matrix &A)
/* ************************************************************************* */ /* ************************************************************************* */
// version with in place modification of A // version with in place modification of A
void svd(Matrix& A, Vector& s, Matrix& V) { void svd(Matrix& A, Vector& s, Matrix& V, bool sort) {
const size_t m=A.size1(), n=A.size2(); const size_t m=A.size1(), n=A.size2();
@ -878,7 +878,7 @@ void svd(Matrix& A, Vector& s, Matrix& V) {
// perform SVD // perform SVD
// need to pass pointer - 1 in NRC routines so u[1][1] is first element ! // need to pass pointer - 1 in NRC routines so u[1][1] is first element !
svdcmp(u-1,m,n,q-1,v-1); svdcmp(u-1,m,n,q-1,v-1, sort);
// copy singular values back // copy singular values back
s.resize(n); s.resize(n);
@ -891,9 +891,9 @@ void svd(Matrix& A, Vector& s, Matrix& V) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V) { void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V, bool sort) {
U = A; // copy U = A; // copy
svd(U,s,V); // call in-place version svd(U,s,V,sort); // call in-place version
} }
#if 0 #if 0
@ -941,7 +941,7 @@ Matrix square_root_positive(const Matrix& A) {
// Perform SVD, TODO: symmetric SVD? // Perform SVD, TODO: symmetric SVD?
Matrix U,V; Matrix U,V;
Vector S; Vector S;
svd(A,U,S,V); svd(A,U,S,V,false);
// invert and sqrt diagonal of S // invert and sqrt diagonal of S
// We also arbitrarily choose sign to make result have positive signs // We also arbitrarily choose sign to make result have positive signs

5
cpp/Matrix.h
View File

@ -308,11 +308,12 @@ inline Matrix skewSymmetric(const Vector& w) { return skewSymmetric(w(0),w(1),w(
* @param U output argument: m*n matrix * @param U output argument: m*n matrix
* @param S output argument: n-dim vector of singular values, *not* sorted !!! * @param S output argument: n-dim vector of singular values, *not* sorted !!!
* @param V output argument: n*n matrix * @param V output argument: n*n matrix
* @param sort boolean flag to sort singular values and V
*/ */
void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V); void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V, bool sort=true);
// in-place version // in-place version
void svd(Matrix& A, Vector& S, Matrix& V); void svd(Matrix& A, Vector& S, Matrix& V, bool sort=true);
/** Use SVD to calculate inverse square root of a matrix */ /** Use SVD to calculate inverse square root of a matrix */
Matrix inverse_square_root(const Matrix& A); Matrix inverse_square_root(const Matrix& A);

110
cpp/svdcmp.cpp
View File

@ -8,10 +8,10 @@
#include <math.h> /* for 'fabs' */ #include <math.h> /* for 'fabs' */
#include <iostream> #include <iostream>
#include <vector> #include <vector>
#include "Matrix.h"
using namespace std; using namespace std;
#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
static double sqrarg; static double sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg) #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
@ -22,7 +22,6 @@ static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\ #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(iminarg1) : (iminarg2)) (iminarg1) : (iminarg2))
/* ************************************************************************* */ /* ************************************************************************* */
/* /*
double pythag(double a, double b) double pythag(double a, double b)
@ -36,12 +35,12 @@ double pythag(double a, double b)
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
double pythag(double a, double b) double pythag(double a, double b) {
{
double absa = 0.0, absb = 0.0; double absa = 0.0, absb = 0.0;
absa = fabs(a); absa = fabs(a);
absb = fabs(b); absb = fabs(b);
if (absa > absb) return absa*sqrt(1.0+SQR(absb/absa)); if (absa > absb)
return absa * sqrt(1.0 + SQR(absb/absa));
else { else {
if (absb == 0.0) if (absb == 0.0)
return 0.0; return 0.0;
@ -51,8 +50,7 @@ double pythag(double a, double b)
} }
/* ************************************************************************* */ /* ************************************************************************* */
void svdcmp(double **a, int m, int n, double w[], double **v) void svdcmp(double **a, int m, int n, double w[], double **v, bool sort) {
{
int flag, i, its, j, jj, k, l, nm; int flag, i, its, j, jj, k, l, nm;
double anorm, c, f, g, h, s, scale, x, y, z; double anorm, c, f, g, h, s, scale, x, y, z;
@ -74,7 +72,8 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
rv1[i - 1] = scale * g; rv1[i - 1] = scale * g;
g = s = scale = 0.0; g = s = scale = 0.0;
if (i <= m) { if (i <= m) {
for (k=i;k<=m;k++) scale += fabs(a[k][i]); for (k = i; k <= m; k++)
scale += fabs(a[k][i]);
if (scale) { if (scale) {
for (k = i; k <= m; k++) { for (k = i; k <= m; k++) {
a[k][i] /= scale; a[k][i] /= scale;
@ -85,17 +84,21 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
h = f * g - s; h = f * g - s;
a[i][i] = f - g; a[i][i] = f - g;
for (j = l; j <= n; j++) { for (j = l; j <= n; j++) {
for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j]; for (s = 0.0, k = i; k <= m; k++)
s += a[k][i] * a[k][j];
f = s / h; f = s / h;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i]; for (k = i; k <= m; k++)
a[k][j] += f * a[k][i];
} }
for (k=i;k<=m;k++) a[k][i] *= scale; for (k = i; k <= m; k++)
a[k][i] *= scale;
} }
} }
w[i] = scale * g; w[i] = scale * g;
g = s = scale = 0.0; g = s = scale = 0.0;
if (i <= m && i != n) { if (i <= m && i != n) {
for (k=l;k<=n;k++) scale += fabs(a[i][k]); for (k = l; k <= n; k++)
scale += fabs(a[i][k]);
if (scale) { if (scale) {
for (k = l; k <= n; k++) { for (k = l; k <= n; k++) {
a[i][k] /= scale; a[i][k] /= scale;
@ -105,20 +108,19 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
g = -SIGN(sqrt(s),f); g = -SIGN(sqrt(s),f);
h = f * g - s; h = f * g - s;
a[i][l] = f - g; a[i][l] = f - g;
for (k=l;k<=n;k++) for (k = l; k <= n; k++) {
{
rv1[k - 1] = a[i][k] / h; rv1[k - 1] = a[i][k] / h;
} }
for (j = l; j <= m; j++) { for (j = l; j <= m; j++) {
for (s = 0.0, k = l; k <= n; k++) for (s = 0.0, k = l; k <= n; k++)
s += a[j][k] * a[i][k]; s += a[j][k] * a[i][k];
for (k=l;k<=n;k++) for (k = l; k <= n; k++) {
{
a[j][k] += s * rv1[k - 1]; a[j][k] += s * rv1[k - 1];
} }
} }
for (k=l;k<=n;k++) a[i][k] *= scale; for (k = l; k <= n; k++)
a[i][k] *= scale;
} }
} }
anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1]))); anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i-1])));
@ -130,11 +132,14 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
for (j = l; j <= n; j++) for (j = l; j <= n; j++)
v[j][i] = (a[i][j] / a[i][l]) / g; v[j][i] = (a[i][j] / a[i][l]) / g;
for (j = l; j <= n; j++) { for (j = l; j <= n; j++) {
for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j]; for (s = 0.0, k = l; k <= n; k++)
for (k=l;k<=n;k++) v[k][j] += s*v[k][i]; 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; for (j = l; j <= n; j++)
v[i][j] = v[j][i] = 0.0;
} }
v[i][i] = 1.0; v[i][i] = 1.0;
g = rv1[i - 1]; g = rv1[i - 1];
@ -143,16 +148,22 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
for (i = IMIN(m,n); i >= 1; i--) { for (i = IMIN(m,n); i >= 1; i--) {
l = i + 1; l = i + 1;
g = w[i]; g = w[i];
for (j=l;j<=n;j++) a[i][j]=0.0; for (j = l; j <= n; j++)
a[i][j] = 0.0;
if (g) { if (g) {
g = 1.0 / g; g = 1.0 / g;
for (j = l; j <= n; j++) { for (j = l; j <= n; j++) {
for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j]; for (s = 0.0, k = l; k <= m; k++)
s += a[k][i] * a[k][j];
f = (s / a[i][i]) * g; f = (s / a[i][i]) * g;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i]; for (k = i; k <= m; k++)
a[k][j] += f * a[k][i];
} }
for (j=i;j<=m;j++) a[j][i] *= g; for (j = i; j <= m; j++)
} else for (j=i;j<=m;j++) a[j][i]=0.0; a[j][i] *= g;
} else
for (j = i; j <= m; j++)
a[j][i] = 0.0;
++a[i][i]; ++a[i][i];
} }
for (k = n; k >= 1; k--) { for (k = n; k >= 1; k--) {
@ -164,7 +175,8 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
flag = 0; flag = 0;
break; break;
} }
if ((double)(fabs(w[nm])+anorm) == anorm) break; if ((double) (fabs(w[nm]) + anorm) == anorm)
break;
} }
if (flag) { if (flag) {
c = 0.0; c = 0.0;
@ -172,7 +184,8 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
for (i = l; i <= k; i++) { for (i = l; i <= k; i++) {
f = s * rv1[i - 1]; f = s * rv1[i - 1];
rv1[i - 1] = c * rv1[i - 1]; rv1[i - 1] = c * rv1[i - 1];
if ((double)(fabs(f)+anorm) == anorm) break; if ((double) (fabs(f) + anorm) == anorm)
break;
g = w[i]; g = w[i];
h = pythag(f, g); h = pythag(f, g);
w[i] = h; w[i] = h;
@ -191,11 +204,14 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
if (l == k) { if (l == k) {
if (z < 0.0) { if (z < 0.0) {
w[k] = -z; w[k] = -z;
for (j=1;j<=n;j++) v[j][k] = -v[j][k]; for (j = 1; j <= n; j++)
v[j][k] = -v[j][k];
} }
break; break;
} }
if (its == 30) throw(std::domain_error("no convergence in 30 svdcmp iterations")); if (its == 30)
throw(std::domain_error(
"no convergence in 30 svdcmp iterations"));
x = w[l]; x = w[l];
nm = k - 1; nm = k - 1;
y = w[nm]; y = w[nm];
@ -247,6 +263,42 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
} }
} }
if (sort) {
int *indices1 = new int[n];
int *indices2 = new int[n];
for (int i1 = 1; i1 <= n; i1++) {
indices1[i1] = i1;
indices2[i1] = i1;
}
for (int i1 = 1; i1 <= n; i1++) {
for (int i2 = i1; i2 <= n; i2++) {
if (w[i1] < w[i2]) {
double temp = w[i1];
w[i1] = w[i2];
w[i2] = temp;
int ind = indices2[i1];
indices2[i1] = indices2[i2];
indices2[i2] = ind;
}
}
}
for (int i1 = 1; i1 <= n; i1++) {
if (indices1[i1] != indices2[i1]) {
int src = indices1[i1], dst = indices2[i1];
for (int j = 1; j <= n; j++) {
double temp = v[j][src];
v[j][src] = v[j][dst];
v[j][dst] = temp;
}
indices1[dst] = src;
}
}
delete[] indices1;
delete[] indices2;
}
delete[] rv1; delete[] rv1;
} }

2
cpp/svdcmp.h
View File

@ -10,5 +10,5 @@
#pragma once #pragma once
/** SVD decomposition */ /** SVD decomposition */
void svdcmp(double **a, int m, int n, double w[], double **v); void svdcmp(double **a, int m, int n, double w[], double **v, bool sort = true);

630
cpp/testMatrix.cpp
View File

@ -21,13 +21,14 @@ static double inf = std::numeric_limits<double>::infinity();
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, constructor_data ) TEST( matrix, constructor_data )
{ {
double data[] = {-5, 3, double data[] = { -5, 3, 0, -5 };
0, -5 };
Matrix A = Matrix_(2, 2, data); Matrix A = Matrix_(2, 2, data);
Matrix B(2, 2); Matrix B(2, 2);
B(0,0) = -5 ; B(0,1) = 3; B(0, 0) = -5;
B(1,0) = 0 ; B(1,1) = -5; B(0, 1) = 3;
B(1, 0) = 0;
B(1, 1) = -5;
EQUALITY(A,B); EQUALITY(A,B);
} }
@ -36,10 +37,10 @@ TEST( matrix, constructor_data )
TEST( matrix, constructor_vector ) TEST( matrix, constructor_vector )
{ {
double data[] = {-5, 3, double data[] = { -5, 3, 0, -5 };
0, -5 };
Matrix A = Matrix_(2, 2, data); Matrix A = Matrix_(2, 2, data);
Vector v(4); copy(data,data+4,v.begin()); Vector v(4);
copy(data, data + 4, v.begin());
Matrix B = Matrix_(2, 2, v); // this one is column order ! Matrix B = Matrix_(2, 2, v); // this one is column order !
EQUALITY(A,trans(B)); EQUALITY(A,trans(B));
} }
@ -47,12 +48,12 @@ TEST( matrix, constructor_vector )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, Matrix_ ) TEST( matrix, Matrix_ )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0);
-5.0 , 3.0,
00.0, -5.0 );
Matrix B(2, 2); Matrix B(2, 2);
B(0,0) = -5 ; B(0,1) = 3; B(0, 0) = -5;
B(1,0) = 0 ; B(1,1) = -5; B(0, 1) = 3;
B(1, 0) = 0;
B(1, 1) = -5;
EQUALITY(A,B); EQUALITY(A,B);
@ -61,9 +62,7 @@ TEST( matrix, Matrix_ )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, row_major ) TEST( matrix, row_major )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
1.0, 2.0,
3.0, 4.0 );
const double * const a = &A(0, 0); const double * const a = &A(0, 0);
CHECK(a[0] == 1); CHECK(a[0] == 1);
CHECK(a[1] == 2); CHECK(a[1] == 2);
@ -74,16 +73,16 @@ TEST( matrix, row_major )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, collect1 ) TEST( matrix, collect1 )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0);
-5.0 , 3.0, Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
00.0, -5.0 );
Matrix B = Matrix_(2,3,
-0.5 , 2.1, 1.1,
3.4 , 2.6 , 7.1);
Matrix AB = collect(2, &A, &B); Matrix AB = collect(2, &A, &B);
Matrix C(2, 5); Matrix C(2, 5);
for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++) C(i,j) = A(i,j); for (int i = 0; i < 2; i++)
for(int i = 0; i < 2; i++) for(int j = 0; j < 3; j++) C(i,j+2) = B(i,j); for (int j = 0; j < 2; j++)
C(i, j) = A(i, j);
for (int i = 0; i < 2; i++)
for (int j = 0; j < 3; j++)
C(i, j + 2) = B(i, j);
EQUALITY(C,AB); EQUALITY(C,AB);
@ -92,19 +91,19 @@ TEST( matrix, collect1 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, collect2 ) TEST( matrix, collect2 )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0);
-5.0 , 3.0, Matrix B = Matrix_(2, 3, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
00.0, -5.0 );
Matrix B = Matrix_(2,3,
-0.5 , 2.1, 1.1,
3.4 , 2.6 , 7.1);
vector<const Matrix*> matrices; vector<const Matrix*> matrices;
matrices.push_back(&A); matrices.push_back(&A);
matrices.push_back(&B); matrices.push_back(&B);
Matrix AB = collect(matrices); Matrix AB = collect(matrices);
Matrix C(2, 5); Matrix C(2, 5);
for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++) C(i,j) = A(i,j); for (int i = 0; i < 2; i++)
for(int i = 0; i < 2; i++) for(int j = 0; j < 3; j++) C(i,j+2) = B(i,j); for (int j = 0; j < 2; j++)
C(i, j) = A(i, j);
for (int i = 0; i < 2; i++)
for (int j = 0; j < 3; j++)
C(i, j + 2) = B(i, j);
EQUALITY(C,AB); EQUALITY(C,AB);
@ -120,9 +119,8 @@ TEST( matrix, collect3 )
matrices.push_back(&A); matrices.push_back(&A);
matrices.push_back(&B); matrices.push_back(&B);
Matrix AB = collect(matrices, 2, 3); Matrix AB = collect(matrices, 2, 3);
Matrix exp = Matrix_(2, 6, Matrix exp = Matrix_(2, 6, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0);
0.0, 1.0, 0.0, 0.0, 1.0, 0.0);
EQUALITY(exp,AB); EQUALITY(exp,AB);
} }
@ -130,17 +128,16 @@ TEST( matrix, collect3 )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, stack ) TEST( matrix, stack )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, -5.0, 3.0, 00.0, -5.0);
-5.0 , 3.0, Matrix B = Matrix_(3, 2, -0.5, 2.1, 1.1, 3.4, 2.6, 7.1);
00.0, -5.0 );
Matrix B = Matrix_(3,2,
-0.5 , 2.1,
1.1, 3.4 ,
2.6 , 7.1);
Matrix AB = stack(2, &A, &B); Matrix AB = stack(2, &A, &B);
Matrix C(5, 2); Matrix C(5, 2);
for(int i = 0; i < 2; i++) for(int j = 0; j < 2; j++) C(i,j) = A(i,j); for (int i = 0; i < 2; i++)
for(int i = 0; i < 3; i++) for(int j = 0; j < 2; j++) C(i+2,j) = B(i,j); for (int j = 0; j < 2; j++)
C(i, j) = A(i, j);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 2; j++)
C(i + 2, j) = B(i, j);
EQUALITY(C,AB); EQUALITY(C,AB);
} }
@ -148,11 +145,9 @@ TEST( matrix, stack )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, column ) TEST( matrix, column )
{ {
Matrix A = Matrix_(4, 7, Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
-1., 0., 1., 0., 0., 0., -0.2, 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
0., -1., 0., 1., 0., 0., 0.3, -0.1);
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Vector a1 = column_(A, 0); Vector a1 = column_(A, 0);
Vector exp1 = Vector_(4, -1., 0., 1., 0.); Vector exp1 = Vector_(4, -1., 0., 1., 0.);
CHECK(assert_equal(a1, exp1)); CHECK(assert_equal(a1, exp1));
@ -175,12 +170,9 @@ TEST( matrix, insert_column )
insertColumn(big, col, j); insertColumn(big, col, j);
Matrix expected = Matrix_(5,6, Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0);
CHECK(assert_equal(expected, big)); CHECK(assert_equal(expected, big));
} }
@ -195,12 +187,9 @@ TEST( matrix, insert_subcolumn )
insertColumn(big, col, i, j); insertColumn(big, col, i, j);
Matrix expected = Matrix_(5,6, Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
CHECK(assert_equal(expected, big)); CHECK(assert_equal(expected, big));
} }
@ -208,11 +197,9 @@ TEST( matrix, insert_subcolumn )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, row ) TEST( matrix, row )
{ {
Matrix A = Matrix_(4, 7, Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
-1., 0., 1., 0., 0., 0., -0.2, 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
0., -1., 0., 1., 0., 0., 0.3, -0.1);
1., 0., 0., 0., -1., 0., 0.2,
0., 1., 0., 0., 0., -1., -0.1);
Vector a1 = row_(A, 0); Vector a1 = row_(A, 0);
Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2);
CHECK(assert_equal(a1, exp1)); CHECK(assert_equal(a1, exp1));
@ -230,8 +217,12 @@ TEST( matrix, row )
TEST( matrix, zeros ) TEST( matrix, zeros )
{ {
Matrix A(2, 3); Matrix A(2, 3);
A(0,0) = 0 ; A(0,1) = 0; A(0,2) = 0; A(0, 0) = 0;
A(1,0) = 0 ; A(1,1) = 0; A(1,2) = 0; A(0, 1) = 0;
A(0, 2) = 0;
A(1, 0) = 0;
A(1, 1) = 0;
A(1, 2) = 0;
Matrix zero = zeros(2, 3); Matrix zero = zeros(2, 3);
@ -241,17 +232,14 @@ TEST( matrix, zeros )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, insert_sub ) TEST( matrix, insert_sub )
{ {
Matrix big = zeros(5,6), Matrix big = zeros(5, 6), small = Matrix_(2, 3, 1.0, 1.0, 1.0, 1.0, 1.0,
small = Matrix_(2,3, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0); 1.0);
insertSub(big, small, 1, 2); insertSub(big, small, 1, 2);
Matrix expected = Matrix_(5,6, Matrix expected = Matrix_(5, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
0.0, 0.0, 1.0, 1.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
CHECK(assert_equal(expected, big)); CHECK(assert_equal(expected, big));
} }
@ -260,18 +248,36 @@ TEST( matrix, insert_sub )
TEST( matrix, scale_columns ) TEST( matrix, scale_columns )
{ {
Matrix A(3, 4); Matrix A(3, 4);
A(0,0) = 1.; A(0,1) = 1.; A(0,2)= 1.; A(0,3)= 1.; A(0, 0) = 1.;
A(1,0) = 1.; A(1,1) = 1.; A(1,2)= 1.; A(1,3)= 1.; A(0, 1) = 1.;
A(2,0) = 1.; A(2,1) = 1.; A(2,2)= 1.; A(2,3)= 1.; A(0, 2) = 1.;
A(0, 3) = 1.;
A(1, 0) = 1.;
A(1, 1) = 1.;
A(1, 2) = 1.;
A(1, 3) = 1.;
A(2, 0) = 1.;
A(2, 1) = 1.;
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = Vector_(4, 2., 3., 4., 5.); Vector v = Vector_(4, 2., 3., 4., 5.);
Matrix actual = vector_scale(A, v); Matrix actual = vector_scale(A, v);
Matrix expected(3, 4); Matrix expected(3, 4);
expected(0,0) = 2.; expected(0,1) = 3.; expected(0,2)= 4.; expected(0,3)= 5.; expected(0, 0) = 2.;
expected(1,0) = 2.; expected(1,1) = 3.; expected(1,2)= 4.; expected(1,3)= 5.; expected(0, 1) = 3.;
expected(2,0) = 2.; expected(2,1) = 3.; expected(2,2)= 4.; expected(2,3)= 5.; expected(0, 2) = 4.;
expected(0, 3) = 5.;
expected(1, 0) = 2.;
expected(1, 1) = 3.;
expected(1, 2) = 4.;
expected(1, 3) = 5.;
expected(2, 0) = 2.;
expected(2, 1) = 3.;
expected(2, 2) = 4.;
expected(2, 3) = 5.;
CHECK(assert_equal(actual, expected)); CHECK(assert_equal(actual, expected));
} }
@ -280,18 +286,36 @@ TEST( matrix, scale_columns )
TEST( matrix, scale_rows ) TEST( matrix, scale_rows )
{ {
Matrix A(3, 4); Matrix A(3, 4);
A(0,0) = 1.; A(0,1) = 1.; A(0,2)= 1.; A(0,3)= 1.; A(0, 0) = 1.;
A(1,0) = 1.; A(1,1) = 1.; A(1,2)= 1.; A(1,3)= 1.; A(0, 1) = 1.;
A(2,0) = 1.; A(2,1) = 1.; A(2,2)= 1.; A(2,3)= 1.; A(0, 2) = 1.;
A(0, 3) = 1.;
A(1, 0) = 1.;
A(1, 1) = 1.;
A(1, 2) = 1.;
A(1, 3) = 1.;
A(2, 0) = 1.;
A(2, 1) = 1.;
A(2, 2) = 1.;
A(2, 3) = 1.;
Vector v = Vector_(3, 2., 3., 4.); Vector v = Vector_(3, 2., 3., 4.);
Matrix actual = vector_scale(v, A); Matrix actual = vector_scale(v, A);
Matrix expected(3, 4); Matrix expected(3, 4);
expected(0,0) = 2.; expected(0,1) = 2.; expected(0,2)= 2.; expected(0,3)= 2.; expected(0, 0) = 2.;
expected(1,0) = 3.; expected(1,1) = 3.; expected(1,2)= 3.; expected(1,3)= 3.; expected(0, 1) = 2.;
expected(2,0) = 4.; expected(2,1) = 4.; expected(2,2)= 4.; expected(2,3)= 4.; expected(0, 2) = 2.;
expected(0, 3) = 2.;
expected(1, 0) = 3.;
expected(1, 1) = 3.;
expected(1, 2) = 3.;
expected(1, 3) = 3.;
expected(2, 0) = 4.;
expected(2, 1) = 4.;
expected(2, 2) = 4.;
expected(2, 3) = 4.;
CHECK(assert_equal(actual, expected)); CHECK(assert_equal(actual, expected));
} }
@ -300,10 +324,22 @@ TEST( matrix, scale_rows )
TEST( matrix, equal ) TEST( matrix, equal )
{ {
Matrix A(4, 4); Matrix A(4, 4);
A(0,0) = -1; A(0,1) = 1; A(0,2)= 2; A(0,3)= 3; A(0, 0) = -1;
A(1,0) = 1; A(1,1) =-3; A(1,2)= 1; A(1,3)= 3; A(0, 1) = 1;
A(2,0) = 1; A(2,1) = 2; A(2,2)=-1; A(2,3)= 4; A(0, 2) = 2;
A(3,0) = 2; A(3,1) = 1; A(3,2)= 2; A(3,3)=-2; A(0, 3) = 3;
A(1, 0) = 1;
A(1, 1) = -3;
A(1, 2) = 1;
A(1, 3) = 3;
A(2, 0) = 1;
A(2, 1) = 2;
A(2, 2) = -1;
A(2, 3) = 4;
A(3, 0) = 2;
A(3, 1) = 1;
A(3, 2) = 2;
A(3, 3) = -2;
Matrix A2(A); Matrix A2(A);
@ -318,10 +354,22 @@ TEST( matrix, equal )
TEST( matrix, equal_nan ) TEST( matrix, equal_nan )
{ {
Matrix A(4, 4); Matrix A(4, 4);
A(0,0) = -1; A(0,1) = 1; A(0,2)= 2; A(0,3)= 3; A(0, 0) = -1;
A(1,0) = 1; A(1,1) =-3; A(1,2)= 1; A(1,3)= 3; A(0, 1) = 1;
A(2,0) = 1; A(2,1) = 2; A(2,2)=-1; A(2,3)= 4; A(0, 2) = 2;
A(3,0) = 2; A(3,1) = 1; A(3,2)= 2; A(3,3)=-2; A(0, 3) = 3;
A(1, 0) = 1;
A(1, 1) = -3;
A(1, 2) = 1;
A(1, 3) = 3;
A(2, 0) = 1;
A(2, 1) = 2;
A(2, 2) = -1;
A(2, 3) = 4;
A(3, 0) = 2;
A(3, 1) = 1;
A(3, 2) = 2;
A(3, 3) = -2;
Matrix A2(A); Matrix A2(A);
@ -334,30 +382,18 @@ TEST( matrix, equal_nan )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, addition ) TEST( matrix, addition )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
1.0, 2.0, Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0);
3.0, 4.0); Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0);
Matrix B = Matrix_(2,2,
4.0, 3.0,
2.0, 1.0);
Matrix C = Matrix_(2,2,
5.0, 5.0,
5.0, 5.0);
EQUALITY(A+B,C); EQUALITY(A+B,C);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, addition_in_place ) TEST( matrix, addition_in_place )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
1.0, 2.0, Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0);
3.0, 4.0); Matrix C = Matrix_(2, 2, 5.0, 5.0, 5.0, 5.0);
Matrix B = Matrix_(2,2,
4.0, 3.0,
2.0, 1.0);
Matrix C = Matrix_(2,2,
5.0, 5.0,
5.0, 5.0);
A += B; A += B;
EQUALITY(A,C); EQUALITY(A,C);
} }
@ -365,30 +401,18 @@ TEST( matrix, addition_in_place )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, subtraction ) TEST( matrix, subtraction )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
1.0, 2.0, Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0);
3.0, 4.0); Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0);
Matrix B = Matrix_(2,2,
4.0, 3.0,
2.0, 1.0);
Matrix C = Matrix_(2,2,
-3.0, -1.0,
1.0, 3.0);
EQUALITY(A-B,C); EQUALITY(A-B,C);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, subtraction_in_place ) TEST( matrix, subtraction_in_place )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
1.0, 2.0, Matrix B = Matrix_(2, 2, 4.0, 3.0, 2.0, 1.0);
3.0, 4.0); Matrix C = Matrix_(2, 2, -3.0, -1.0, 1.0, 3.0);
Matrix B = Matrix_(2,2,
4.0, 3.0,
2.0, 1.0);
Matrix C = Matrix_(2,2,
-3.0, -1.0,
1.0, 3.0);
A -= B; A -= B;
EQUALITY(A,C); EQUALITY(A,C);
} }
@ -397,8 +421,10 @@ TEST( matrix, subtraction_in_place )
TEST( matrix, multiplication ) TEST( matrix, multiplication )
{ {
Matrix A(2, 2); Matrix A(2, 2);
A(0,0) = -1; A(1,0) = 1; A(0, 0) = -1;
A(0,1) = 1; A(1,1) =-3; A(1, 0) = 1;
A(0, 1) = 1;
A(1, 1) = -3;
Matrix B(2, 1); Matrix B(2, 1);
B(0, 0) = 1.2; B(0, 0) = 1.2;
@ -417,12 +443,16 @@ TEST( matrix, scalar_matrix_multiplication )
Vector result(2); Vector result(2);
Matrix A(2, 2); Matrix A(2, 2);
A(0,0) = -1; A(1,0) = 1; A(0, 0) = -1;
A(0,1) = 1; A(1,1) =-3; A(1, 0) = 1;
A(0, 1) = 1;
A(1, 1) = -3;
Matrix B(2, 2); Matrix B(2, 2);
B(0,0) = -10; B(1,0) = 10; B(0, 0) = -10;
B(0,1) = 10; B(1,1) =-30; B(1, 0) = 10;
B(0, 1) = 10;
B(1, 1) = -30;
EQUALITY((10*A),B); EQUALITY((10*A),B);
} }
@ -432,10 +462,7 @@ TEST( matrix, matrix_vector_multiplication )
{ {
Vector result(2); Vector result(2);
Matrix A = Matrix_(2,3, Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
1.0,2.0,3.0,
4.0,5.0,6.0
);
Vector v = Vector_(3, 1., 2., 3.); Vector v = Vector_(3, 1., 2., 3.);
Vector Av = Vector_(2, 14., 32.); Vector Av = Vector_(2, 14., 32.);
Vector AtAv = Vector_(3, 142., 188., 234.); Vector AtAv = Vector_(3, 142., 188., 234.);
@ -452,17 +479,20 @@ TEST( matrix, nrRowsAndnrCols )
LONGS_EQUAL( A.size2() , 6 ); LONGS_EQUAL( A.size2() , 6 );
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, scalar_divide ) TEST( matrix, scalar_divide )
{ {
Matrix A(2, 2); Matrix A(2, 2);
A(0,0) = 10; A(1,0) = 30; A(0, 0) = 10;
A(0,1) = 20; A(1,1) = 40; A(1, 0) = 30;
A(0, 1) = 20;
A(1, 1) = 40;
Matrix B(2, 2); Matrix B(2, 2);
B(0,0) = 1; B(1,0) = 3; B(0, 0) = 1;
B(0,1) = 2; B(1,1) = 4; B(1, 0) = 3;
B(0, 1) = 2;
B(1, 1) = 4;
EQUALITY(B,A/10); EQUALITY(B,A/10);
} }
@ -471,35 +501,41 @@ TEST( matrix, scalar_divide )
TEST( matrix, inverse ) TEST( matrix, inverse )
{ {
Matrix A(3, 3); Matrix A(3, 3);
A(0,0)= 1; A(0,1)=2; A(0,2)=3; A(0, 0) = 1;
A(1,0)= 0; A(1,1)=4; A(1,2)=5; A(0, 1) = 2;
A(2,0)= 1; A(2,1)=0; A(2,2)=6; A(0, 2) = 3;
A(1, 0) = 0;
A(1, 1) = 4;
A(1, 2) = 5;
A(2, 0) = 1;
A(2, 1) = 0;
A(2, 2) = 6;
Matrix Ainv = inverse(A); Matrix Ainv = inverse(A);
CHECK(assert_equal(eye(3), A*Ainv)); CHECK(assert_equal(eye(3), A*Ainv));
CHECK(assert_equal(eye(3), Ainv*A)); CHECK(assert_equal(eye(3), Ainv*A));
Matrix expected(3, 3); Matrix expected(3, 3);
expected(0,0)= 1.0909; expected(0,1)=-0.5454; expected(0,2)=-0.0909; expected(0, 0) = 1.0909;
expected(1,0)= 0.2272; expected(1,1)= 0.1363; expected(1,2)=-0.2272; expected(0, 1) = -0.5454;
expected(2,0)= -0.1818; expected(2,1)= 0.0909; expected(2,2)=0.1818; expected(0, 2) = -0.0909;
expected(1, 0) = 0.2272;
expected(1, 1) = 0.1363;
expected(1, 2) = -0.2272;
expected(2, 0) = -0.1818;
expected(2, 1) = 0.0909;
expected(2, 2) = 0.1818;
CHECK(assert_equal(expected, Ainv, 1e-4)); CHECK(assert_equal(expected, Ainv, 1e-4));
// These two matrices failed before version 2003 because we called LU incorrectly // These two matrices failed before version 2003 because we called LU incorrectly
Matrix lMg(Matrix_(3,3, Matrix lMg(Matrix_(3, 3, 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0));
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0));
CHECK(assert_equal(Matrix_(3,3, CHECK(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0, 0.0, -1.0, 1.0,
1.0, 0.0, 2.0, 1.0, 0.0, 2.0,
0.0, 0.0, 1.0), 0.0, 0.0, 1.0),
inverse(lMg))); inverse(lMg)));
Matrix gMl(Matrix_(3,3, Matrix gMl(Matrix_(3, 3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0));
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0));
CHECK(assert_equal(Matrix_(3,3, CHECK(assert_equal(Matrix_(3,3,
0.0, 1.0,-2.0, 0.0, 1.0,-2.0,
-1.0, 0.0, 1.0, -1.0, 0.0, 1.0,
@ -511,16 +547,28 @@ TEST( matrix, inverse )
TEST( matrix, inverse2 ) TEST( matrix, inverse2 )
{ {
Matrix A(3, 3); Matrix A(3, 3);
A(0,0)= 0; A(0,1)=-1; A(0,2)=1; A(0, 0) = 0;
A(1,0)= 1; A(1,1)= 0; A(1,2)=2; A(0, 1) = -1;
A(2,0)= 0; A(2,1)= 0; A(2,2)=1; A(0, 2) = 1;
A(1, 0) = 1;
A(1, 1) = 0;
A(1, 2) = 2;
A(2, 0) = 0;
A(2, 1) = 0;
A(2, 2) = 1;
Matrix Ainv = inverse(A); Matrix Ainv = inverse(A);
Matrix expected(3, 3); Matrix expected(3, 3);
expected(0,0)= 0; expected(0,1)=1; expected(0,2)=-2; expected(0, 0) = 0;
expected(1,0)=-1; expected(1,1)=0; expected(1,2)= 1; expected(0, 1) = 1;
expected(2,0)= 0; expected(2,1)=0; expected(2,2)= 1; expected(0, 2) = -2;
expected(1, 0) = -1;
expected(1, 1) = 0;
expected(1, 2) = 1;
expected(2, 0) = 0;
expected(2, 1) = 0;
expected(2, 2) = 1;
CHECK(assert_equal(expected, Ainv, 1e-4)); CHECK(assert_equal(expected, Ainv, 1e-4));
} }
@ -530,18 +578,13 @@ TEST( matrix, backsubtitution )
{ {
// TEST ONE 2x2 matrix U1*x=b1 // TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = Vector_(2, 3.6250, -0.75); Vector expected1 = Vector_(2, 3.6250, -0.75);
Matrix U22 = Matrix_(2, 2, Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.);
2., 3.,
0., 4.);
Vector b1 = U22 * expected1; Vector b1 = U22 * expected1;
CHECK( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); CHECK( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
// TEST TWO 3x3 matrix U2*x=b2 // TEST TWO 3x3 matrix U2*x=b2
Vector expected2 = Vector_(3, 5.5, -8.5, 5.); Vector expected2 = Vector_(3, 5.5, -8.5, 5.);
Matrix U33 = Matrix_(3, 3, Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.);
3., 5., 6.,
0., 2., 3.,
0., 0., 1.);
Vector b2 = U33 * expected2; Vector b2 = U33 * expected2;
CHECK( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); CHECK( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
@ -560,29 +603,22 @@ TEST( matrix, backsubtitution )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, houseHolder ) TEST( matrix, houseHolder )
{ {
double data[] = { double data[] = { -5, 0, 5, 0, 0, 0, -1, 00, -5, 0, 5, 0, 0, 1.5, 10, 0, 0,
-5, 0, 5, 0, 0, 0, -1, 0, -10, 0, 2, 00, 10, 0, 0, 0, -10, -1 };
00, -5, 0, 5, 0, 0, 1.5,
10, 0, 0, 0,-10, 0, 2,
00, 10, 0, 0, 0,-10, -1};
// check in-place householder, with v vectors below diagonal // check in-place householder, with v vectors below diagonal
double data1[] = { double data1[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, -2.2361, 0, -8.9443, -1.565, -0.618034, 0, 4.4721, 0, -4.4721,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 };
-0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894 };
Matrix expected1 = Matrix_(4, 7, data1); Matrix expected1 = Matrix_(4, 7, data1);
Matrix A1 = Matrix_(4, 7, data); Matrix A1 = Matrix_(4, 7, data);
householder_(A1, 3); householder_(A1, 3);
CHECK(assert_equal(expected1, A1, 1e-3)); CHECK(assert_equal(expected1, A1, 1e-3));
// in-place, with zeros below diagonal // in-place, with zeros below diagonal
double data2[] = { double data2[] = { 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0.894 };
0, 0, 4.4721, 0, -4.4721, 0, 0,
0, 0, 0, 4.4721, 0, -4.4721, 0.894 };
Matrix expected = Matrix_(4, 7, data2); Matrix expected = Matrix_(4, 7, data2);
Matrix A2 = Matrix_(4, 7, data); Matrix A2 = Matrix_(4, 7, data);
householder(A2, 3); householder(A2, 3);
@ -594,32 +630,18 @@ TEST( matrix, houseHolder )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, qr ) TEST( matrix, qr )
{ {
double data[] = {-5, 0, 5, 0, double data[] = { -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
00, -5, 0, 5, 0, 0, -10, 10, 0, -10, 0 };
10, 0, 0, 0,
00, 10, 0, 0,
00, 0, 0,-10,
10, 0,-10, 0};
Matrix A = Matrix_(6, 4, data); Matrix A = Matrix_(6, 4, data);
double dataQ[] = { double dataQ[] = { -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0,
-0.3333, 0, 0.2981, 0, 0, -0.8944, 0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944,
0000000, -0.4472, 0, 0.3651, -0.8165, 0, 0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667,
00.6667, 0, 0.7454, 0, 0, 0, 0, -0.5963, 0, 0, -0.4472, };
0000000, 0.8944, 0, 0.1826, -0.4082, 0,
0000000, 0, 0, -0.9129, -0.4082, 0,
00.6667, 0, -0.5963, 0, 0, -0.4472,
};
Matrix expectedQ = Matrix_(6, 6, dataQ); Matrix expectedQ = Matrix_(6, 6, dataQ);
double dataR[] = { double dataR[] = { 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
15, 0, -8.3333, 0, 7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0, };
00, 11.1803, 0, -2.2361,
00, 0, 7.4536, 0,
00, 0, 0, 10.9545,
00, 0, 0, 0,
00, 0, 0, 0,
};
Matrix expectedR = Matrix_(6, 4, dataR); Matrix expectedR = Matrix_(6, 4, dataR);
Matrix Q, R; Matrix Q, R;
@ -632,34 +654,22 @@ TEST( matrix, qr )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, sub ) TEST( matrix, sub )
{ {
double data1[] = { double data1[] = { -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
-5, 0, 5, 0, 0, 0, 0, 00, 10, 0, 0, 0, -10 };
00, -5, 0, 5, 0, 0,
10, 0, 0, 0,-10, 0,
00, 10, 0, 0, 0,-10
};
Matrix A = Matrix_(4, 6, data1); Matrix A = Matrix_(4, 6, data1);
Matrix actual = sub(A, 1, 3, 1, 5); Matrix actual = sub(A, 1, 3, 1, 5);
double data2[] = { double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, };
-5, 0, 5, 0,
00, 0, 0,-10,
};
Matrix expected = Matrix_(2, 4, data2); Matrix expected = Matrix_(2, 4, data2);
EQUALITY(actual,expected); EQUALITY(actual,expected);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, trans ) TEST( matrix, trans )
{ {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2, 2, 1.0, 3.0, 2.0, 4.0);
1.0 ,3.0, Matrix B = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0);
2.0, 4.0 );
Matrix B = Matrix_(2,2,
1.0 ,2.0,
3.0, 4.0 );
EQUALITY(trans(A),B); EQUALITY(trans(A),B);
} }
@ -675,8 +685,10 @@ TEST( matrix, row_major_access )
TEST( matrix, svd ) TEST( matrix, svd )
{ {
double data[] = { 2, 1, 0 }; double data[] = { 2, 1, 0 };
Vector v(3); copy(data,data+3,v.begin()); Vector v(3);
Matrix U1=eye(4,3), S1=diag(v), V1=eye(3,3), A=(U1*S1)*Matrix(trans(V1)); copy(data, data + 3, v.begin());
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
* Matrix(trans(V1));
Matrix U, V; Matrix U, V;
Vector s; Vector s;
svd(A, U, s, V); svd(A, U, s, V);
@ -685,6 +697,40 @@ TEST( matrix, svd )
EQUALITY(S,S1); EQUALITY(S,S1);
} }
/* ************************************************************************* */
TEST( matrix, svdordering )
{
double data[] = {0,0,0,-4,-5,-1,0,0,0,
4,5,1,0,0,0,0,0,0,
0,0,0,0,0,0,0,0,0,
0,0,0,-5,-5,-1,0,0,0,
5,5,1,0,0,0,-5,-5,-1,
0,0,0,5,5,1,0,0,0,
0,0,0,-5,-6,-1,5,6,1,
5,6,1,0,0,0,-5,-6,-1,
-5,-6,-1,5,6,1,0,0,0,
0,0,0,-4,-6,-1,4,6,1,
4,6,1,0,0,0,0,0,0,
-4,-6,-1,0,0,0,0,0,0};
Matrix A = Matrix_(12, 9, data);
Matrix U1, U2, V1, V2;
Vector s1, s2;
svd(A, U1, s1, V1);
for(int i = 0 ; i < 8 ; i++)
CHECK(s1[i]>=s1[i+1]); // Check if singular values are sorted
svd(A, U2, s2, V2, false);
CHECK(s1[8]==s2[7]); // Check if swapping is done
CHECK(s1[7]==s2[8]);
Vector v17 = column_(V1, 7);
Vector v18 = column_(V1, 8);
Vector v27 = column_(V2, 7);
Vector v28 = column_(V2, 8);
CHECK(v17==v28); // Check if vectors are also swapped correctly
CHECK(v18==v27); // Check if vectors are also swapped correctly
}
/* ************************************************************************* */ /* ************************************************************************* */
// update A, b // update A, b
// A' \define A_{S}-ar and b'\define b-ad // A' \define A_{S}-ar and b'\define b-ad
@ -707,39 +753,31 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
TEST( matrix, weighted_elimination ) TEST( matrix, weighted_elimination )
{ {
// create a matrix to eliminate // create a matrix to eliminate
Matrix A = Matrix_(4, 6, Matrix A = Matrix_(4, 6, -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
-1., 0., 1., 0., 0., 0., 1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.);
0., -1., 0., 1., 0., 0.,
1., 0., 0., 0., -1., 0.,
0., 1., 0., 0., 0., -1.);
Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1); Vector b = Vector_(4, -0.2, 0.3, 0.2, -0.1);
Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); Vector sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1);
// expected values // expected values
Matrix expectedR = Matrix_(4, 6, Matrix expectedR = Matrix_(4, 6, 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0.,
1., 0., -0.2, 0., -0.8, 0., -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.);
0., 1., 0.,-0.2, 0., -0.8,
0., 0., 1., 0., -1., 0.,
0., 0., 0., 1., 0., -1.);
Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2); Vector d = Vector_(4, 0.2, -0.14, 0.0, 0.2);
Vector newSigmas = Vector_(4, Vector newSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607);
0.0894427,
0.0894427,
0.223607,
0.223607);
Vector r; double di, sigma; Vector r;
double di, sigma;
size_t i; size_t i;
// perform elimination // perform elimination
Matrix A1 = A; Vector b1 = b; Matrix A1 = A;
Vector b1 = b;
std::list<boost::tuple<Vector, double, double> > solution = std::list<boost::tuple<Vector, double, double> > solution =
weighted_eliminate(A1, b1, sigmas); weighted_eliminate(A1, b1, sigmas);
// unpack and verify // unpack and verify
i = 0; i = 0;
BOOST_FOREACH(boost::tie(r, di, sigma), solution) { BOOST_FOREACH(boost::tie(r, di, sigma), solution)
CHECK(assert_equal(r, row(expectedR, i))); // verify r { CHECK(assert_equal(r, row(expectedR, i))); // verify r
DOUBLES_EQUAL(d(i), di, 1e-8); // verify d DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
i += 1; i += 1;
@ -749,18 +787,12 @@ TEST( matrix, weighted_elimination )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, inverse_square_root ) TEST( matrix, inverse_square_root )
{ {
Matrix measurement_covariance = Matrix_(3,3, Matrix measurement_covariance = Matrix_(3, 3, 0.25, 0.0, 0.0, 0.0, 0.25,
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01);
0.0, 0.25, 0.0,
0.0, 0.0, 0.01
);
Matrix actual = inverse_square_root(measurement_covariance); Matrix actual = inverse_square_root(measurement_covariance);
Matrix expected = Matrix_(3,3, Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
2.0, 0.0, 0.0, 10.0);
0.0, 2.0, 0.0,
0.0, 0.0, 10.0
);
EQUALITY(expected,actual); EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual)); EQUALITY(measurement_covariance,inverse(actual*actual));
@ -771,19 +803,21 @@ TEST( matrix, inverse_square_root )
// use the same inverse routing inside inverse_square_root() // use the same inverse routing inside inverse_square_root()
// as we use here to check it. // as we use here to check it.
Matrix M = Matrix_(5, 5, Matrix M = Matrix_(5, 5, 0.0785892, 0.0137923, -0.0142219, -0.0171880,
0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726, 0.0028726, 0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868,
0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868, -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, -0.0171880,
-0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064, -0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0028726, 0.0175868,
-0.0171880, -0.0101952, 0.0054906, 0.0892453, -0.0059468, 0.0047064, -0.0059468, 0.0816517);
0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517);
expected = Matrix_(5, 5, expected = Matrix_(5, 5, 3.567126953241796, 0.000000000000000,
3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.590030436566913, 3.362022286742925, 0.000000000000000,
0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.618207860252376,
0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000, -0.168166020746503, 3.253086082942785, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937); 0.000000000000000, 0.683045380655496, 0.283773848115276,
-0.099969232183396, 3.433537147891568, 0.000000000000000,
-0.006740136923185, -0.669325697387650, -0.169716689114923,
0.171493059476284, 3.583921085468937);
EQUALITY(expected, inverse_square_root(M)); EQUALITY(expected, inverse_square_root(M));
} }
@ -793,19 +827,21 @@ TEST( matrix, inverse_square_root )
// we are checking against was generated via chol(M)' on octave // we are checking against was generated via chol(M)' on octave
TEST( matrix, LLt ) TEST( matrix, LLt )
{ {
Matrix M = Matrix_(5, 5, Matrix M = Matrix_(5, 5, 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0874197, -0.0030860, 0.0116969, 0.0081463, 0.0048741, 0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
-0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363, 0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
0.0081463, 0.0125325, 0.0103894, 0.0747324, 0.0036415, -0.0021113, 0.0036415, 0.0909464);
0.0048741, -0.0037363, -0.0021113, 0.0036415, 0.0909464);
Matrix expected = Matrix_(5, 5, Matrix expected = Matrix_(5, 5, 0.295668226226627, 0.000000000000000,
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000, -0.010437374483502, 0.295235094820875, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.039560896175007,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.063407813693827, 0.301721866387571, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); 0.000000000000000, 0.027552165831157, 0.043423266737274,
0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331,
0.014405837566082, 0.300462176944247);
EQUALITY(expected, LLt(M)); EQUALITY(expected, LLt(M));
} }
@ -813,17 +849,10 @@ TEST( matrix, LLt )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, square_root_positive ) TEST( matrix, square_root_positive )
{ {
Matrix cov = Matrix_(3,3, Matrix cov = Matrix_(3, 3, 4.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 100.0);
4.0, 0.0, 0.0,
0.0, 4.0, 0.0,
0.0, 0.0, 100.0
);
Matrix expected = Matrix_(3,3, Matrix expected = Matrix_(3, 3, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
2.0, 0.0, 0.0, 10.0);
0.0, 2.0, 0.0,
0.0, 0.0, 10.0
);
Matrix actual = square_root_positive(cov); Matrix actual = square_root_positive(cov);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
@ -833,11 +862,7 @@ TEST( matrix, square_root_positive )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, multiplyAdd ) TEST( matrix, multiplyAdd )
{ {
Matrix A = Matrix_(3,4, Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
4., 0., 0., 1.,
0., 4., 0., 2.,
0., 0., 1., 3.
);
Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.),
expected = e + prod(A, x); expected = e + prod(A, x);
@ -848,11 +873,7 @@ TEST( matrix, multiplyAdd )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( matrix, transposeMultiplyAdd ) TEST( matrix, transposeMultiplyAdd )
{ {
Matrix A = Matrix_(3,4, Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.);
4., 0., 0., 1.,
0., 4., 0., 2.,
0., 0., 1., 3.
);
Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.), Vector x = Vector_(4, 1., 2., 3., 4.), e = Vector_(3, 5., 6., 7.),
expected = x + prod(trans(A), e); expected = x + prod(trans(A), e);
@ -861,5 +882,8 @@ TEST( matrix, transposeMultiplyAdd )
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */