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
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();
@ -878,7 +878,7 @@ void svd(Matrix& A, Vector& s, Matrix& V) {
// perform SVD
// 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
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
svd(U,s,V); // call in-place version
svd(U,s,V,sort); // call in-place version
}
#if 0
@ -941,7 +941,7 @@ Matrix square_root_positive(const Matrix& A) {
// Perform SVD, TODO: symmetric SVD?
Matrix U,V;
Vector S;
svd(A,U,S,V);
svd(A,U,S,V,false);
// invert and sqrt diagonal of S
// 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 S output argument: n-dim vector of singular values, *not* sorted !!!
* @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
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 */
Matrix inverse_square_root(const Matrix& A);

110
cpp/svdcmp.cpp
View File

@ -8,10 +8,10 @@
#include <math.h> /* for 'fabs' */
#include <iostream>
#include <vector>
#include "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)
@ -22,7 +22,6 @@ static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(iminarg1) : (iminarg2))
/* ************************************************************************* */
/*
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;
absa = fabs(a);
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 {
if (absb == 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;
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;
g = s = scale = 0.0;
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) {
for (k = i; k <= m; k++) {
a[k][i] /= scale;
@ -85,17 +84,21 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
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];
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][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;
g = s = scale = 0.0;
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) {
for (k = l; k <= n; k++) {
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);
h = f * g - s;
a[i][l] = f - g;
for (k=l;k<=n;k++)
{
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++)
{
for (k = l; k <= n; k++) {
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])));
@ -130,11 +132,14 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
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 (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;
for (j = l; j <= n; j++)
v[i][j] = v[j][i] = 0.0;
}
v[i][i] = 1.0;
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--) {
l = i + 1;
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) {
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];
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 (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;
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--) {
@ -164,7 +175,8 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
flag = 0;
break;
}
if ((double)(fabs(w[nm])+anorm) == anorm) break;
if ((double) (fabs(w[nm]) + anorm) == anorm)
break;
}
if (flag) {
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++) {
f = s * 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];
h = pythag(f, g);
w[i] = h;
@ -191,11 +204,14 @@ void svdcmp(double **a, int m, int n, double w[], double **v)
if (l == k) {
if (z < 0.0) {
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;
}
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];
nm = k - 1;
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;
}

2
cpp/svdcmp.h
View File

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