From 40a715327203ecfd5148c170cbeb83b667e26d7d Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 21 Oct 2013 05:12:48 +0000 Subject: [PATCH] Fix Vector_() to Vec() in gtsam/linear --- gtsam/linear/tests/testErrors.cpp | 4 +- .../tests/testGaussianBayesNetUnordered.cpp | 42 ++++++------ .../tests/testGaussianBayesTreeUnordered.cpp | 34 +++++----- .../testGaussianConditionalUnordered.cpp | 26 +++---- gtsam/linear/tests/testGaussianDensity.cpp | 2 +- .../testGaussianFactorGraphUnordered.cpp | 36 +++++----- .../tests/testHessianFactorUnordered.cpp | 68 +++++++++---------- .../tests/testJacobianFactorUnordered.cpp | 40 +++++------ gtsam/linear/tests/testKalmanFilter.cpp | 14 ++-- gtsam/linear/tests/testNoiseModel.cpp | 48 ++++++------- .../linear/tests/testSerializationLinear.cpp | 20 +++--- .../tests/testVectorValuesUnordered.cpp | 60 ++++++++-------- 12 files changed, 197 insertions(+), 197 deletions(-) diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index c8932a45e..a627dfe6a 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -29,12 +29,12 @@ using namespace gtsam; TEST( Errors, arithmetic ) { Errors e; - e += Vector_(2,1.0,2.0), Vector_(3,3.0,4.0,5.0); + e += (Vec(2) << 1.0,2.0), (Vec(3) << 3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); axpy(2.0,e,e); Errors expected; - expected += Vector_(2,3.0,6.0), Vector_(3,9.0,12.0,15.0); + expected += (Vec(2) << 3.0,6.0), (Vec(3) << 9.0,12.0,15.0); CHECK(assert_equal(expected,e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp index e9898924f..8125fcfbf 100644 --- a/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesNetUnordered.cpp @@ -38,8 +38,8 @@ using namespace gtsam; static const Key _x_=0, _y_=1, _z_=2; static GaussianBayesNet smallBayesNet = list_of - (GaussianConditional(_x_, Vector_(1, 9.0), Matrix_(1, 1, 1.0), _y_, Matrix_(1, 1, 1.0))) - (GaussianConditional(_y_, Vector_(1, 5.0), Matrix_(1, 1, 1.0))); + (GaussianConditional(_x_, (Vec(1) << 9.0), Matrix_(1, 1, 1.0), _y_, Matrix_(1, 1, 1.0))) + (GaussianConditional(_y_, (Vec(1) << 5.0), Matrix_(1, 1, 1.0))); /* ************************************************************************* */ TEST( GaussianBayesNet, matrix ) @@ -51,7 +51,7 @@ TEST( GaussianBayesNet, matrix ) 1.0, 1.0, 0.0, 1.0 ); - Vector d1 = Vector_(2, 9.0, 5.0); + Vector d1 = (Vec(2) << 9.0, 5.0); EXPECT(assert_equal(R,R1)); EXPECT(assert_equal(d,d1)); @@ -63,8 +63,8 @@ TEST( GaussianBayesNet, optimize ) VectorValues actual = smallBayesNet.optimize(); VectorValues expected = map_list_of - (_x_, Vector_(1, 4.0)) - (_y_, Vector_(1, 5.0)); + (_x_, (Vec(1) << 4.0)) + (_y_, (Vec(1) << 5.0)); EXPECT(assert_equal(expected,actual)); } @@ -78,13 +78,13 @@ TEST( GaussianBayesNet, optimize3 ) // NOTE: we are supplying a new RHS here VectorValues expected = map_list_of - (_x_, Vector_(1, -1.0)) - (_y_, Vector_(1, 5.0)); + (_x_, (Vec(1) << -1.0)) + (_y_, (Vec(1) << 5.0)); // Test different RHS version VectorValues gx = map_list_of - (_x_, Vector_(1, 4.0)) - (_y_, Vector_(1, 5.0)); + (_x_, (Vec(1) << 4.0)) + (_y_, (Vec(1) << 5.0)); VectorValues actual = smallBayesNet.backSubstitute(gx); EXPECT(assert_equal(expected, actual)); } @@ -97,11 +97,11 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) // 5 1 1 3 VectorValues x = map_list_of - (_x_, Vector_(1, 2.0)) - (_y_, Vector_(1, 5.0)), + (_x_, (Vec(1) << 2.0)) + (_y_, (Vec(1) << 5.0)), expected = map_list_of - (_x_, Vector_(1, 2.0)) - (_y_, Vector_(1, 3.0)); + (_x_, (Vec(1) << 2.0)) + (_y_, (Vec(1) << 3.0)); VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); @@ -113,15 +113,15 @@ TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; cbn += GaussianConditional( - 0, Vector_( 2, 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ), + 0, (Vec(2) << 3.0, 4.0 ), Matrix_(2, 2, 1.0, 3.0, 0.0, 4.0 ), 1, Matrix_(2, 2, 2.0, 1.0, 2.0, 3.0), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 1, Vector_( 2, 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ), + 1, (Vec(2) << 5.0, 6.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 3.0 ), 2, Matrix_(2, 2, 1.0, 0.0, 5.0, 2.0), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 3, Vector_( 2, 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); + 3, (Vec(2) << 7.0, 8.0 ), Matrix_(2, 2, 1.0, 1.0, 0.0, 5.0 ), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; double actualDeterminant = cbn.determinant(); @@ -144,21 +144,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0), + 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))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0), + 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))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0), + 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))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0), + 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))); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0))); + 4, (Vec(2) << 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp index 593bdc141..1a0762d5d 100644 --- a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -38,10 +38,10 @@ namespace { const Key x1=1, x2=2, x3=3, x4=4; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)); + (JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)); const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); /* ************************************************************************* */ @@ -89,8 +89,8 @@ TEST( GaussianBayesTree, eliminate ) GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of (x3, Matrix_(2,1, 2., 0.)) (x4, Matrix_(2,1, 2., 2.)), 2, Vector_(2, 2., 2.)), list_of - (MakeClique(GaussianConditional(pair_list_of (x2, Matrix_(2,1, -2.*sqrt(2.), 0.)) (x1, Matrix_(2,1, -sqrt(2.), -sqrt(2.))) (x3, Matrix_(2,1, -sqrt(2.), sqrt(2.))), 2, Vector_(2, -2.*sqrt(2.), 0.)))))); + MakeClique(GaussianConditional(pair_list_of (x3, Matrix_(2,1, 2., 0.)) (x4, Matrix_(2,1, 2., 2.)), 2, (Vec(2) << 2., 2.)), list_of + (MakeClique(GaussianConditional(pair_list_of (x2, Matrix_(2,1, -2.*sqrt(2.), 0.)) (x1, Matrix_(2,1, -sqrt(2.), -sqrt(2.))) (x3, Matrix_(2,1, -sqrt(2.), sqrt(2.))), 2, (Vec(2) << -2.*sqrt(2.), 0.)))))); EXPECT(assert_equal(bayesTree_expected, bt)); } @@ -99,10 +99,10 @@ TEST( GaussianBayesTree, eliminate ) TEST( GaussianBayesTree, optimizeMultiFrontal ) { VectorValues expected = pair_list_of - (x1, Vector_(1, 0.)) - (x2, Vector_(1, 1.)) - (x3, Vector_(1, 0.)) - (x4, Vector_(1, 1.)); + (x1, (Vec(1) << 0.)) + (x2, (Vec(1) << 1.)) + (x3, (Vec(1) << 0.)) + (x4, (Vec(1) << 1.)); VectorValues actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); EXPECT(assert_equal(expected,actual)); @@ -212,7 +212,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 47.0,48.0, 51.0,52.0, 0.0,54.0)), - 3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0)), list_of + 3, (Vec(6) << 29.0,30.0,39.0,40.0,49.0,50.0)), list_of (MakeClique(GaussianConditional( pair_list_of (0, Matrix_(4,2, @@ -240,7 +240,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 13.0,14.0, 25.0,26.0, 27.0,28.0)), - 2, Vector_(4, 1.0,2.0,15.0,16.0)))))); + 2, (Vec(4) << 1.0,2.0,15.0,16.0)))))); // Compute the Hessian numerically Matrix hessian = numericalHessian( @@ -264,11 +264,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Known steepest descent point from Bayes' net version VectorValues expectedFromBN = pair_list_of - (0, Vector_(2, 0.000129034, 0.000688183)) - (1, Vector_(2, 0.0109679, 0.0253767)) - (2, Vector_(2, 0.0680441, 0.114496)) - (3, Vector_(2, 0.16125, 0.241294)) - (4, Vector_(2, 0.300134, 0.423233)); + (0, (Vec(2) << 0.000129034, 0.000688183)) + (1, (Vec(2) << 0.0109679, 0.0253767)) + (2, (Vec(2) << 0.0680441, 0.114496)) + (3, (Vec(2) << 0.16125, 0.241294)) + (4, (Vec(2) << 0.300134, 0.423233)); // Compute the steepest descent point with the dogleg function VectorValues actual = bt.optimizeGradientSearch(); diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp index 793d13bf1..5fb5631aa 100644 --- a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp +++ b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp @@ -56,8 +56,8 @@ TEST(GaussianConditional, constructor) -11.3820, -7.2581, -3.0153, -3.5635); - Vector d = Vector_(2, 1.0, 2.0); - SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector_(2, 3.0, 4.0)); + Vector d = (Vec(2) << 1.0, 2.0); + SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vec(2) << 3.0, 4.0)); vector > terms = pair_list_of (1, R) @@ -114,9 +114,9 @@ TEST( GaussianConditional, equals ) R(0,0) = 0.1 ; R(1,0) = 0.3; R(0,1) = 0.0 ; R(1,1) = 0.34; - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 0.34)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(2) << 1.0, 0.34)); - Vector d = Vector_(2, 0.2, 0.5); + Vector d = (Vec(2) << 0.2, 0.5); GaussianConditional expected(1, d, R, 2, A1, 10, A2, model), @@ -177,7 +177,7 @@ TEST( GaussianConditional, solve_simple ) GaussianConditional cg(list_of(1)(2), 1, blockMatrix); // partial solution - Vector sx1 = Vector_(2, 9.0, 10.0); + Vector sx1 = (Vec(2) << 9.0, 10.0); // elimination order: 1, 2 VectorValues actual = map_list_of @@ -185,7 +185,7 @@ TEST( GaussianConditional, solve_simple ) VectorValues expected = map_list_of (2, sx1) - (1, Vector_(4, -3.1,-3.4,-11.9,-13.2)); + (1, (Vec(4) << -3.1,-3.4,-11.9,-13.2)); // verify indices/size EXPECT_LONGS_EQUAL(2, (long)cg.size()); @@ -213,15 +213,15 @@ TEST( GaussianConditional, solve_multifrontal ) EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); // partial solution - Vector sl1 = Vector_(2, 9.0, 10.0); + Vector sl1 = (Vec(2) << 9.0, 10.0); // elimination order; _x_, _x1_, _l1_ VectorValues actual = map_list_of (10, sl1); // parent VectorValues expected = map_list_of - (1, Vector_(2, -3.1,-3.4)) - (2, Vector_(2, -11.9,-13.2)) + (1, (Vector)(Vec(2) << -3.1,-3.4)) + (2, (Vector)(Vec(2) << -11.9,-13.2)) (10, sl1); // verify indices/size @@ -258,11 +258,11 @@ TEST( GaussianConditional, solveTranspose ) { VectorValues x = map_list_of - (1, Vector_(1,2.)) - (2, Vector_(1,5.)), + (1, (Vec(1) << 2.)) + (2, (Vec(1) << 5.)), y = map_list_of - (1, Vector_(1,2.)) - (2, Vector_(1,3.)); + (1, (Vec(1) << 2.)) + (2, (Vec(1) << 3.)); // test functional version VectorValues actual = cbn.backSubstituteTranspose(x); diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index b54fc9fd0..bb225065a 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -29,7 +29,7 @@ TEST(GaussianDensity, constructor) -12.1244, -5.1962, 0., 4.6904); - Vector d = Vector_(2, 1.0, 2.0), s = Vector_(2, 3.0, 4.0); + Vector d = (Vec(2) << 1.0, 2.0), s = (Vec(2) << 3.0, 4.0); GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index e229e2245..d3eec4fee 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -43,11 +43,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector_(2, 2.0, -1.0), unit2); + fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector_(2, 0.0, 1.0), unit2); + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2); return fg; } @@ -59,9 +59,9 @@ TEST(GaussianFactorGraph, initialization) { fg += JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), - JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector_(2, 2.0, -1.0), unit2), - JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2), - JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2); + JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vec(2) << 2.0, -1.0), unit2), + JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vec(2) << 0.0, 1.0), unit2), + JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vec(2) << -1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -106,7 +106,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); + gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model); gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -125,7 +125,7 @@ TEST(GaussianFactorGraph, matrices) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model); + gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), (Vec(2) << 4., 8.), model); gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model); Matrix jacobian(4,6); @@ -164,9 +164,9 @@ TEST( GaussianFactorGraph, gradient ) // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] VectorValues expected = map_list_of - (1, Vector_(2, 5.0,-12.5)) - (2, Vector_(2, 30.0, 5.0)) - (0, Vector_(2,-25.0, 17.5)); + (1, (Vec(2) << 5.0, -12.5)) + (2, (Vec(2) << 30.0, 5.0)) + (0, (Vec(2) << -25.0, 17.5)); // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -185,15 +185,15 @@ TEST( GaussianFactorGraph, transposeMultiplication ) GaussianFactorGraph A = createSimpleGaussianFactorGraph(); Errors e; e += - Vector_(2, 0.0, 0.0), - Vector_(2,15.0, 0.0), - Vector_(2, 0.0,-5.0), - Vector_(2,-7.5,-5.0); + (Vec(2) << 0.0, 0.0), + (Vec(2) << 15.0, 0.0), + (Vec(2) << 0.0,-5.0), + (Vec(2) << -7.5,-5.0); VectorValues expected; - expected.insert(1, Vector_(2, -37.5,-50.0)); - expected.insert(2, Vector_(2,-150.0, 25.0)); - expected.insert(0, Vector_(2, 187.5, 25.0)); + expected.insert(1, (Vec(2) << -37.5,-50.0)); + expected.insert(2, (Vec(2) << -150.0, 25.0)); + expected.insert(0, (Vec(2) << 187.5, 25.0)); VectorValues actual = A.transposeMultiply(e); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index c2d99026a..0ec14d80e 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -69,14 +69,14 @@ TEST(HessianFactor, ConversionConstructor) 0., 1., 0.00, 0., // f4 0., 0., -1., 0., // f2 0., 0., 0.00, -1.), // f2 - Vector_(4, -0.2, 0.3, 0.2, -0.1), - noiseModel::Diagonal::Sigmas(Vector_(4, 0.2, 0.2, 0.1, 0.1))); + (Vec(4) << -0.2, 0.3, 0.2, -0.1), + noiseModel::Diagonal::Sigmas((Vec(4) << 0.2, 0.2, 0.1, 0.1))); HessianFactor actual(jacobian); VectorValues values = pair_list_of - (0, Vector_(2, 1.0, 2.0)) - (1, Vector_(4, 3.0, 4.0, 5.0, 6.0)); + (0, (Vec(2) << 1.0, 2.0)) + (1, (Vec(4) << 3.0, 4.0, 5.0, 6.0)); EXPECT_LONGS_EQUAL(2, (long)actual.size()); EXPECT(assert_equal(expected, actual, 1e-9)); @@ -87,11 +87,11 @@ TEST(HessianFactor, ConversionConstructor) TEST(HessianFactor, Constructor1) { Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g = Vector_(2, -8.0, -9.0); + Vector g = (Vec(2) << -8.0, -9.0); double f = 10.0; VectorValues dx = pair_list_of - (0, Vector_(2, 1.5, 2.5)); + (0, (Vec(2) << 1.5, 2.5)); HessianFactor factor(0, G, g, f); @@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1) /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { - Vector mu = Vector_(2,1.0,2.0); + Vector mu = (Vec(2) << 1.0,2.0); Matrix Sigma = eye(2,2); HessianFactor factor(0, mu, Sigma); @@ -134,12 +134,12 @@ TEST(HessianFactor, Constructor2) Matrix G11 = Matrix_(1,1, 1.0); Matrix G12 = Matrix_(1,2, 2.0, 4.0); Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); + Vector g1 = (Vec(1) << -7.0); + Vector g2 = (Vec(2) << -8.0, -9.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); + Vector dx0 = (Vec(1) << 0.5); + Vector dx1 = (Vec(2) << 1.5, 2.5); VectorValues dx = pair_list_of (0, dx0) @@ -165,7 +165,7 @@ TEST(HessianFactor, Constructor2) VectorValues dxLarge = pair_list_of (0, dx0) (1, dx1) - (2, Vector_(2, 0.1, 0.2)); + (2, (Vec(2) << 0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } @@ -181,15 +181,15 @@ TEST(HessianFactor, Constructor3) Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vec(1) << -7.0); + Vector g2 = (Vec(2) << -8.0, -9.0); + Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + Vector dx0 = (Vec(1) << 0.5); + Vector dx1 = (Vec(2) << 1.5, 2.5); + Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -228,15 +228,15 @@ TEST(HessianFactor, ConstructorNWay) Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0); - Vector g1 = Vector_(1, -7.0); - Vector g2 = Vector_(2, -8.0, -9.0); - Vector g3 = Vector_(3, 1.0, 2.0, 3.0); + Vector g1 = (Vec(1) << -7.0); + Vector g2 = (Vec(2) << -8.0, -9.0); + Vector g3 = (Vec(3) << 1.0, 2.0, 3.0); double f = 10.0; - Vector dx0 = Vector_(1, 0.5); - Vector dx1 = Vector_(2, 1.5, 2.5); - Vector dx2 = Vector_(3, 1.5, 2.5, 3.5); + Vector dx0 = (Vec(1) << 0.5); + Vector dx1 = (Vec(2) << 1.5, 2.5); + Vector dx2 = (Vec(3) << 1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + Vector b0 = (Vec(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vec(3) << 1.6, 1.6, 1.6); Matrix A10 = Matrix_(3,3, 2.0, 0.0, 0.0, @@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate) -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + Vector b1 = (Vec(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vec(3) << 2.6, 2.6, 2.6); Matrix A21 = Matrix_(3,3, 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + Vector b2 = (Vec(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vec(3) << 3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -331,7 +331,7 @@ TEST(HessianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor Matrix Ax2 = Matrix_(4,2, @@ -379,7 +379,7 @@ TEST(HessianFactor, eliminate2 ) -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; + Vector d = (Vec(2) << 0.2,-0.14)/oldSigma; GaussianConditional expectedCG(0, d, R11, 1, S12); EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); @@ -390,7 +390,7 @@ TEST(HessianFactor, eliminate2 ) 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 = Vector_(2,0.0,0.894427); + Vector b1 = (Vec(2) << 0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } @@ -408,7 +408,7 @@ TEST(HessianFactor, combine) { Matrix A2 = Matrix_(2, 2, -8.94427191, 0.0, 0.0, -8.94427191); - Vector b = Vector_(2, 2.23606798,-1.56524758); + Vector b = (Vec(2) << 2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); GaussianFactorGraph factors = list_of(f); diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index 454e21298..000a11544 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -41,8 +41,8 @@ namespace { (make_pair(15, 3*Matrix3::Identity())); // RHS and sigmas - const Vector b = Vector_(3, 1., 2., 3.); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.5, 0.5, 0.5)); + const Vector b = (Vec(3) << 1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.5, 0.5, 0.5)); } } @@ -239,22 +239,22 @@ TEST(JacobianFactor, operators ) SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); Matrix I = eye(2); - Vector b = Vector_(2,0.2,-0.1); + Vector b = (Vec(2) << 0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); VectorValues c; - c.insert(1, Vector_(2,10.,20.)); - c.insert(2, Vector_(2,30.,60.)); + c.insert(1, (Vec(2) << 10.,20.)); + c.insert(2, (Vec(2) << 30.,60.)); // test A*x - Vector expectedE = Vector_(2,200.,400.); + Vector expectedE = (Vec(2) << 200.,400.); Vector actualE = lf * c; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, Vector_(2,-2000.,-4000.)); - expectedX.insert(2, Vector_(2, 2000., 4000.)); + expectedX.insert(1, (Vec(2) << -2000.,-4000.)); + expectedX.insert(2, (Vec(2) << 2000., 4000.)); VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); @@ -283,8 +283,8 @@ TEST(JacobianFactor, eliminate) 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); - Vector b0 = Vector_(3, 1.5, 1.5, 1.5); - Vector s0 = Vector_(3, 1.6, 1.6, 1.6); + Vector b0 = (Vec(3) << 1.5, 1.5, 1.5); + Vector s0 = (Vec(3) << 1.6, 1.6, 1.6); Matrix A10 = Matrix_(3,3, 2.0, 0.0, 0.0, @@ -294,15 +294,15 @@ TEST(JacobianFactor, eliminate) -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0); - Vector b1 = Vector_(3, 2.5, 2.5, 2.5); - Vector s1 = Vector_(3, 2.6, 2.6, 2.6); + Vector b1 = (Vec(3) << 2.5, 2.5, 2.5); + Vector s1 = (Vec(3) << 2.6, 2.6, 2.6); Matrix A21 = Matrix_(3,3, 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0); - Vector b2 = Vector_(3, 3.5, 3.5, 3.5); - Vector s2 = Vector_(3, 3.6, 3.6, 3.6); + Vector b2 = (Vec(3) << 3.5, 3.5, 3.5); + Vector s2 = (Vec(3) << 3.6, 3.6, 3.6); GaussianFactorGraph gfg; gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); @@ -334,7 +334,7 @@ TEST(JacobianFactor, eliminate2 ) // sigmas double sigma1 = 0.2; double sigma2 = 0.1; - Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); + Vector sigmas = (Vec(4) << sigma1, sigma1, sigma2, sigma2); // the combined linear factor Matrix Ax2 = Matrix_(4,2, @@ -379,7 +379,7 @@ TEST(JacobianFactor, eliminate2 ) -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 )/oldSigma; - Vector d = Vector_(2,0.2,-0.14)/oldSigma; + Vector d = (Vec(2) << 0.2,-0.14)/oldSigma; GaussianConditional expectedCG(2, d, R11, 11, S12); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); @@ -391,7 +391,7 @@ TEST(JacobianFactor, eliminate2 ) 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 )/sigma; - Vector b1 = Vector_(2, 0.0, 0.894427); + Vector b1 = (Vec(2) << 0.0, 0.894427); JacobianFactor expectedLF(11, Bl1x1, b1); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -475,7 +475,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) EXPECT(actual.second->size() == 0); // verify conditional Gaussian - Vector sigmas = Vector_(2, 0.0, 0.0); + Vector sigmas = (Vec(2) << 0.0, 0.0); GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } @@ -518,8 +518,8 @@ TEST ( JacobianFactor, constraint_eliminate2 ) Matrix S = Matrix_(2,2, 1.0, 2.0, 0.0, 0.0); - Vector d = Vector_(2, 3.0, 0.6666); - Vector sigmas = Vector_(2, 0.0, 0.0); + Vector d = (Vec(2) << 3.0, 0.6666); + Vector sigmas = (Vec(2) << 0.0, 0.0); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 5a33e97f3..071158971 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -30,7 +30,7 @@ using namespace gtsam; /** Small 2D point class implemented as a Vector */ struct State: Vector { State(double x, double y) : - Vector(Vector_(2, x, y)) { + Vector((Vec(2) << x, y)) { } }; @@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = Vector_(2, 1.0, 0.0); + Vector u = (Vec(2) << 1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01*eye(2, 2); Matrix H = eye(2, 2); @@ -137,7 +137,7 @@ TEST( KalmanFilter, predict ) { // Create dynamics model Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1); Matrix B = Matrix_(2, 3, 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); - Vector u = Vector_(3, 1.0, 0.0, 2.0); + Vector u = (Vec(3) << 1.0, 0.0, 2.0); Matrix R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0); Matrix M = trans(R)*R; Matrix Q = inverse(M); @@ -219,7 +219,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa->information(), pb->information(), 1e-7)); // and in addition attain the correct covariance - Vector expectedMean = Vector_(9, 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); + Vector expectedMean = (Vec(9) << 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); EXPECT(assert_equal(expectedMean, pa->mean(), 1e-7)); EXPECT(assert_equal(expectedMean, pb->mean(), 1e-7)); Matrix expected = 1e-6*Matrix_(9, 9, @@ -240,8 +240,8 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.); - Vector z = Vector_(3, 0.2599 , 1.3327 , 0.2007); - Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904); + Vector z = (Vec(3) << 0.2599 , 1.3327 , 0.2007); + Vector sigmas = (Vec(3) << 0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); // do update @@ -253,7 +253,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(pa2->information(), pb2->information(), 1e-7)); // and in addition attain the correct mean and covariance - Vector expectedMean2 = Vector_(9, 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); + Vector expectedMean2 = (Vec(9) << 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); EXPECT(assert_equal(expectedMean2, pa2->mean(), 1e-4));// not happy with tolerance here ! EXPECT(assert_equal(expectedMean2, pb2->mean(), 1e-4));// is something still amiss? Matrix expected2 = 1e-6*Matrix_(9, 9, diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 9d3f19fb1..b9c108a54 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -45,17 +45,17 @@ static Matrix Sigma = Matrix_(3, 3, /* ************************************************************************* */ TEST(NoiseModel, constructors) { - Vector whitened = Vector_(3,5.0,10.0,15.0); - Vector unwhitened = Vector_(3,10.0,20.0,30.0); + Vector whitened = (Vec(3) << 5.0,10.0,15.0); + Vector unwhitened = (Vec(3) << 10.0,20.0,30.0); // Construct noise models vector m; m.push_back(Gaussian::SqrtInformation(R)); m.push_back(Gaussian::Covariance(Sigma)); //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma))); - m.push_back(Diagonal::Variances(Vector_(3, var, var, var))); - m.push_back(Diagonal::Precisions(Vector_(3, prc, prc, prc))); + m.push_back(Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma))); + m.push_back(Diagonal::Variances((Vec(3) << var, var, var))); + m.push_back(Diagonal::Precisions((Vec(3) << prc, prc, prc))); m.push_back(Isotropic::Sigma(3, sigma)); m.push_back(Isotropic::Variance(3, var)); m.push_back(Isotropic::Precision(3, prc)); @@ -104,7 +104,7 @@ TEST(NoiseModel, constructors) /* ************************************************************************* */ TEST(NoiseModel, Unit) { - Vector v = Vector_(3,5.0,10.0,15.0); + Vector v = (Vec(3) << 5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); EXPECT(assert_equal(v,u->whiten(v))); } @@ -114,8 +114,8 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector_(3, sigma, sigma, sigma)), - d2 = Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vec(3) << sigma, sigma, sigma)), + d2 = Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), i2 = Isotropic::Sigma(3, 0.7); @@ -133,7 +133,7 @@ TEST(NoiseModel, equals) ///* ************************************************************************* */ //TEST(NoiseModel, ConstrainedSmart ) //{ -// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma), true); +// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma), true); // Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); // Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); @@ -152,8 +152,8 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = Vector_(3, sigma, 0.0, 0.0); - Vector mu = Vector_(3, 200.0, 300.0, 400.0); + Vector sigmas = (Vec(3) << sigma, 0.0, 0.0); + Vector mu = (Vec(3) << 200.0, 300.0, 400.0); actual = Constrained::All(d); EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); @@ -173,12 +173,12 @@ TEST(NoiseModel, ConstrainedConstructors ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) { - Vector feasible = Vector_(3, 1.0, 0.0, 1.0), - infeasible = Vector_(3, 1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas(Vector_(3, sigma, 0.0, sigma)); + Vector feasible = (Vec(3) << 1.0, 0.0, 1.0), + infeasible = (Vec(3) << 1.0, 1.0, 1.0); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vec(3) << sigma, 0.0, sigma)); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal(Vector_(3, 0.5, 1.0, 0.5),d->whiten(infeasible))); - EXPECT(assert_equal(Vector_(3, 0.5, 0.0, 0.5),d->whiten(feasible))); + EXPECT(assert_equal((Vec(3) << 0.5, 1.0, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal((Vec(3) << 0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); @@ -187,13 +187,13 @@ TEST(NoiseModel, ConstrainedMixed ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedAll ) { - Vector feasible = Vector_(3, 0.0, 0.0, 0.0), - infeasible = Vector_(3, 1.0, 1.0, 1.0); + Vector feasible = (Vec(3) << 0.0, 0.0, 0.0), + infeasible = (Vec(3) << 1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal(Vector_(3, 1.0, 1.0, 1.0),i->whiten(infeasible))); - EXPECT(assert_equal(Vector_(3, 0.0, 0.0, 0.0),i->whiten(feasible))); + EXPECT(assert_equal((Vec(3) << 1.0, 1.0, 1.0),i->whiten(infeasible))); + EXPECT(assert_equal((Vec(3) << 0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9); @@ -207,7 +207,7 @@ namespace exampleQR { 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 sigmas = Vector_(4, 0.2, 0.2, 0.1, 0.1); + Vector sigmas = (Vec(4) << 0.2, 0.2, 0.1, 0.1); // the matrix AB yields the following factorized version: Matrix Rd = Matrix_(4, 6+1, @@ -225,7 +225,7 @@ TEST( NoiseModel, QR ) Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! // Expected result - Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); + Vector expectedSigmas = (Vec(4) << 0.0894427, 0.0894427, 0.223607, 0.223607); SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // Call Gaussian version @@ -281,7 +281,7 @@ TEST(NoiseModel, ScalarOrVector ) /* ************************************************************************* */ TEST(NoiseModel, WhitenInPlace) { - Vector sigmas = Vector_(3, 0.1, 0.1, 0.1); + Vector sigmas = (Vec(3) << 0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); Matrix A = eye(3); model->WhitenInPlace(A); @@ -305,7 +305,7 @@ TEST(NoiseModel, robustNoise) { const double k = 10.0, error1 = 1.0, error2 = 100.0; Matrix A = Matrix_(2, 2, 1.0, 10.0, 100.0, 1000.0); - Vector b = Vector_(2, error1, error2); + Vector b = (Vec(2) << error1, error2); const Robust::shared_ptr robust = Robust::Create( mEstimator::Huber::Create(k, mEstimator::Huber::Scalar), Unit::Create(2)); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 0fa214ca0..cb7b71a8d 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -49,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vec(3) << 0.1, 0.2, 0.3)); static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vec(3) << 0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ @@ -134,9 +134,9 @@ BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); /* ************************************************************************* */ TEST (Serialization, linear_factors) { VectorValues values; - values.insert(0, Vector_(1, 1.0)); - values.insert(1, Vector_(2, 2.0,3.0)); - values.insert(2, Vector_(2, 4.0,5.0)); + values.insert(0, (Vec(1) << 1.0)); + values.insert(1, (Vec(2) << 2.0,3.0)); + values.insert(2, (Vec(2) << 4.0,5.0)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); @@ -144,7 +144,7 @@ TEST (Serialization, linear_factors) { Index i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); EXPECT(equalsXML(jacobianfactor)); @@ -175,10 +175,10 @@ TEST (Serialization, gaussian_bayes_tree) { const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4)); const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = list_of - (JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) - (JacobianFactor(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)); + (JacobianFactor(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)) + (JacobianFactor(x4, Matrix_(1,1,1.), (Vec(1) << 1.), chainNoise)); GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); diff --git a/gtsam/linear/tests/testVectorValuesUnordered.cpp b/gtsam/linear/tests/testVectorValuesUnordered.cpp index a93a29e48..307b8de32 100644 --- a/gtsam/linear/tests/testVectorValuesUnordered.cpp +++ b/gtsam/linear/tests/testVectorValuesUnordered.cpp @@ -34,10 +34,10 @@ TEST(VectorValues, basics) // insert VectorValues actual; - actual.insert(0, Vector_(1, 1.0)); - actual.insert(1, Vector_(2, 2.0, 3.0)); - actual.insert(5, Vector_(2, 6.0, 7.0)); - actual.insert(2, Vector_(2, 4.0, 5.0)); + actual.insert(0, (Vec(1) << 1.0)); + actual.insert(1, (Vec(2) << 2.0, 3.0)); + actual.insert(5, (Vec(2) << 6.0, 7.0)); + actual.insert(2, (Vec(2) << 4.0, 5.0)); // Check dimensions LONGS_EQUAL(4, actual.size()); @@ -56,12 +56,12 @@ TEST(VectorValues, basics) EXPECT(!actual.exists(6)); // Check values - EXPECT(assert_equal(Vector_(1, 1.0), actual[0])); - EXPECT(assert_equal(Vector_(2, 2.0, 3.0), actual[1])); - EXPECT(assert_equal(Vector_(2, 4.0, 5.0), actual[2])); - EXPECT(assert_equal(Vector_(2, 6.0, 7.0), actual[5])); + EXPECT(assert_equal((Vec(1) << 1.0), actual[0])); + EXPECT(assert_equal((Vec(2) << 2.0, 3.0), actual[1])); + EXPECT(assert_equal((Vec(2) << 4.0, 5.0), actual[2])); + EXPECT(assert_equal((Vec(2) << 6.0, 7.0), actual[5])); FastVector keys = list_of(0)(1)(2)(5); - EXPECT(assert_equal(Vector_(7, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector(keys))); + EXPECT(assert_equal((Vec(7) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0), actual.vector(keys))); // Check exceptions CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument); @@ -72,18 +72,18 @@ TEST(VectorValues, basics) TEST(VectorValues, combine) { VectorValues expected; - expected.insert(0, Vector_(1, 1.0)); - expected.insert(1, Vector_(2, 2.0, 3.0)); - expected.insert(5, Vector_(2, 6.0, 7.0)); - expected.insert(2, Vector_(2, 4.0, 5.0)); + expected.insert(0, (Vec(1) << 1.0)); + expected.insert(1, (Vec(2) << 2.0, 3.0)); + expected.insert(5, (Vec(2) << 6.0, 7.0)); + expected.insert(2, (Vec(2) << 4.0, 5.0)); VectorValues first; - first.insert(0, Vector_(1, 1.0)); - first.insert(1, Vector_(2, 2.0, 3.0)); + first.insert(0, (Vec(1) << 1.0)); + first.insert(1, (Vec(2) << 2.0, 3.0)); VectorValues second; - second.insert(5, Vector_(2, 6.0, 7.0)); - second.insert(2, Vector_(2, 4.0, 5.0)); + second.insert(5, (Vec(2) << 6.0, 7.0)); + second.insert(2, (Vec(2) << 4.0, 5.0)); VectorValues actual(first, second); @@ -94,14 +94,14 @@ TEST(VectorValues, combine) TEST(VectorValues, subvector) { VectorValues init; - init.insert(10, Vector_(1, 1.0)); - init.insert(11, Vector_(2, 2.0, 3.0)); - init.insert(12, Vector_(2, 4.0, 5.0)); - init.insert(13, Vector_(2, 6.0, 7.0)); + init.insert(10, (Vec(1) << 1.0)); + init.insert(11, (Vec(2) << 2.0, 3.0)); + init.insert(12, (Vec(2) << 4.0, 5.0)); + init.insert(13, (Vec(2) << 6.0, 7.0)); std::vector keys; keys += 10, 12, 13; - Vector expSubVector = Vector_(5, 1.0, 4.0, 5.0, 6.0, 7.0); + Vector expSubVector = (Vec(5) << 1.0, 4.0, 5.0, 6.0, 7.0); EXPECT(assert_equal(expSubVector, init.vector(keys))); } @@ -109,16 +109,16 @@ TEST(VectorValues, subvector) TEST(VectorValues, LinearAlgebra) { VectorValues test1; - test1.insert(0, Vector_(1, 1.0)); - test1.insert(1, Vector_(2, 2.0, 3.0)); - test1.insert(5, Vector_(2, 6.0, 7.0)); - test1.insert(2, Vector_(2, 4.0, 5.0)); + test1.insert(0, (Vec(1) << 1.0)); + test1.insert(1, (Vec(2) << 2.0, 3.0)); + test1.insert(5, (Vec(2) << 6.0, 7.0)); + test1.insert(2, (Vec(2) << 4.0, 5.0)); VectorValues test2; - test2.insert(0, Vector_(1, 6.0)); - test2.insert(1, Vector_(2, 1.0, 6.0)); - test2.insert(5, Vector_(2, 4.0, 3.0)); - test2.insert(2, Vector_(2, 1.0, 8.0)); + test2.insert(0, (Vec(1) << 6.0)); + test2.insert(1, (Vec(2) << 1.0, 6.0)); + test2.insert(5, (Vec(2) << 4.0, 3.0)); + test2.insert(2, (Vec(2) << 1.0, 8.0)); // Dot product double dotExpected = test1.vector().dot(test2.vector());