904 lines
24 KiB
C++
904 lines
24 KiB
C++
/**
|
|
* @file testMatrix.cpp
|
|
* @brief Unit test for Matrix Library
|
|
* @author Christian Potthast
|
|
* @author Carlos Nieto
|
|
**/
|
|
|
|
#include <iostream>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/numeric/ublas/matrix_proxy.hpp>
|
|
#include <boost/numeric/ublas/io.hpp>
|
|
#include "Matrix.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
static double inf = std::numeric_limits<double>::infinity();
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, constructor_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;
|
|
|
|
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));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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;
|
|
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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 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);
|
|
|
|
EQUALITY(C,AB);
|
|
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
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);
|
|
|
|
EQUALITY(C,AB);
|
|
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, collect3 )
|
|
{
|
|
Matrix A, B;
|
|
A = eye(2, 3);
|
|
B = eye(2, 3);
|
|
vector<const Matrix*> 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);
|
|
|
|
EQUALITY(exp,AB);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
|
|
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);
|
|
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.);
|
|
CHECK(assert_equal(a2, exp2));
|
|
|
|
Vector a3 = column_(A, 6);
|
|
Vector exp3 = Vector_(4, -0.2, 0.3, 0.2, -0.1);
|
|
CHECK(assert_equal(a3, exp3));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, insert_column )
|
|
{
|
|
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);
|
|
|
|
CHECK(assert_equal(expected, big));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, insert_subcolumn )
|
|
{
|
|
Matrix big = zeros(5, 6);
|
|
Vector col1 = ones(2);
|
|
size_t i = 1;
|
|
size_t j = 3;
|
|
|
|
insertColumn(big, col1, i, j); // check 1
|
|
|
|
Vector col2 = ones(1);
|
|
insertColumn(big, col2, 4, 5); // check 2
|
|
|
|
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, 1.0);
|
|
|
|
CHECK(assert_equal(expected, big));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
Vector a1 = row_(A, 0);
|
|
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);
|
|
CHECK(assert_equal(a2, exp2));
|
|
|
|
Vector a3 = row_(A, 3);
|
|
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 zero = zeros(2, 3);
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
CHECK(assert_equal(expected, big));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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.;
|
|
|
|
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.;
|
|
|
|
CHECK(assert_equal(actual, expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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.;
|
|
|
|
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.;
|
|
|
|
CHECK(assert_equal(actual, expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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 A2(A);
|
|
|
|
Matrix A3(A);
|
|
A3(3, 3) = -2.1;
|
|
|
|
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 A2(A);
|
|
|
|
Matrix A3(A);
|
|
A3(3, 3) = inf;
|
|
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, multiplication )
|
|
{
|
|
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 AB(2, 1);
|
|
AB(0, 0) = 2.2;
|
|
AB(1, 0) = -9.;
|
|
|
|
EQUALITY(A*B,AB);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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;
|
|
|
|
Matrix B(2, 2);
|
|
B(0, 0) = -10;
|
|
B(1, 0) = 10;
|
|
B(0, 1) = 10;
|
|
B(1, 1) = -30;
|
|
|
|
EQUALITY((10*A),B);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, nrRowsAndnrCols )
|
|
{
|
|
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 B(2, 2);
|
|
B(0, 0) = 1;
|
|
B(1, 0) = 3;
|
|
B(0, 1) = 2;
|
|
B(1, 1) = 4;
|
|
|
|
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 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;
|
|
|
|
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)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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 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;
|
|
|
|
CHECK(assert_equal(expected, Ainv, 1e-4));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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;
|
|
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;
|
|
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;
|
|
CHECK( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
|
|
|
|
// TEST FOUR Try the above with transpose backSubstituteUpper
|
|
CHECK( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// unit tests for housholder transformation
|
|
/* ************************************************************************* */
|
|
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 };
|
|
|
|
// 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);
|
|
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 };
|
|
Matrix expected = Matrix_(4, 7, data2);
|
|
Matrix A2 = Matrix_(4, 7, data);
|
|
householder(A2, 3);
|
|
CHECK(assert_equal(expected, A2, 1e-3));
|
|
}
|
|
/* ************************************************************************* */
|
|
// unit test for qr factorization (and hence householder)
|
|
// This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs
|
|
/* ************************************************************************* */
|
|
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 };
|
|
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 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));
|
|
CHECK(assert_equal(expectedR, R, 1e-4));
|
|
CHECK(assert_equal(A, Q*R, 1e-14));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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 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);
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, svdordering )
|
|
{
|
|
/// Homography matrix for points
|
|
//Point2h(0, 0, 1), Point2h(4, 5, 1);
|
|
//Point2h(1, 0, 1), Point2h(5, 5, 1);
|
|
//Point2h(1, 1, 1), Point2h(5, 6, 1);
|
|
//Point2h(0, 1, 1), Point2h(4, 6, 1);
|
|
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
|
|
// __attribute__ ((noinline)) // uncomment to prevent inlining when profiling
|
|
static void updateAb(Matrix& A, Vector& b, int j, const Vector& a,
|
|
const Vector& r, double d) {
|
|
const size_t m = A.size1(), n = A.size2();
|
|
for (int i = 0; i < m; i++) { // update all rows
|
|
double ai = a(i);
|
|
b(i) -= ai * d;
|
|
double *Aij = A.data().begin() + i * n + j + 1;
|
|
const double *rptr = r.data().begin() + j + 1;
|
|
// A(i,j+1:end) -= ai*r(j+1:end)
|
|
for (int j2 = j + 1; j2 < n; j2++, Aij++, rptr++)
|
|
*Aij -= ai * (*rptr);
|
|
}
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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.);
|
|
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.);
|
|
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 r;
|
|
double di, sigma;
|
|
size_t i;
|
|
|
|
// perform elimination
|
|
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
|
|
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 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);
|
|
|
|
EQUALITY(expected,actual);
|
|
EQUALITY(measurement_covariance,inverse(actual*actual));
|
|
|
|
// Randomly generated test. This test really requires inverse to
|
|
// be working well; if it's not, there's the possibility of a
|
|
// 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);
|
|
EQUALITY(expected, inverse_square_root(M));
|
|
|
|
}
|
|
|
|
/* *********************************************************************** */
|
|
// 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 )
|
|
{
|
|
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);
|
|
|
|
EQUALITY(expected, LLt(M));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
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 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)));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, multiplyAdd )
|
|
{
|
|
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));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( matrix, transposeMultiplyAdd )
|
|
{
|
|
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));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|