diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2c932680d..67fa287d3 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,15 +6,21 @@ #include #include +#include +#include #include -#include #include #include #include #include +#include +#include +#include #include +#include +#include +#include #include -#include #include @@ -39,21 +45,21 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1 ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, - boost::optional full_graph = boost::none, + boost::optional full_graph = boost::none, const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { // These variables will be reused and accumulate factors and values ISAM2 isam(params); Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph 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); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -65,8 +71,8 @@ ISAM2 createSlamlikeISAM2( // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -78,10 +84,10 @@ ISAM2 createSlamlikeISAM2( // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -98,8 +104,8 @@ ISAM2 createSlamlikeISAM2( // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -111,10 +117,10 @@ ISAM2 createSlamlikeISAM2( // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -298,8 +304,8 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { // typedef GaussianISAM2::Impl Impl; // // Ordering ordering; ordering += (0), (0), (1); -// planarSLAM::Graph graph; -// graph.addPosePrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); +// NonlinearFactorGraph graph; +// graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); // // FastSet expected; @@ -378,7 +384,7 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { +bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { TestResult& result_ = result; const std::string name_ = test.getName(); @@ -439,7 +445,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions @@ -451,7 +457,7 @@ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions @@ -463,7 +469,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -475,7 +481,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions @@ -584,13 +590,13 @@ TEST(ISAM2, removeFactors) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the 2nd measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(12); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factor from the full system fullgraph.remove(12); @@ -604,14 +610,14 @@ TEST_UNSAFE(ISAM2, removeVariables) { // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the measurement on landmark 0 (Key 100) FastVector toRemove; toRemove.push_back(7); toRemove.push_back(14); - isam.update(planarSLAM::Graph(), Values(), toRemove); + isam.update(NonlinearFactorGraph(), Values(), toRemove); // Remove the factors and variable from the full system fullgraph.remove(7); @@ -629,7 +635,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // then swaps the 2nd-to-last landmark measurement with a different one Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); // Remove the measurement on landmark 0 and replace with a different one @@ -639,15 +645,15 @@ TEST_UNSAFE(ISAM2, swapFactors) toRemove.push_back(swap_idx); fullgraph.remove(swap_idx); - planarSLAM::Graph swapfactors; -// swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.addBearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + NonlinearFactorGraph swapfactors; +// swapfactors.add(BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor + swapfactors.add(BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise)); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } // Compare solutions - EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); + EXPECT(assert_equal(fullgraph, NonlinearFactorGraph(isam.getFactorsUnsafe()))); EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node @@ -685,7 +691,7 @@ TEST(ISAM2, constrained_ordering) // 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; + NonlinearFactorGraph fullgraph; // We will constrain x3 and x4 to the end FastMap constrained; @@ -697,8 +703,8 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { - planarSLAM::Graph newfactors; - newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + NonlinearFactorGraph newfactors; + newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -712,8 +718,8 @@ TEST(ISAM2, constrained_ordering) // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -728,10 +734,10 @@ TEST(ISAM2, constrained_ordering) // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -748,8 +754,8 @@ TEST(ISAM2, constrained_ordering) // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); fullgraph.push_back(newfactors); Values init; @@ -761,10 +767,10 @@ TEST(ISAM2, constrained_ordering) // 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); + NonlinearFactorGraph newfactors; + newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); fullgraph.push_back(newfactors); Values init; @@ -816,7 +822,7 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) // These variables will be reused and accumulate factors and values Values fullinit; - planarSLAM::Graph fullgraph; + NonlinearFactorGraph fullgraph; ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); params.enablePartialRelinearizationCheck = true; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params);