/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /* * testGaussianJunctionTree.cpp * * Created on: Jul 8, 2010 * Author: nikai * Description: */ #include #include #include #include #include // for operator += #include // for operator += #include using namespace boost::assign; #define GTSAM_MAGIC_KEY #include #include #include #include #include using namespace std; using namespace gtsam; using namespace example; /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: C1 x5 x6 x4 C2 x3 x2 : x4 C3 x1 : x2 C4 x7 : x6 */ TEST( GaussianJunctionTree, constructor2 ) { // create a graph Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; GaussianFactorGraph fg = createSmoother(7, ordering).first; // create an ordering GaussianJunctionTree actual(fg); vector frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"]; vector frontal2; frontal2 += ordering["x3"], ordering["x2"]; vector frontal3; frontal3 += ordering["x1"]; vector frontal4; frontal4 += ordering["x7"]; vector sep1; vector sep2; sep2 += ordering["x4"]; vector sep3; sep3 += ordering["x2"]; vector sep4; sep4 += ordering["x6"]; EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_equal(sep1, actual.root()->separator)); LONGS_EQUAL(5, actual.root()->size()); list::const_iterator child0it = actual.root()->children().begin(); list::const_iterator child1it = child0it; ++child1it; GaussianJunctionTree::sharedClique child0 = *child0it; GaussianJunctionTree::sharedClique child1 = *child1it; EXPECT(assert_equal(frontal2, child0->frontal)); EXPECT(assert_equal(sep2, child0->separator)); LONGS_EQUAL(4, child0->size()); EXPECT(assert_equal(frontal3, child0->children().front()->frontal)); EXPECT(assert_equal(sep3, child0->children().front()->separator)); LONGS_EQUAL(2, child0->children().front()->size()); EXPECT(assert_equal(frontal4, child1->frontal)); EXPECT(assert_equal(sep4, child1->separator)); LONGS_EQUAL(2, child1->size()); } /* ************************************************************************* */ TEST( GaussianJunctionTree, optimizeMultiFrontal ) { // create a graph GaussianFactorGraph fg; Ordering ordering; boost::tie(fg,ordering) = createSmoother(7); // optimize the graph GaussianJunctionTree tree(fg); VectorValues actual = tree.optimize(&EliminateQR); // verify VectorValues expected(vector(7,2)); // expected solution Vector v = Vector_(2, 0., 0.); for (int i=1; i<=7; i++) expected[ordering[Symbol('x',i)]] = v; EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ TEST( GaussianJunctionTree, optimizeMultiFrontal2) { // create a graph Graph nlfg = createNonlinearFactorGraph(); Values noisy = createNoisyValues(); Ordering ordering; ordering += "x1","x2","l1"; GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); // optimize the graph GaussianJunctionTree tree(fg); VectorValues actual = tree.optimize(&EliminateQR); // verify VectorValues expected = createCorrectDelta(ordering); // expected solution EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ TEST(GaussianJunctionTree, slamlike) { typedef planarSLAM::PoseKey PoseKey; typedef planarSLAM::PointKey PointKey; planarSLAM::Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); size_t i = 0; newfactors = planarSLAM::Graph(); newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullgraph.push_back(newfactors); for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); fullgraph.push_back(newfactors); ++ i; for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullgraph.push_back(newfactors); ++ i; // Compare solutions Ordering ordering = *fullgraph.orderingCOLAMD(init); GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); GaussianJunctionTree gjt(linearized); VectorValues deltaactual = gjt.optimize(&EliminateQR); planarSLAM::Values actual = init.expmap(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); planarSLAM::Values expected = init.expmap(delta, ordering); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */