From ffe8ea5399a23e0f665188f904478043d0c53c00 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 14 May 2014 17:48:40 -0400 Subject: [PATCH] added findMinimumSpanning tree to testPlanarSLAMExample_lago + minor refinements to shorten the code --- .cproject | 428 +++++++++--------- examples/tests/testPlanarSLAMExample_lago.cpp | 65 ++- 2 files changed, 256 insertions(+), 237 deletions(-) diff --git a/.cproject b/.cproject index 97e36d81f..5d3fca7f6 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -540,10 +542,10 @@ true true - + make - -j2 - testGaussianFactor.run + -j5 + testPlanarSLAMExample_lago.run true true true @@ -574,6 +576,7 @@ make + tests/testBayesTree.run true false @@ -581,6 +584,7 @@ make + testBinaryBayesNet.run true false @@ -628,6 +632,7 @@ make + testSymbolicBayesNet.run true false @@ -635,6 +640,7 @@ make + tests/testSymbolicFactor.run true false @@ -642,6 +648,7 @@ make + testSymbolicFactorGraph.run true false @@ -657,11 +664,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -758,22 +774,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -790,6 +790,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -814,42 +830,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -918,26 +918,42 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -1328,6 +1344,7 @@ make + testGraph.run true false @@ -1335,6 +1352,7 @@ make + testJunctionTree.run true false @@ -1342,6 +1360,7 @@ make + testSymbolicBayesNetB.run true false @@ -1509,6 +1528,7 @@ make + testErrors.run true false @@ -1554,22 +1574,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1650,6 +1654,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1930,22 +1950,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2028,7 +2032,6 @@ make - testSimulated2DOriented.run true false @@ -2068,7 +2071,6 @@ make - testSimulated2D.run true false @@ -2076,7 +2078,6 @@ make - testSimulated3D.run true false @@ -2090,6 +2091,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2364,7 +2381,6 @@ make - tests/testGaussianISAM2 true false @@ -2386,102 +2402,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2683,6 +2603,7 @@ cpack + -G DEB true false @@ -2690,6 +2611,7 @@ cpack + -G RPM true false @@ -2697,6 +2619,7 @@ cpack + -G TGZ true false @@ -2704,6 +2627,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2877,34 +2801,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2948,6 +2936,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/tests/testPlanarSLAMExample_lago.cpp b/examples/tests/testPlanarSLAMExample_lago.cpp index 9ce97397a..5ae112d3d 100644 --- a/examples/tests/testPlanarSLAMExample_lago.cpp +++ b/examples/tests/testPlanarSLAMExample_lago.cpp @@ -12,13 +12,7 @@ /** * @file testPlanarSLAMExample_lago.cpp * @brief Unit tests for planar SLAM example using the initialization technique - * LAGO (Linear Approximation for Graph Optimization) proposed in: - * - * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate - * approximation for planar pose graph optimization, IJRR, 2014. - * - * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation - * for graph-based simultaneous localization and mapping, RSS, 2011. + * LAGO (Linear Approximation for Graph Optimization) * * @author Luca Carlone * @author Frank Dellaert @@ -59,8 +53,27 @@ Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x',3); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); static const double PI = boost::math::constants::pi(); +/** + * @brief Initialization technique for planar pose SLAM using + * LAGO (Linear Approximation for Graph Optimization). see papers: + * + * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate + * approximation for planar pose graph optimization, IJRR, 2014. + * + * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation + * for graph-based simultaneous localization and mapping, RSS, 2011. + * + * @param graph: nonlinear factor graph including between (Pose2) measurements + * @return Values: initial guess including orientation estimate from LAGO + */ + /* ************************************************************************* */ +// +#include Values initializeLago(const NonlinearFactorGraph& graph) { + // Find a minimum spanning tree + PredecessorMap tree = findMinimumSpanningTree >(graph); + // Order measurements: ordered spanning path first, loop closure later // Extract angles in so2 from relative rotations in SO2 @@ -84,10 +97,11 @@ TEST( Lago, smallGraph_GTmeasurements ) { // x2 0 1 // / | \ 1 2 // / | \ 2 3 -// x3 | x4 2 1 -// \ | / 1 3 +// x3 | x1 2 0 +// \ | / 0 3 // \ | / // x0 +// Pose2 pose0 = Pose2( 0.000000, 0.000000, 0.000000); Pose2 pose1 = Pose2( 1.000000, 1.000000, 1.570796); @@ -95,35 +109,20 @@ TEST( Lago, smallGraph_GTmeasurements ) { Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389); NonlinearFactorGraph graph; - - BetweenFactor factor01(x0, x1, pose0.between(pose1), model); - graph.add(factor01); - - BetweenFactor factor12(x1, x2, pose1.between(pose2), model); - graph.add(factor12); - - BetweenFactor factor23(x2, x3, pose2.between(pose3), model); - graph.add(factor23); - - BetweenFactor factor20(x2, x0, pose2.between(pose0), model); - graph.add(factor20); - - BetweenFactor factor03(x0, x3, pose0.between(pose3), model); - graph.add(factor03); + graph.add( BetweenFactor(x0, x1, pose0.between(pose1), model)); + graph.add( BetweenFactor(x1, x2, pose1.between(pose2), model)); + graph.add( BetweenFactor(x2, x3, pose2.between(pose3), model)); + graph.add( BetweenFactor(x2, x0, pose2.between(pose0), model)); + graph.add( BetweenFactor(x0, x3, pose0.between(pose3), model)); // graph.print("graph"); Values initialGuessLago = initializeLago(graph); - Vector expectedOrientations = (Vector(4) << 0.0, 0.5*PI, PI, 1.5*PI); - Vector actualOrientations(4); - actualOrientations(0) = (initialGuessLago.at(x0)).theta(); - actualOrientations(1) = (initialGuessLago.at(x1)).theta(); - actualOrientations(2) = (initialGuessLago.at(x2)).theta(); - actualOrientations(3) = (initialGuessLago.at(x3)).theta(); - - EXPECT(assert_equal(expectedOrientations, actualOrientations, 1e-6)); - //DOUBLES_EQUAL(expected, actual, 1e-6); + DOUBLES_EQUAL(0.0, (initialGuessLago.at(x0)).theta(), 1e-6); + DOUBLES_EQUAL(0.5*PI, (initialGuessLago.at(x1)).theta(), 1e-6); + DOUBLES_EQUAL(PI, (initialGuessLago.at(x2)).theta(), 1e-6); + DOUBLES_EQUAL(1.5*PI, (initialGuessLago.at(x3)).theta(), 1e-6); } /* ************************************************************************* */