diff --git a/tests/smallExample.h b/tests/smallExample.h index ca06a96df..024afed65 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -297,8 +297,8 @@ GaussianFactorGraph createGaussianFactorGraph() { */ GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); + Matrix R11 = (Mat(1, 1) << 1.0), S12 = (Mat(1, 1) << 1.0); + Matrix R22 = (Mat(1, 1) << 1.0); Vector d1(1), d2(1); d1(0) = 9; d2(0) = 5; @@ -323,7 +323,7 @@ Point2 h(const Point2& v) { } Matrix H(const Point2& v) { - return Matrix_(2, 2, + return (Mat(2, 2) << -sin(v.x()), 0.0, 0.0, cos(v.y())); } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index b8679d465..a4fb72c23 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) { config2.insert(key, pt2); GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - JacobianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1); + JacobianFactor expected1(key, (Mat(1, 2) << 1.0, 0.0), repeat(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Mat(1, 2) << 0.0, 1.0), repeat(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index c94166fa1..810086d87 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -48,21 +48,21 @@ TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vec(2) << 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0))); + 0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Mat(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Mat(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vec(2) << 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0))); + 1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Mat(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Mat(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vec(2) << 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0))); + 2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Mat(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vec(2) << 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0))); + 3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Mat(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0))); + 4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0))); // Compute steepest descent point VectorValues xu = gbn.optimizeGradientSearch(); @@ -84,21 +84,21 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vec(2) << 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), - 3, Matrix_(2,2, 7.0,8.0,9.0,10.0), - 4, Matrix_(2,2, 11.0,12.0,13.0,14.0))); + 0, (Vec(2) << 1.0,2.0), (Mat(2, 2) << 3.0,4.0,0.0,6.0), + 3, (Mat(2, 2) << 7.0,8.0,9.0,10.0), + 4, (Mat(2, 2) << 11.0,12.0,13.0,14.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vec(2) << 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), - 2, Matrix_(2,2, 21.0,22.0,23.0,24.0), - 4, Matrix_(2,2, 25.0,26.0,27.0,28.0))); + 1, (Vec(2) << 15.0,16.0), (Mat(2, 2) << 17.0,18.0,0.0,20.0), + 2, (Mat(2, 2) << 21.0,22.0,23.0,24.0), + 4, (Mat(2, 2) << 25.0,26.0,27.0,28.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vec(2) << 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), - 3, Matrix_(2,2, 35.0,36.0,37.0,38.0))); + 2, (Vec(2) << 29.0,30.0), (Mat(2, 2) << 31.0,32.0,0.0,34.0), + 3, (Mat(2, 2) << 35.0,36.0,37.0,38.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vec(2) << 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), - 4, Matrix_(2,2, 45.0,46.0,47.0,48.0))); + 3, (Vec(2) << 39.0,40.0), (Mat(2, 2) << 41.0,42.0,0.0,44.0), + 4, (Mat(2, 2) << 45.0,46.0,47.0,48.0))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0))); + 4, (Vec(2) << 49.0,50.0), (Mat(2, 2) << 51.0,52.0,0.0,54.0))); // Compute dogleg point for different deltas diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTree.cpp index 7c46ff2dc..8ca7259ba 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTree.cpp @@ -77,7 +77,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) // Check the conditional P(C3|Root) double sigma3 = 0.61808; - Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); + Matrix A56 = (Mat(2,2) << -0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; @@ -86,7 +86,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) // Check the conditional P(C4|Root) double sigma4 = 0.661968; - Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); + Matrix A46 = (Mat(2,2) << -0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; @@ -296,13 +296,13 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) // f(6,7) GaussianFactorGraph fg; noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - fg.add(1, Matrix_(1,1, 1.0), 3, Matrix_(1,1, 2.0), 5, Matrix_(1,1, 3.0), (Vec(1) << 4.0), model); - fg.add(1, Matrix_(1,1, 5.0), (Vec(1) << 6.0), model); - fg.add(2, Matrix_(1,1, 7.0), 4, Matrix_(1,1, 8.0), 5, Matrix_(1,1, 9.0), (Vec(1) << 10.0), model); - fg.add(2, Matrix_(1,1, 11.0), (Vec(1) << 12.0), model); - fg.add(5, Matrix_(1,1, 13.0), 6, Matrix_(1,1, 14.0), (Vec(1) << 15.0), model); - fg.add(6, Matrix_(1,1, 17.0), 7, Matrix_(1,1, 18.0), (Vec(1) << 19.0), model); - fg.add(7, Matrix_(1,1, 20.0), (Vec(1) << 21.0), model); + fg.add(1, (Mat(1, 1) << 1.0), 3, (Mat(1, 1) << 2.0), 5, (Mat(1, 1) << 3.0), (Vec(1) << 4.0), model); + fg.add(1, (Mat(1, 1) << 5.0), (Vec(1) << 6.0), model); + fg.add(2, (Mat(1, 1) << 7.0), 4, (Mat(1, 1) << 8.0), 5, (Mat(1, 1) << 9.0), (Vec(1) << 10.0), model); + fg.add(2, (Mat(1, 1) << 11.0), (Vec(1) << 12.0), model); + fg.add(5, (Mat(1, 1) << 13.0), 6, (Mat(1, 1) << 14.0), (Vec(1) << 15.0), model); + fg.add(6, (Mat(1, 1) << 17.0), 7, (Mat(1, 1) << 18.0), (Vec(1) << 19.0), model); + fg.add(7, (Mat(1, 1) << 20.0), (Vec(1) << 21.0), model); // Eliminate into BayesTree // c(6,7) diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 51619f1f0..f4410d523 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -134,12 +134,12 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor - JacobianFactor expectedFactor(1, Matrix_(4,2, + JacobianFactor expectedFactor(1, (Mat(4,2) << 4.714045207910318, 0., 0., 4.714045207910318, 0., 0., 0., 0.), - 2, Matrix_(4,2, + 2, (Mat(4,2) << -2.357022603955159, 0., 0., -2.357022603955159, 7.071067811865475, 0., @@ -405,10 +405,10 @@ TEST( GaussianFactorGraph, elimination ) // Check matrix Matrix R;Vector d; boost::tie(R,d) = matrix(bayesNet); - Matrix expected = Matrix_(2,2, + Matrix expected = (Mat(2, 2) << 0.707107, -0.353553, 0.0, 0.612372); - Matrix expected2 = Matrix_(2,2, + Matrix expected2 = (Mat(2, 2) << 0.707107, -0.353553, 0.0, -0.612372); EXPECT(equal_with_abs_tol(expected, R, 1e-6) || equal_with_abs_tol(expected2, R, 1e-6)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 5bbcbcb0f..1fbe0a633 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -209,7 +209,7 @@ TEST( NonlinearFactor, linearize_constraint1 ) // create expected Vector b = (Vec(2) << 0., -3.); - JacobianFactor expected(X(1), Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, + JacobianFactor expected(X(1), (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0), b, noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -229,7 +229,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) GaussianFactor::shared_ptr actual = f0.linearize(config); // create expected - Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); + Matrix A = (Mat(2, 2) << 5.0, 0.0, 0.0, 1.0); Vector b = (Vec(2) << -15., -3.); JacobianFactor expected(X(1), -1*A, L(1), A, b, noiseModel::Constrained::MixedSigmas((Vec(2) << 1.0, 0.0))); @@ -249,10 +249,10 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); + *H1 = (Mat(1, 1) << 1.0); + *H2 = (Mat(1, 1) << 2.0); + *H3 = (Mat(1, 1) << 3.0); + *H4 = (Mat(1, 1) << 4.0); } return (Vector(1) << x1 + x2 + x3 + x4).finished(); } @@ -277,10 +277,10 @@ TEST(NonlinearFactor, NoiseModelFactor4) { LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); EXPECT(assert_equal((Vector)(Vec(1) << -5.0), jf.getb())); } @@ -298,11 +298,11 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); - *H5 = Matrix_(1,1, 5.0); + *H1 = (Mat(1, 1) << 1.0); + *H2 = (Mat(1, 1) << 2.0); + *H3 = (Mat(1, 1) << 3.0); + *H4 = (Mat(1, 1) << 4.0); + *H5 = (Mat(1, 1) << 5.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished(); } @@ -325,11 +325,11 @@ TEST(NonlinearFactor, NoiseModelFactor5) { LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4))); EXPECT(assert_equal((Vector)(Vec(1) << -7.5), jf.getb())); } @@ -348,12 +348,12 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const { if(H1) { - *H1 = Matrix_(1,1, 1.0); - *H2 = Matrix_(1,1, 2.0); - *H3 = Matrix_(1,1, 3.0); - *H4 = Matrix_(1,1, 4.0); - *H5 = Matrix_(1,1, 5.0); - *H6 = Matrix_(1,1, 6.0); + *H1 = (Mat(1, 1) << 1.0); + *H2 = (Mat(1, 1) << 2.0); + *H3 = (Mat(1, 1) << 3.0); + *H4 = (Mat(1, 1) << 4.0); + *H5 = (Mat(1, 1) << 5.0); + *H6 = (Mat(1, 1) << 6.0); } return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished(); } @@ -379,12 +379,12 @@ TEST(NonlinearFactor, NoiseModelFactor6) { LONGS_EQUAL((long)X(4), (long)jf.keys()[3]); LONGS_EQUAL((long)X(5), (long)jf.keys()[4]); LONGS_EQUAL((long)X(6), (long)jf.keys()[5]); - EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin()))); - EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1))); - EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2))); - EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3))); - EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4))); - EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 0.5), jf.getA(jf.begin()))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.0), jf.getA(jf.begin()+1))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 1.5), jf.getA(jf.begin()+2))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.0), jf.getA(jf.begin()+3))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 2.5), jf.getA(jf.begin()+4))); + EXPECT(assert_equal((Matrix)(Mat(1, 1) << 3.0), jf.getA(jf.begin()+5))); EXPECT(assert_equal((Vector)(Vec(1) << -10.5), jf.getb())); }