From f06c7ad2e9904e3318c215ac0a77e8adfe70ceae Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 30 Jun 2012 22:32:42 +0000 Subject: [PATCH] Lots of code de-duplication and cleanup in testGaussianISAM2 --- tests/testGaussianISAM2.cpp | 748 ++++-------------------------------- 1 file changed, 71 insertions(+), 677 deletions(-) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 7d7db2cfd..2d2f68b71 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -44,7 +44,6 @@ ISAM2 createSlamlikeISAM2( // These variables will be reused and accumulate factors and values ISAM2 isam(params); -// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); Values fullinit; planarSLAM::Graph fullgraph; @@ -136,7 +135,7 @@ ISAM2 createSlamlikeISAM2( } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, AddVariables) { +TEST_UNSAFE(ISAM2, ImplAddVariables) { // Create initial state Values theta; @@ -207,7 +206,7 @@ TEST_UNSAFE(ISAM2, AddVariables) { EXPECT(assert_equal(orderingExpected, ordering)); } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, RemoveVariables) { +TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // Create initial state Values theta; @@ -381,7 +380,11 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { + + TestResult& result_ = result; + const SimpleString name_ = test.getName(); + Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); @@ -391,488 +394,94 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons VectorValues delta = optimize(gbn); Values expected = fullinit.retract(delta, ordering); - return assert_equal(expected, actual); + bool isamEqual = assert_equal(expected, actual); + + // The following two checks make sure that the cached gradients are maintained and used correctly + + // Check gradient at each node + bool nodeGradientsOk = true; + typedef ISAM2::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + // Compute expected gradient + FactorGraph jfg; + jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(jfg, expectedGradient); + // Compare with actual gradients + int variablePosition = 0; + for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { + const int dim = clique->conditional()->dim(jit); + Vector actual = clique->gradientContribution().segment(variablePosition, dim); + bool gradOk = assert_equal(expectedGradient[*jit], actual); + EXPECT(gradOk); + nodeGradientsOk = nodeGradientsOk && gradOk; + variablePosition += dim; + } + bool dimOk = clique->gradientContribution().rows() == variablePosition; + EXPECT(dimOk); + nodeGradientsOk = nodeGradientsOk && dimOk; + } + + // Check gradient + VectorValues expectedGradient(*allocateVectorValues(isam)); + gradientAtZero(FactorGraph(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); + VectorValues actualGradient(*allocateVectorValues(isam)); + gradientAtZero(isam, actualGradient); + bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); + EXPECT(expectedGradOk); + bool totalGradOk = assert_equal(expectedGradient, actualGradient); + EXPECT(totalGradOk); + + return nodeGradientsOk && expectedGradOk && totalGradOk; } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton) { - // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); Values fullinit; planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } - - // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ @@ -976,127 +585,20 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - // i keeps track of the time step - size_t i = 0; + // Remove the 2nd measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(12); + isam.update(planarSLAM::Graph(), Values(), toRemove); - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors[0]); - fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0 - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - ISAM2Result result = isam.update(newfactors, init); - ++ i; - - // Remove the measurement on landmark 0 - FastVector toRemove; - EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]); - toRemove.push_back(result.newFactorsIndices[1]); - isam.update(planarSLAM::Graph(), Values(), toRemove); - } + // Remove the factor from the full system + fullgraph.remove(12); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ @@ -1253,7 +755,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // Compare solutions EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node typedef ISAM2::sharedClique sharedClique; @@ -1313,7 +815,7 @@ TEST(ISAM2, constrained_ordering) isam.update(newfactors, init); } - CHECK(isam_check(fullgraph, fullinit, isam)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); // Add odometry from time 0 to time 5 for( ; i<5; ++i) { @@ -1381,7 +883,7 @@ TEST(ISAM2, constrained_ordering) } // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check that x3 and x4 are last, but either can come before the other EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); @@ -1420,122 +922,14 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) { // These variables will be reused and accumulate factors and values - ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); - params.enablePartialRelinearizationCheck = true; - ISAM2 isam(params); Values fullinit; planarSLAM::Graph fullgraph; - - // i keeps track of the time step - size_t i = 0; - - // Add a prior at time 0 and update isam - { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); - - isam.update(newfactors, init); - } - - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Add odometry from time 0 to time 5 - for( ; i<5; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 5 to 6 and landmark measurement at time 5 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - - isam.update(newfactors, init); - ++ i; - } - - // Add odometry from time 6 to time 10 - for( ; i<10; ++i) { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - - isam.update(newfactors, init); - } - - // Add odometry from time 10 to 11 and landmark measurement at time 10 - { - planarSLAM::Graph newfactors; - newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - fullgraph.push_back(newfactors); - - Values init; - init.insert((i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); - - // Check gradient at each node - typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { - // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); - // Compare with actual gradients - int variablePosition = 0; - for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); - Vector actual = clique->gradientContribution().segment(variablePosition, dim); - EXPECT(assert_equal(expectedGradient[*jit], actual)); - variablePosition += dim; - } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); - } - - // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); - gradientAtZero(isam, actualGradient); - EXPECT(assert_equal(expectedGradient2, expectedGradient)); - EXPECT(assert_equal(expectedGradient, actualGradient)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */