From 693e13ef881280ccdba26a0175a1fc6dfd1e9ec6 Mon Sep 17 00:00:00 2001 From: Manohar Paluri Date: Sun, 14 Feb 2010 04:54:39 +0000 Subject: [PATCH] added default bool option to svd to sort the singular values and V. the default is true so pass false to avoid sorting --- cpp/Matrix.cpp | 10 +- cpp/Matrix.h | 5 +- cpp/svdcmp.cpp | 476 ++++++++++++----------- cpp/svdcmp.h | 2 +- cpp/testMatrix.cpp | 912 +++++++++++++++++++++++---------------------- 5 files changed, 741 insertions(+), 664 deletions(-) diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp index 7d3fc2615..85bfe922a 100644 --- a/cpp/Matrix.cpp +++ b/cpp/Matrix.cpp @@ -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 diff --git a/cpp/Matrix.h b/cpp/Matrix.h index bc75eb498..fc0286771 100644 --- a/cpp/Matrix.h +++ b/cpp/Matrix.h @@ -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); diff --git a/cpp/svdcmp.cpp b/cpp/svdcmp.cpp index c730f30e0..618bc5232 100644 --- a/cpp/svdcmp.cpp +++ b/cpp/svdcmp.cpp @@ -8,247 +8,299 @@ #include /* for 'fabs' */ #include #include +#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) -static double maxarg1,maxarg2; +static double maxarg1, maxarg2; #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\ (maxarg1) : (maxarg2)) -static int iminarg1,iminarg2; +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 return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb))); -} -*/ + 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 return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb))); + } + */ /* ************************************************************************* */ -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))); - } +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) -{ - int flag,i,its,j,jj,k,l,nm; - double anorm,c,f,g,h,s,scale,x,y,z; +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 + //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 + //Current progress on verifying array bounds: + // rv1 references have been fixed - double *rv1 = new double[n]; + 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; - } + 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 (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; + 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; } - 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; - } + 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]; } - } - 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]; + 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; + } } - 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; - } - } - delete[] rv1; - + 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; + } /* ************************************************************************* */ diff --git a/cpp/svdcmp.h b/cpp/svdcmp.h index aa3d87b48..d909dfc37 100644 --- a/cpp/svdcmp.h +++ b/cpp/svdcmp.h @@ -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); diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index 703282b4a..65ea7444a 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -21,69 +21,68 @@ static double inf = std::numeric_limits::infinity(); /* ************************************************************************* */ TEST( matrix, constructor_data ) { - double data[] = {-5, 3, - 0, -5 }; - Matrix A = Matrix_(2,2,data); + 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; + Matrix B(2, 2); + B(0, 0) = -5; + B(0, 1) = 3; + B(1, 0) = 0; + B(1, 1) = -5; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, constructor_vector ) { - double data[] = {-5, 3, - 0, -5 }; - Matrix A = Matrix_(2,2,data); - Vector v(4); copy(data,data+4,v.begin()); - Matrix B = Matrix_(2,2,v); // this one is column order ! - EQUALITY(A,trans(B)); + double data[] = { -5, 3, 0, -5 }; + Matrix A = Matrix_(2, 2, data); + Vector v(4); + copy(data, data + 4, v.begin()); + Matrix B = Matrix_(2, 2, v); // this one is column order ! + EQUALITY(A,trans(B)); } /* ************************************************************************* */ TEST( matrix, Matrix_ ) { - 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; + 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; - EQUALITY(A,B); + EQUALITY(A,B); } /* ************************************************************************* */ TEST( matrix, row_major ) { - 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); - CHECK(a[2] == 3); - CHECK(a[3] == 4); + 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); + CHECK(a[2] == 3); + CHECK(a[3] == 4); } /* ************************************************************************* */ 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); + 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); 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 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); + 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); EQUALITY(C,AB); @@ -114,15 +113,14 @@ TEST( matrix, collect2 ) TEST( matrix, collect3 ) { Matrix A, B; - A = eye(2,3); - B = eye(2,3); + A = eye(2, 3); + B = eye(2, 3); vector matrices; 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,35 +128,32 @@ 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 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); + 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); - EQUALITY(C,AB); + EQUALITY(C,AB); } /* ************************************************************************* */ 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)); Vector a2 = column_(A, 3); - Vector exp2 = Vector_(4, 0., 1., 0., 0.); + Vector exp2 = Vector_(4, 0., 1., 0., 0.); CHECK(assert_equal(a2, exp2)); Vector a3 = column_(A, 6); @@ -169,18 +164,15 @@ TEST( matrix, column ) /* ************************************************************************* */ TEST( matrix, insert_column ) { - Matrix big = zeros(5,6); + Matrix big = zeros(5, 6); Vector col = ones(5); size_t j = 3; 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)); } @@ -188,19 +180,16 @@ TEST( matrix, insert_column ) /* ************************************************************************* */ TEST( matrix, insert_subcolumn ) { - Matrix big = zeros(5,6); + Matrix big = zeros(5, 6); Vector col = ones(2); size_t i = 1; size_t j = 3; 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,50 +197,49 @@ 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); + Vector exp1 = Vector_(7, -1., 0., 1., 0., 0., 0., -0.2); CHECK(assert_equal(a1, exp1)); Vector a2 = row_(A, 2); - Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); + Vector exp2 = Vector_(7, 1., 0., 0., 0., -1., 0., 0.2); CHECK(assert_equal(a2, exp2)); Vector a3 = row_(A, 3); - Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); + Vector exp3 = Vector_(7, 0., 1., 0., 0., 0., -1., -0.1); CHECK(assert_equal(a3, exp3)); } /* ************************************************************************* */ 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; + 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; - Matrix zero = zeros(2,3); + Matrix zero = zeros(2, 3); - EQUALITY(A , zero); + EQUALITY(A , zero); } /* ************************************************************************* */ 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)); } @@ -259,19 +247,37 @@ 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.; + 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.; Vector v = Vector_(4, 2., 3., 4., 5.); - Matrix actual = vector_scale(A,v); + 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.; + 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.; CHECK(assert_equal(actual, expected)); } @@ -279,19 +285,37 @@ 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.; + 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.; Vector v = Vector_(3, 2., 3., 4.); - Matrix actual = vector_scale(v,A); + 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.; + 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.; CHECK(assert_equal(actual, expected)); } @@ -299,230 +323,254 @@ 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; + 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; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3,3)=-2.1; + Matrix A3(A); + A3(3, 3) = -2.1; - CHECK(A==A2); - CHECK(A!=A3); + CHECK(A==A2); + CHECK(A!=A3); } /* ************************************************************************* */ 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; + 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; - Matrix A2(A); + Matrix A2(A); - Matrix A3(A); - A3(3,3)=inf; + Matrix A3(A); + A3(3, 3) = inf; - CHECK(A!=A3); + CHECK(A!=A3); } /* ************************************************************************* */ 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); - EQUALITY(A+B,C); + 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); - A += B; - EQUALITY(A,C); + 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); } /* ************************************************************************* */ 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); - EQUALITY(A-B,C); + 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); - A -= B; - EQUALITY(A,C); + 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); } /* ************************************************************************* */ TEST( matrix, multiplication ) { - Matrix A(2,2); - A(0,0) = -1; A(1,0) = 1; - A(0,1) = 1; A(1,1) =-3; + Matrix A(2, 2); + 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; - B(1,0) = 3.4; + Matrix B(2, 1); + B(0, 0) = 1.2; + B(1, 0) = 3.4; - Matrix AB(2,1); - AB(0,0) = 2.2; - AB(1,0) = -9.; + Matrix AB(2, 1); + AB(0, 0) = 2.2; + AB(1, 0) = -9.; - EQUALITY(A*B,AB); + EQUALITY(A*B,AB); } /* ************************************************************************* */ TEST( matrix, scalar_matrix_multiplication ) { - Vector result(2); + Vector result(2); - Matrix A(2,2); - A(0,0) = -1; A(1,0) = 1; - A(0,1) = 1; A(1,1) =-3; + Matrix A(2, 2); + 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; + Matrix B(2, 2); + B(0, 0) = -10; + B(1, 0) = 10; + B(0, 1) = 10; + B(1, 1) = -30; - EQUALITY((10*A),B); + EQUALITY((10*A),B); } /* ************************************************************************* */ TEST( matrix, matrix_vector_multiplication ) { - Vector result(2); + Vector result(2); - 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.); + 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.); - EQUALITY(A*v,Av); - EQUALITY(A^Av,AtAv); + EQUALITY(A*v,Av); + EQUALITY(A^Av,AtAv); } /* ************************************************************************* */ TEST( matrix, nrRowsAndnrCols ) { - Matrix A(3,6); - LONGS_EQUAL( A.size1() , 3 ); - LONGS_EQUAL( A.size2() , 6 ); + Matrix A(3, 6); + LONGS_EQUAL( A.size1() , 3 ); + 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; + Matrix A(2, 2); + 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; + Matrix B(2, 2); + B(0, 0) = 1; + B(1, 0) = 3; + B(0, 1) = 2; + B(1, 1) = 4; - EQUALITY(B,A/10); + EQUALITY(B,A/10); } /* ************************************************************************* */ 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; + 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; - Matrix Ainv = inverse(A); - CHECK(assert_equal(eye(3), A*Ainv)); - CHECK(assert_equal(eye(3), Ainv*A)); + 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; + 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; - 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 - 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)); - CHECK(assert_equal(Matrix_(3,3, - 0.0, 1.0,-2.0, - -1.0, 0.0, 1.0, - 0.0, 0.0, 1.0), - inverse(gMl))); + // 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)); + 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)); + CHECK(assert_equal(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0), + inverse(gMl))); } /* ************************************************************************* */ 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; + 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; - Matrix Ainv = inverse(A); + 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; + 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; - CHECK(assert_equal(expected, Ainv, 1e-4)); + CHECK(assert_equal(expected, Ainv, 1e-4)); } /* ************************************************************************* */ @@ -530,25 +578,20 @@ 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.); - Vector b1 = U22*expected1; + 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.); - Vector b2 = U33*expected2; + 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)); // TEST THREE Lower triangular 3x3 matrix L3*x=b3 Vector expected3 = Vector_(3, 1., 1., 1.); Matrix L3 = trans(U33); - Vector b3 = L3*expected3; + Vector b3 = L3 * expected3; CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); // TEST FOUR Try the above with transpose backSubstituteUpper @@ -560,32 +603,25 @@ 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 }; - Matrix expected1 = Matrix_(4,7, data1); + 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); + 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 }; - Matrix expected = Matrix_(4,7, data2); + 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); + householder(A2, 3); CHECK(assert_equal(expected, A2, 1e-3)); } /* ************************************************************************* */ @@ -594,37 +630,23 @@ 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, - }; - Matrix expectedQ = Matrix_(6,6, dataQ); + 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, - }; - Matrix expectedR = Matrix_(6,4, dataR); + 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; - boost::tie(Q,R) = qr(A); - CHECK(assert_equal(expectedQ, Q, 1e-4)); + Matrix Q, R; + boost::tie(Q, R) = qr(A); + CHECK(assert_equal(expectedQ, Q, 1e-4)); CHECK(assert_equal(expectedR, R, 1e-4)); CHECK(assert_equal(A, Q*R, 1e-14)); } @@ -632,57 +654,81 @@ 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 - }; - Matrix A = Matrix_(4,6, data1); - Matrix actual = sub(A,1,3,1,5); + 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, - }; - Matrix expected = Matrix_(2,4, data2); + double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; + Matrix expected = Matrix_(2, 4, data2); - EQUALITY(actual,expected); + 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 ); - EQUALITY(trans(A),B); + 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); } /* ************************************************************************* */ TEST( matrix, row_major_access ) { - Matrix A = Matrix_(2,2,1.0,2.0,3.0,4.0); - const double* a = &A(0,0); - DOUBLES_EQUAL(3,a[2],1e-9); + Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + const double* a = &A(0, 0); + DOUBLES_EQUAL(3,a[2],1e-9); } /* ************************************************************************* */ 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)); - Matrix U,V; - Vector s; - svd(A,U,s,V); - Matrix S=diag(s); - EQUALITY(U*S*Matrix(trans(V)),A); - EQUALITY(S,S1); +{ + 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)); + Matrix U, V; + Vector s; + svd(A, U, s, V); + Matrix S = diag(s); + EQUALITY(U*S*Matrix(trans(V)),A); + 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 } /* ************************************************************************* */ @@ -707,60 +753,46 @@ 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 > solution = - weighted_eliminate(A1, b1, sigmas); + 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 - DOUBLES_EQUAL(d(i), di, 1e-8); // verify d - DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma - i += 1; - } + i = 0; + 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; +} } /* ************************************************************************* */ 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)); @@ -770,20 +802,22 @@ TEST( matrix, inverse_square_root ) // bug in inverse masking a bug in this routine since we // 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); - - 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); + + 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); EQUALITY(expected, inverse_square_root(M)); } @@ -791,21 +825,23 @@ TEST( matrix, inverse_square_root ) /* *********************************************************************** */ // M was generated as the covariance of a set of random numbers. L that // we are checking against was generated via chol(M)' on octave -TEST( matrix, LLt ) +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,53 +849,41 @@ 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)); - CHECK(assert_equal(cov, prod(trans(actual),actual))); + Matrix actual = square_root_positive(cov); + CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(cov, prod(trans(actual),actual))); } /* ************************************************************************* */ 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); - multiplyAdd(1,A,x,e); - CHECK(assert_equal(expected, e)); + multiplyAdd(1, A, x, e); + CHECK(assert_equal(expected, e)); } /* ************************************************************************* */ 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); - transposeMultiplyAdd(1,A,e,x); - CHECK(assert_equal(expected, x)); + transposeMultiplyAdd(1, A, e, x); + CHECK(assert_equal(expected, x)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */