From bbb6ff90fd75ba3f07f439abd203d1a4ef9c277a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 9 Sep 2013 16:59:04 +0000 Subject: [PATCH] Added tests and updated comments regarding using disconnected systems --- gtsam/inference/ISAM-inst.h | 1 - tests/testNonlinearISAM.cpp | 162 +++++++++++++++++++++++++++++++ tests/testNonlinearOptimizer.cpp | 20 ++++ 3 files changed, 182 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index d73673755..645499b2c 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -27,7 +27,6 @@ namespace gtsam { void ISAM::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) { // Remove the contaminated part of the Bayes tree - // Throw exception if disconnected BayesNetType bn; if (!this->empty()) { const FastSet newFactorKeys = newFactors.keys(); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 5b510b894..0291704eb 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include #include #include @@ -66,6 +68,166 @@ TEST(testNonlinearISAM, markov_chain ) { } } +/* ************************************************************************* */ +TEST(testNonlinearISAM, markov_chain_with_disconnects ) { + int reorder_interval = 2; + NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object + NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object + + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 2.0, 2.0)); + Sampler sampler(model3, 42u); + + // create initial graph + Pose2 cur_pose; // start at origin + NonlinearFactorGraph start_factors; + start_factors += NonlinearEquality(0, cur_pose); + + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); + isamChol.update(start_factors, init); + isamQR.update(start_factors, init); + + size_t nrPoses = 21; + + // create a constrained constellation of landmarks + Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3; + Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.); + expected.insert(lm1, landmark1); + expected.insert(lm2, landmark2); + expected.insert(lm3, landmark3); + + // loop for a period of time to verify memory usage + Pose2 z(1.0, 2.0, 0.1); + for (size_t i=1; i<=nrPoses; ++i) { + NonlinearFactorGraph new_factors; + new_factors += BetweenFactor(i-1, i, z, model3); + Values new_init; + + cur_pose = cur_pose.compose(z); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); + + // Add a floating landmark constellation + if (i == 7) { + new_factors += PriorFactor(lm1, landmark1, model2); + new_factors += PriorFactor(lm2, landmark2, model2); + new_factors += PriorFactor(lm3, landmark3, model2); + + // Initialize to origin + new_init.insert(lm1, Point2()); + new_init.insert(lm2, Point2()); + new_init.insert(lm3, Point2()); + } + + isamChol.update(new_factors, new_init); + isamQR.update(new_factors, new_init); + } + + // verify values - all but the last one should be very close + Values actualChol = isamChol.estimate(); + for (size_t i=0; i(i), actualChol.at(i), tol)); + + Values actualQR = isamQR.estimate(); + for (size_t i=0; i(i), actualQR.at(i), tol)); + + // Check landmarks + EXPECT(assert_equal(expected.at(lm1), actualChol.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualChol.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualChol.at(lm3), tol)); + + EXPECT(assert_equal(expected.at(lm1), actualQR.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualQR.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); +} + +/* ************************************************************************* */ +TEST(testNonlinearISAM, markov_chain_with_reconnect ) { + int reorder_interval = 2; + NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object + NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object + + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector_(2, 2.0, 2.0)); + Sampler sampler(model3, 42u); + + // create initial graph + Pose2 cur_pose; // start at origin + NonlinearFactorGraph start_factors; + start_factors += NonlinearEquality(0, cur_pose); + + Values init; + Values expected; + init.insert(0, cur_pose); + expected.insert(0, cur_pose); + isamChol.update(start_factors, init); + isamQR.update(start_factors, init); + + size_t nrPoses = 21; + + // create a constrained constellation of landmarks + Key lm1 = nrPoses+1, lm2 = nrPoses+2, lm3 = nrPoses+3; + Point2 landmark1(3., 4.), landmark2(6., 4.), landmark3(6., 9.); + expected.insert(lm1, landmark1); + expected.insert(lm2, landmark2); + expected.insert(lm3, landmark3); + + // loop for a period of time to verify memory usage + Pose2 z(1.0, 2.0, 0.1); + for (size_t i=1; i<=nrPoses; ++i) { + NonlinearFactorGraph new_factors; + new_factors += BetweenFactor(i-1, i, z, model3); + Values new_init; + + cur_pose = cur_pose.compose(z); + new_init.insert(i, cur_pose.retract(sampler.sample())); + expected.insert(i, cur_pose); + + // Add a floating landmark constellation + if (i == 7) { + new_factors += PriorFactor(lm1, landmark1, model2); + new_factors += PriorFactor(lm2, landmark2, model2); + new_factors += PriorFactor(lm3, landmark3, model2); + + // Initialize to origin + new_init.insert(lm1, Point2()); + new_init.insert(lm2, Point2()); + new_init.insert(lm3, Point2()); + } + + // Reconnect with observation later + if (i == 15) { + new_factors += BearingRangeFactor( + i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2); + } + + isamChol.update(new_factors, new_init); + isamQR.update(new_factors, new_init); + } + + // verify values - all but the last one should be very close + Values actualChol = isamChol.estimate(); + for (size_t i=0; i(i), actualChol.at(i), tol)); + + Values actualQR = isamQR.estimate(); + for (size_t i=0; i(i), actualQR.at(i), tol)); + + // Check landmarks + EXPECT(assert_equal(expected.at(lm1), actualChol.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualChol.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualChol.at(lm3), tol)); + + EXPECT(assert_equal(expected.at(lm1), actualQR.at(lm1), tol)); + EXPECT(assert_equal(expected.at(lm2), actualQR.at(lm2), tol)); + EXPECT(assert_equal(expected.at(lm3), actualQR.at(lm3), tol)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index ec035b3be..72e6ae445 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -282,6 +282,26 @@ TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); } +/* ************************************************************************* */ +TEST(NonlinearOptimizer, disconnected_graph) { + Values expected; + expected.insert(X(1), Pose2(0.,0.,0.)); + expected.insert(X(2), Pose2(1.5,0.,0.)); + expected.insert(X(3), Pose2(3.0,0.,0.)); + + Values init; + init.insert(X(1), Pose2(0.,0.,0.)); + init.insert(X(2), Pose2(0.,0.,0.)); + init.insert(X(3), Pose2(0.,0.,0.)); + + NonlinearFactorGraph graph; + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + + EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); +} + /* ************************************************************************* */ int main() { TestResult tr;