diff --git a/tests/testGaussianBayesTree.cpp b/tests/testGaussianBayesTree.cpp index e1610192d..0f77b3dd1 100644 --- a/tests/testGaussianBayesTree.cpp +++ b/tests/testGaussianBayesTree.cpp @@ -118,18 +118,18 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) TEST( GaussianBayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree + Ordering ordering; + ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); VectorValues actualSolution = bayesTree.optimize(); VectorValues expectedSolution = VectorValues::Zero(actualSolution); EXPECT(assert_equal(expectedSolution,actualSolution,tol)); - LONGS_EQUAL(4, (long)bayesTree.size()); + LONGS_EQUAL(3, (long)bayesTree.size()); double tol=1e-5; @@ -138,8 +138,8 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; - GaussianFactor::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky); - actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky)->information().inverse(); + GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); + actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); @@ -172,7 +172,9 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(); + Ordering ordering; + ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Check the conditional P(Root|Root) GaussianBayesNet empty; @@ -266,8 +268,8 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) double sig14 = 0.784465; Matrix A14 = -0.0769231*I; GaussianBayesNet expected3 = list_of - (GaussianConditional(X(4), zero(2), I/sigmax4)) - (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)); + (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) + (GaussianConditional(X(4), zero(2), I/sigmax4)); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); @@ -314,8 +316,8 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); Matrix expectedJointJ = (Matrix(2,3) << - 0, 11, 12, - -5, 0, -6 + 5, 0, 6, + 0, -11, -12 ).finished(); Matrix actualJointJ = joint.augmentedJacobian(); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index c74504a7d..bb4193955 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -104,8 +104,8 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(0, zero(3)); - expected.insert(1, Vector_(3,-0.5,0.,0.)); + expected.insert(X(1), zero(3)); + expected.insert(X(2), Vector_(3,-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -131,8 +131,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(0, zero(3)); - expected.insert(1, Vector_(3,-0.5,0.,0.)); + expected.insert(X(1), zero(3)); + expected.insert(X(2), Vector_(3,-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 3059c61a0..e31261869 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -55,9 +55,9 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(0, eye(3), zero(3), constraintModel); + JacobianFactor expLF(key, eye(3), zero(3), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); - EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF)); + EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } /* ********************************************************************** */ @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { Matrix A1 = eye(3,3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(0, A1, b, model)); + GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 7a2feaabd..2a3eb80fc 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -209,7 +209,8 @@ TEST( NonlinearFactor, linearize_constraint1 ) // create expected Vector b = Vector_(2, 0., -3.); - JacobianFactor expected(X(1), Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); + JacobianFactor expected(X(1), Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, + noiseModel::Constrained::MixedSigmas(Vector_(2, 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -230,7 +231,8 @@ TEST( NonlinearFactor, linearize_constraint2 ) // create expected Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); Vector b = Vector_(2, -15., -3.); - JacobianFactor expected(X(1), -1*A, L(1), A, b, constraint); + JacobianFactor expected(X(1), -1*A, L(1), A, b, + noiseModel::Constrained::MixedSigmas(Vector_(2, 1.0, 0.0))); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); }