From b806db8b20a157088f0997c665b92699019938ef Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 13 Nov 2013 00:04:59 +0000 Subject: [PATCH] Fix Matrix_(...) to Mat() <<... in gtsam/base --- gtsam/base/tests/testCholesky.cpp | 6 +- gtsam/base/tests/testLieMatrix.cpp | 4 +- gtsam/base/tests/testMatrix.cpp | 252 ++++++++++++++--------------- 3 files changed, 128 insertions(+), 134 deletions(-) diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 93e8ce2a2..cda681c2f 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -27,7 +27,7 @@ TEST(cholesky, choleskyPartial) { // choleskyPartial should only use the upper triangle, so this represents a // symmetric matrix. - Matrix ABC = Matrix_(7,7, + Matrix ABC = (Mat(7,7) << 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063, 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914, 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992, @@ -55,7 +55,7 @@ TEST(cholesky, choleskyPartial) { /* ************************************************************************* */ TEST(cholesky, BadScalingCholesky) { - Matrix A = Matrix_(2,2, + Matrix A = (Mat(2,2) << 1e-40, 0.0, 0.0, 1.0); @@ -70,7 +70,7 @@ TEST(cholesky, BadScalingCholesky) { /* ************************************************************************* */ TEST(cholesky, BadScalingSVD) { - Matrix A = Matrix_(2,2, + Matrix A = (Mat(2,2) << 1.0, 0.0, 0.0, 1e-40); diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 7e5356fc0..8be94c6cc 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieMatrix) /* ************************************************************************* */ TEST( LieMatrix, construction ) { - Matrix m = Matrix_(2,2, 1.0, 2.0, 3.0, 4.0); + Matrix m = (Mat(2,2) << 1.0,2.0, 3.0,4.0); LieMatrix lie1(m), lie2(m); EXPECT(lie1.dim() == 4); @@ -37,7 +37,7 @@ TEST( LieMatrix, construction ) { /* ************************************************************************* */ TEST( LieMatrix, other_constructors ) { - Matrix init = Matrix_(2,2, 10.0, 20.0, 30.0, 40.0); + Matrix init = (Mat(2,2) << 10.0,20.0, 30.0,40.0); LieMatrix exp(init); LieMatrix a(2,2,10.0,20.0,30.0,40.0); double data[] = {10,30,20,40}; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 0433eb863..e8f7eb2f3 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -32,8 +32,7 @@ static const double tol = 1e-9; /* ************************************************************************* */ TEST( matrix, constructor_data ) { - double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); + Matrix A = (Mat(2, 2) << -5, 3, 0, -5); Matrix B(2, 2); B(0, 0) = -5; @@ -45,20 +44,21 @@ TEST( matrix, constructor_data ) } /* ************************************************************************* */ +/* TEST( matrix, constructor_vector ) { double data[] = { -5, 3, 0, -5 }; - Matrix A = Matrix_(2, 2, data); + Matrix A = Matrix_(2, 2, -5, 3, 0, -5); Vector v(4); copy(data, data + 4, v.data()); 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 A = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); Matrix B(2, 2); B(0, 0) = -5; B(0, 1) = 3; @@ -117,7 +117,7 @@ TEST( matrix, special_comma_initializer) /* ************************************************************************* */ TEST( matrix, col_major ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); const double * const a = &A(0, 0); EXPECT_DOUBLES_EQUAL(1, a[0], tol); EXPECT_DOUBLES_EQUAL(3, a[1], tol); @@ -128,8 +128,8 @@ TEST( matrix, col_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 = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Mat(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++) @@ -145,8 +145,8 @@ 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 = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Mat(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -172,7 +172,7 @@ TEST( matrix, collect3 ) matrices.push_back(&A); matrices.push_back(&B); Matrix AB = collect(matrices, 2, 3); - Matrix exp = Matrix_(2, 6, + Matrix exp = (Mat(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); @@ -182,8 +182,8 @@ 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 = (Mat(2, 2) << -5.0, 3.0, 00.0, -5.0); + Matrix B = (Mat(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++) @@ -205,7 +205,7 @@ TEST( matrix, stack ) /* ************************************************************************* */ TEST( matrix, column ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Mat(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); @@ -230,7 +230,7 @@ TEST( matrix, insert_column ) insertColumn(big, col, j); - Matrix expected = Matrix_(5, 6, + Matrix expected = (Mat(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, @@ -253,7 +253,7 @@ TEST( matrix, insert_subcolumn ) Vector col2 = ones(1); insertColumn(big, col2, 4, 5); // check 2 - Matrix expected = Matrix_(5, 6, + Matrix expected = (Mat(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, @@ -266,7 +266,7 @@ TEST( matrix, insert_subcolumn ) /* ************************************************************************* */ TEST( matrix, row ) { - Matrix A = Matrix_(4, 7, -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., + Matrix A = (Mat(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); @@ -301,12 +301,12 @@ 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, + Matrix big = zeros(5, 6), small = (Mat(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, + Matrix expected = (Mat(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); @@ -323,7 +323,7 @@ TEST( matrix, diagMatrices ) Matrix actual = diag(Hs); - Matrix expected = Matrix_(9, 9, + Matrix expected = (Mat(9, 9) << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -339,7 +339,7 @@ TEST( matrix, diagMatrices ) /* ************************************************************************* */ TEST( matrix, stream_read ) { - Matrix expected = Matrix_(3,4, + Matrix expected = (Mat(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, 1.2, 3.4, 4.5, 6.7); @@ -533,18 +533,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 = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Mat(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 = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Mat(2, 2) << 5.0, 5.0, 5.0, 5.0); A += B; EQUALITY(A,C); } @@ -552,18 +552,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 = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Mat(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 = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); + Matrix B = (Mat(2, 2) << 4.0, 3.0, 2.0, 1.0); + Matrix C = (Mat(2, 2) << -3.0, -1.0, 1.0, 3.0); A -= B; EQUALITY(A,C); } @@ -613,7 +613,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 = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); Vector v = (Vec(3) << 1., 2., 3.); Vector Av = (Vec(2) << 14., 32.); Vector AtAv = (Vec(3) << 142., 188., 234.); @@ -650,12 +650,12 @@ TEST( matrix, scalar_divide ) /* ************************************************************************* */ TEST( matrix, zero_below_diagonal ) { - Matrix A1 = Matrix_(3, 4, + Matrix A1 = (Mat(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0); - Matrix expected1 = Matrix_(3, 4, + Matrix expected1 = (Mat(3, 4) << 1.0, 2.0, 3.0, 4.0, 0.0, 2.0, 3.0, 4.0, 0.0, 0.0, 3.0, 4.0); @@ -671,13 +671,13 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual1c, 4); EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10)); - Matrix A2 = Matrix_(5, 3, + Matrix A2 = (Mat(5, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0, 2.0, 3.0); - Matrix expected2 = Matrix_(5, 3, + Matrix expected2 = (Mat(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 0.0, 3.0, @@ -692,7 +692,7 @@ TEST( matrix, zero_below_diagonal ) { zeroBelowDiagonal(actual2c); EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10)); - Matrix expected2_partial = Matrix_(5, 3, + Matrix expected2_partial = (Mat(5, 3) << 1.0, 2.0, 3.0, 0.0, 2.0, 3.0, 0.0, 2.0, 3.0, @@ -735,14 +735,14 @@ TEST( matrix, inverse ) EXPECT(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)); - EXPECT(assert_equal(Matrix_(3,3, + Matrix lMg((Mat(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Mat(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)); - EXPECT(assert_equal(Matrix_(3,3, + Matrix gMl((Mat(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0)); + EXPECT(assert_equal((Mat(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0), @@ -784,13 +784,13 @@ TEST( matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 Vector expected1 = (Vec(2) << 3.6250, -0.75); - Matrix U22 = Matrix_(2, 2, 2., 3., 0., 4.); + Matrix U22 = (Mat(2, 2) << 2., 3., 0., 4.); Vector b1 = U22 * expected1; EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); // TEST TWO 3x3 matrix U2*x=b2 Vector expected2 = (Vec(3) << 5.5, -8.5, 5.); - Matrix U33 = Matrix_(3, 3, 3., 5., 6., 0., 2., 3., 0., 0., 1.); + Matrix U33 = (Mat(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.); Vector b2 = U33 * expected2; EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); @@ -807,29 +807,28 @@ 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 }; - // check in-place householder, with v vectors below diagonal - double data1[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, + + Matrix expected1 = (Mat(4, 7) << 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); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894); + Matrix A1 = (Mat(4, 7) << -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 ); householder_(A1, 3); EXPECT(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, + + Matrix expected = (Mat(4, 7) << 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); + 0, 0, 4.4721, 0, -4.4721, 0.894); + Matrix A2 = (Mat(4, 7) << -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); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -837,30 +836,28 @@ TEST( matrix, householder ) /* ************************************************************************* */ TEST( matrix, householder_colMajor ) { - 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, + + Matrix expected1((Mat(4, 7) << 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)); + 0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A1((Mat(4, 7) << -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)); householder_(A1, 3); EXPECT(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, + + Matrix expected((Mat(4, 7) << 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)); + 0, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A2((Mat(4, 7) << -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)); householder(A2, 3); EXPECT(assert_equal(expected, A2, 1e-3)); } @@ -869,26 +866,26 @@ TEST( matrix, householder_colMajor ) TEST( matrix, eigen_QR ) { // use standard Eigen function to yield a non-in-place QR factorization - 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 }; // in-place, with zeros below diagonal - double data2[] = { - 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803, + + Matrix expected((Mat(4, 7) << 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 A(Matrix_(4, 7, data)); + 0, 0, 4.4721, 0, -4.4721, 0.894)); + Matrix A((Mat(4, 7) << -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)); Matrix actual = A.householderQr().matrixQR(); zeroBelowDiagonal(actual); EXPECT(assert_equal(expected, actual, 1e-3)); // use shiny new in place QR inside gtsam - A = Matrix(Matrix_(4, 7, data)); + A = Matrix((Mat(4, 7) << -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)); inplace_QR(A); EXPECT(assert_equal(expected, A, 1e-3)); } @@ -899,19 +896,18 @@ TEST( matrix, eigen_QR ) /* ************************************************************************* */ 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, + Matrix A = (Mat(6, 4) << -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 expectedQ = (Mat(6, 6) << -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); + 0, -0.5963, 0, 0, -0.4472); - 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 expectedR = (Mat(6, 4) << 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 Q, R; boost::tie(Q, R) = qr(A); @@ -923,13 +919,11 @@ 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 A = (Mat(4, 6) << -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 actual = sub(A, 1, 3, 1, 5); - double data2[] = { -5, 0, 5, 0, 00, 0, 0, -10, }; - Matrix expected = Matrix_(2, 4, data2); + Matrix expected = (Mat(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10); EQUALITY(actual,expected); } @@ -937,15 +931,15 @@ TEST( matrix, sub ) /* ************************************************************************* */ 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 = (Mat(2, 2) << 1.0, 3.0, 2.0, 4.0); + Matrix B = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); EQUALITY(trans(A),B); } /* ************************************************************************* */ TEST( matrix, col_major_access ) { - Matrix A = Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0); + Matrix A = (Mat(2, 2) << 1.0, 2.0, 3.0, 4.0); const double* a = &A(0, 0); DOUBLES_EQUAL(2.0,a[2],1e-9); } @@ -954,13 +948,13 @@ TEST( matrix, col_major_access ) 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., + Matrix A = (Mat(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 = (Vec(4) << -0.2, 0.3, 0.2, -0.1); Vector sigmas = (Vec(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., + Matrix expectedR = (Mat(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 = (Vec(4) << 0.2, -0.14, 0.0, 0.2); Vector newSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); @@ -988,11 +982,11 @@ 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, + Matrix measurement_covariance = (Mat(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, + Matrix expected = (Mat(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 10.0); EQUALITY(expected,actual); @@ -1004,14 +998,14 @@ 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, + Matrix M = (Mat(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, + expected = (Mat(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, @@ -1026,13 +1020,13 @@ 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, + Matrix M = (Mat(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, + Matrix expected = (Mat(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, @@ -1045,7 +1039,7 @@ TEST( matrix, LLt ) /* ************************************************************************* */ TEST( matrix, multiplyAdd ) { - Matrix A = Matrix_(3, 4, 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); + Matrix A = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.), expected = e + A * x; @@ -1056,7 +1050,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 = (Mat(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.); Vector x = (Vec(4) << 1., 2., 3., 4.), e = (Vec(3) << 5., 6., 7.), expected = x + trans(A) * e; @@ -1067,24 +1061,24 @@ TEST( matrix, transposeMultiplyAdd ) /* ************************************************************************* */ TEST( matrix, linear_dependent ) { - Matrix A = Matrix_(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Mat(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Mat(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent2 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); + Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0); EXPECT(linear_dependent(A, B)); } /* ************************************************************************* */ TEST( matrix, linear_dependent3 ) { - Matrix A = Matrix_(2, 3, 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); - Matrix B = Matrix_(2, 3, 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); + Matrix A = (Mat(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0); + Matrix B = (Mat(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0); EXPECT(linear_independent(A, B)); } @@ -1104,7 +1098,7 @@ TEST( matrix, svd1 ) /* ************************************************************************* */ /// Sample A matrix for SVD -static Matrix sampleA = Matrix_(3, 2, 0.,-2., 0., 0., 3., 0.); +static Matrix sampleA = (Mat(3, 2) << 0.,-2., 0., 0., 3., 0.); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ @@ -1113,9 +1107,9 @@ TEST( matrix, svd2 ) Matrix U, V; Vector s; - Matrix expectedU = Matrix_(3, 2, 0.,-1.,0.,0.,1.,0.); + Matrix expectedU = (Mat(3, 2) << 0.,-1.,0.,0.,1.,0.); Vector expected_s = (Vec(2) << 3.,2.); - Matrix expectedV = Matrix_(2, 2, 1.,0.,0.,1.); + Matrix expectedV = (Mat(2, 2) << 1.,0.,0.,1.); svd(sampleA, U, s, V); @@ -1130,9 +1124,9 @@ TEST( matrix, svd3 ) Matrix U, V; Vector s; - Matrix expectedU = Matrix_(2, 2, -1.,0.,0.,-1.); + Matrix expectedU = (Mat(2, 2) << -1.,0.,0.,-1.); Vector expected_s = (Vec(2) << 3.0, 2.0); - Matrix expectedV = Matrix_(3, 2, 0.,1.,0.,0.,-1.,0.); + Matrix expectedV = (Mat(3, 2) << 0.,1.,0.,0.,-1.,0.); svd(sampleAt, U, s, V); Matrix S = diag(s); @@ -1151,19 +1145,19 @@ TEST( matrix, svd4 ) Matrix U, V; Vector s; - Matrix A = Matrix_(3,2, + Matrix A = (Mat(3, 2) << 0.8147, 0.9134, 0.9058, 0.6324, 0.1270, 0.0975); - Matrix expectedU = Matrix_(3,2, + Matrix expectedU = (Mat(3, 2) << 0.7397, 0.6724, 0.6659, -0.7370, 0.0970, -0.0689); Vector expected_s = (Vec(2) << 1.6455, 0.1910); - Matrix expectedV = Matrix_(2,2, + Matrix expectedV = (Mat(2, 2) << 0.7403, -0.6723, 0.6723, 0.7403); @@ -1179,7 +1173,7 @@ TEST( matrix, svd4 ) /* ************************************************************************* */ TEST( matrix, DLT ) { - Matrix A = Matrix_(8,9, + Matrix A = (Mat(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, 0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04, 0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39,