From 5160c2eb502827d3fb3e03c422d81bedc6e032db Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 2 Jun 2012 16:18:40 +0000 Subject: [PATCH] Significant API change in slam (GTSAM 2.0.1 or 2.1): to eliminate confusion and give the user more freedom in creating their own Keys, the different slam variants no longer create Symbol keys themselves. Instead, all interaction is done via Keys (which are just unordered, unsigned ints). All PoseSLAM unit tests and examples now just use sequential keys. However, a user can still create Keys using the Symbol constructor, which is illustrated in the landmark-based unit tests and examples. --- .cproject | 719 ++++++++---------- examples/LocalizationExample.cpp | 6 +- examples/LocalizationExample2.cpp | 13 +- examples/PlanarSLAMExample_easy.cpp | 26 +- examples/PlanarSLAMSelfContained_advanced.cpp | 62 +- examples/Pose2SLAMExample_advanced.cpp | 17 +- examples/matlab/LocalizationExample.m | 6 +- examples/vSLAMexample/Feature2D.h | 2 +- examples/vSLAMexample/vISAMexample.cpp | 35 +- examples/vSLAMexample/vSFMexample.cpp | 37 +- examples/vSLAMexample/vSLAMutils.h | 5 +- gtsam/nonlinear/ISAM2.h | 4 +- gtsam/slam/dataset.cpp | 8 +- gtsam/slam/planarSLAM.cpp | 24 +- gtsam/slam/planarSLAM.h | 56 +- gtsam/slam/pose2SLAM.cpp | 15 +- gtsam/slam/pose2SLAM.h | 25 +- gtsam/slam/pose3SLAM.cpp | 14 +- gtsam/slam/pose3SLAM.h | 9 +- gtsam/slam/simulated2D.h | 30 +- gtsam/slam/simulated2DOriented.h | 25 +- gtsam/slam/smallExample.cpp | 59 +- gtsam/slam/tests/testAntiFactor.cpp | 18 +- gtsam/slam/tests/testPlanarSLAM.cpp | 49 +- gtsam/slam/tests/testPose2SLAM.cpp | 133 ++-- gtsam/slam/tests/testPose3SLAM.cpp | 80 +- gtsam/slam/tests/testProjectionFactor.cpp | 36 +- gtsam/slam/tests/testSerializationSLAM.cpp | 37 +- gtsam/slam/tests/testSimulated2D.cpp | 4 +- gtsam/slam/tests/testSimulated2DOriented.cpp | 10 +- gtsam/slam/tests/testStereoFactor.cpp | 26 +- .../{testVSLAM.cpp => testVisualSLAM.cpp} | 122 +-- gtsam/slam/visualSLAM.cpp | 24 +- gtsam/slam/visualSLAM.h | 18 +- gtsam_unstable/slam/simulated3D.h | 11 +- gtsam_unstable/slam/tests/testSimulated3D.cpp | 4 +- tests/testBoundingConstraint.cpp | 2 +- tests/testDoglegOptimizer.cpp | 4 +- tests/testGaussianISAM2.cpp | 423 ++++------- tests/testGaussianJunctionTreeB.cpp | 43 +- tests/testGraph.cpp | 54 +- tests/testInferenceB.cpp | 26 +- tests/testNonlinearEquality.cpp | 8 +- tests/testNonlinearFactor.cpp | 97 +-- tests/testNonlinearISAM.cpp | 2 +- tests/testNonlinearOptimizer.cpp | 34 +- 46 files changed, 1118 insertions(+), 1344 deletions(-) rename gtsam/slam/tests/{testVSLAM.cpp => testVisualSLAM.cpp} (65%) diff --git a/.cproject b/.cproject index 0acb12e7e..c19b1a91a 100644 --- a/.cproject +++ b/.cproject @@ -311,14 +311,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -345,6 +337,7 @@ make + tests/testBayesTree.run true false @@ -352,6 +345,7 @@ make + testBinaryBayesNet.run true false @@ -399,6 +393,7 @@ make + testSymbolicBayesNet.run true false @@ -406,6 +401,7 @@ make + tests/testSymbolicFactor.run true false @@ -413,6 +409,7 @@ make + testSymbolicFactorGraph.run true false @@ -428,168 +425,17 @@ make + tests/testBayesTree true false true - + make -j2 - check - true - true - true - - - make - -j2 - testGaussianFactorGraph.run - true - true - true - - - make - -j2 - testGaussianISAM.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - -j2 - testIterative.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testNonlinearFactor.run - true - true - true - - - make - -j2 - testNonlinearFactorGraph.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testSubgraphPreconditioner.run - true - true - true - - - make - -j2 - testTupleConfig.run - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - - testInference.run - true - false - true - - - make - testGaussianFactor.run true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testGaussianJunctionTree - true - true - true - - - make - -j2 - testSerialization.run - true true true @@ -665,22 +511,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -697,6 +527,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -721,54 +567,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j2 - tests/testGeneralSFMFactor.run - true - true - true - - - make - -j2 - tests/testPlanarSLAM.run - true - true - true - - - make - -j2 - tests/testPose2SLAM.run - true - true - true - - - make - -j2 - tests/testPose3SLAM.run - true - true - true - make -j2 @@ -793,6 +591,94 @@ true true + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testPlanarSLAM.run + true + true + true + + + make + -j5 + testPose2SLAM.run + true + true + true + + + make + -j5 + testPose3SLAM.run + true + true + true + + + make + -j5 + testSimulated2D.run + true + true + true + + + make + -j5 + testSimulated2DOriented.run + true + true + true + + + make + -j5 + testVisualSLAM.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testSerializationSLAM.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + make -j5 @@ -897,22 +783,6 @@ true true - - make - -j2 - testGaussianJunctionTree.run - true - true - true - - - make - -j2 - testGaussianFactorGraph.run - true - true - true - make -j2 @@ -921,42 +791,26 @@ true true - + make - -j2 - testTupleConfig.run + -j5 + testMarginals.run true true true - + make - -j2 - testSerialization.run + -j5 + testGaussianISAM2.run true true true - + make - -j2 - testInference.run - true - true - true - - - make - -j2 - testGaussianISAM.run - true - true - true - - - make - -j2 - testSymbolicFactorGraph.run + -j5 + testSymbolicFactorGraphB.run true true true @@ -985,14 +839,6 @@ true true - - make - -j2 - testPose2SLAMwSPCG.run - true - true - true - make -j2 @@ -1017,14 +863,6 @@ true true - - make - -j5 - tests.testBoundingConstraint.run - true - true - true - make -j2 @@ -1057,6 +895,62 @@ true true + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + + testJunctionTree.run + true + false + true + + + make + + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + make -j2 @@ -1147,6 +1041,7 @@ make + testErrors.run true false @@ -1602,7 +1497,6 @@ make - testSimulated2DOriented.run true false @@ -1642,7 +1536,6 @@ make - testSimulated2D.run true false @@ -1650,7 +1543,6 @@ make - testSimulated3D.run true false @@ -1752,10 +1644,10 @@ true true - + make - -j2 - CameraResectioning + -j5 + CameraResectioning.run true true true @@ -1832,6 +1724,30 @@ true true + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + LocalizationExample2.run + true + true + true + + + make + -j5 + Pose2SLAMwSPCG_advanced.run + true + true + true + make -j2 @@ -1842,7 +1758,6 @@ make - tests/testGaussianISAM2 true false @@ -1864,102 +1779,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 -j2 @@ -2161,6 +1980,7 @@ cpack + -G DEB true false @@ -2168,6 +1988,7 @@ cpack + -G RPM true false @@ -2175,6 +1996,7 @@ cpack + -G TGZ true false @@ -2182,6 +2004,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2235,42 +2058,98 @@ true true - + make - -j5 - wrap.testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - wrap.testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap_gtsam + -j2 + timeRot3.run true true true - + make - -j5 - wrap + -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 @@ -2314,6 +2193,46 @@ false true + + make + -j5 + wrap.testSpirit.run + true + true + true + + + make + -j5 + wrap.testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 2d2357dad..a8ff59d68 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -66,9 +66,9 @@ int main(int argc, char** argv) { // Query the marginals Marginals marginals = graph.marginals(result); cout.precision(2); - cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(3)) << endl; + cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; + cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; + cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; return 0; } diff --git a/examples/LocalizationExample2.cpp b/examples/LocalizationExample2.cpp index 903101d87..9a4330181 100644 --- a/examples/LocalizationExample2.cpp +++ b/examples/LocalizationExample2.cpp @@ -71,10 +71,9 @@ int main(int argc, char** argv) { // add unary measurement factors, like GPS, on all three poses SharedDiagonal noiseModel(Vector_(2, 0.1, 0.1)); // 10cm std on x,y - Symbol x1('x',1), x2('x',2), x3('x',3); - graph.push_back(boost::make_shared(x1, 0, 0, noiseModel)); - graph.push_back(boost::make_shared(x2, 2, 0, noiseModel)); - graph.push_back(boost::make_shared(x3, 4, 0, noiseModel)); + graph.push_back(boost::make_shared(1, 0, 0, noiseModel)); + graph.push_back(boost::make_shared(2, 2, 0, noiseModel)); + graph.push_back(boost::make_shared(3, 4, 0, noiseModel)); // print graph.print("\nFactor graph:\n"); @@ -94,9 +93,9 @@ int main(int argc, char** argv) { // Query the marginals Marginals marginals(graph, result); cout.precision(2); - cout << "\nP1:\n" << marginals.marginalCovariance(x1) << endl; - cout << "\nP2:\n" << marginals.marginalCovariance(x2) << endl; - cout << "\nP3:\n" << marginals.marginalCovariance(x3) << endl; + cout << "\nP1:\n" << marginals.marginalCovariance(1) << endl; + cout << "\nP2:\n" << marginals.marginalCovariance(2) << endl; + cout << "\nP3:\n" << marginals.marginalCovariance(3) << endl; return 0; } diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index 5039b1145..cd4a7aefd 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -35,16 +35,20 @@ int main(int argc, char** argv) { // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph) planarSLAM::Graph graph; + // Create some keys + static Symbol i1('x',1), i2('x',2), i3('x',3); + static Symbol j1('l',1), j2('l',2); + // add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPrior(1, priorMean, priorNoise); // add directly to graph + graph.addPrior(i1, priorMean, priorNoise); // add directly to graph // add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addOdometry(1, 2, odometry, odometryNoise); - graph.addOdometry(2, 3, odometry, odometryNoise); + graph.addOdometry(i1, i2, odometry, odometryNoise); + graph.addOdometry(i2, i3, odometry, odometryNoise); // create a noise model for the landmark measurements SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range @@ -58,20 +62,20 @@ int main(int argc, char** argv) { range32 = 2.0; // add bearing/range factors (created by "addBearingRange") - graph.addBearingRange(1, 1, bearing11, range11, measurementNoise); - graph.addBearingRange(2, 1, bearing21, range21, measurementNoise); - graph.addBearingRange(3, 2, bearing32, range32, measurementNoise); + graph.addBearingRange(i1, j1, bearing11, range11, measurementNoise); + graph.addBearingRange(i2, j1, bearing21, range21, measurementNoise); + graph.addBearingRange(i3, j2, bearing32, range32, measurementNoise); // print graph.print("Factor graph"); // create (deliberatly inaccurate) initial estimate planarSLAM::Values initialEstimate; - initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insertPoint(1, Point2(1.8, 2.1)); - initialEstimate.insertPoint(2, Point2(4.1, 1.8)); + initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insertPoint(j1, Point2(1.8, 2.1)); + initialEstimate.insertPoint(j2, Point2(4.1, 1.8)); initialEstimate.print("Initial estimate:\n "); diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 36f9ad8ca..bff4ee7ea 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -15,29 +15,29 @@ * @author Alex Cunningham */ -#include -#include - -// for all nonlinear keys -#include - -// for points and poses -#include -#include - -// for modeling measurement uncertainty - all models included here -#include - // add in headers for specific factors #include #include #include +// for all nonlinear keys +#include + // implementations for structures - needed if self-contained, and these should be included last #include #include #include +// for modeling measurement uncertainty - all models included here +#include + +// for points and poses +#include +#include + +#include +#include + using namespace std; using namespace gtsam; @@ -52,8 +52,8 @@ using namespace gtsam; */ int main(int argc, char** argv) { // create keys for variables - Symbol x1('x',1), x2('x',2), x3('x',3); - Symbol l1('l',1), l2('l',2); + Symbol i1('x',1), i2('x',2), i3('x',3); + Symbol j1('l',1), j2('l',2); // create graph container and add factors to it NonlinearFactorGraph graph; @@ -62,7 +62,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + PriorFactor posePrior(i1, prior_measurement, prior_model); // create the factor graph.add(posePrior); // add the factor to the graph /* add odometry */ @@ -70,8 +70,8 @@ int main(int argc, char** argv) { SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - BetweenFactor odom12(x1, x2, odom_measurement, odom_model); - BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + BetweenFactor odom12(i1, i2, odom_measurement, odom_model); + BetweenFactor odom23(i2, i3, odom_measurement, odom_model); graph.add(odom12); // add both to graph graph.add(odom23); @@ -88,9 +88,9 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors - BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); - BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); - BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + BearingRangeFactor meas11(i1, j1, bearing11, range11, meas_model); + BearingRangeFactor meas21(i2, j1, bearing21, range21, meas_model); + BearingRangeFactor meas32(i3, j2, bearing32, range32, meas_model); // add the factors graph.add(meas11); @@ -101,11 +101,11 @@ int main(int argc, char** argv) { // initialize to noisy points Values initial; - initial.insert(x1, Pose2(0.5, 0.0, 0.2)); - initial.insert(x2, Pose2(2.3, 0.1,-0.2)); - initial.insert(x3, Pose2(4.1, 0.1, 0.1)); - initial.insert(l1, Point2(1.8, 2.1)); - initial.insert(l2, Point2(4.1, 1.8)); + initial.insert(i1, Pose2(0.5, 0.0, 0.2)); + initial.insert(i2, Pose2(2.3, 0.1,-0.2)); + initial.insert(i3, Pose2(4.1, 0.1, 0.1)); + initial.insert(j1, Point2(1.8, 2.1)); + initial.insert(j2, Point2(4.1, 1.8)); initial.print("initial estimate"); @@ -126,11 +126,11 @@ int main(int argc, char** argv) { // Print marginals covariances for all variables Marginals marginals(graph, resultMultifrontal, Marginals::CHOLESKY); - print(marginals.marginalCovariance(x1), "x1 covariance"); - print(marginals.marginalCovariance(x2), "x2 covariance"); - print(marginals.marginalCovariance(x3), "x3 covariance"); - print(marginals.marginalCovariance(l1), "l1 covariance"); - print(marginals.marginalCovariance(l2), "l2 covariance"); + print(marginals.marginalCovariance(i1), "i1 covariance"); + print(marginals.marginalCovariance(i2), "i2 covariance"); + print(marginals.marginalCovariance(i3), "i3 covariance"); + print(marginals.marginalCovariance(j1), "j1 covariance"); + print(marginals.marginalCovariance(j2), "j2 covariance"); return 0; } diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 29d8c58cf..3884e03e4 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -16,26 +16,23 @@ * @author Chris Beall */ -#include -#include -#include - // pull in the Pose2 SLAM domain with all typedefs and helper functions defined #include #include #include - #include #include +#include +#include +#include + using namespace std; using namespace gtsam; -using namespace boost; -using namespace pose2SLAM; int main(int argc, char** argv) { /* 1. create graph container and add factors to it */ - Graph graph; + pose2SLAM::Graph graph; /* 2.a add prior */ // gaussian for prior @@ -77,8 +74,8 @@ int main(int argc, char** argv) { /* Get covariances */ Marginals marginals(graph, result, Marginals::CHOLESKY); - Matrix covariance1 = marginals.marginalCovariance(PoseKey(1)); - Matrix covariance2 = marginals.marginalCovariance(PoseKey(2)); + Matrix covariance1 = marginals.marginalCovariance(1); + Matrix covariance2 = marginals.marginalCovariance(2); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/matlab/LocalizationExample.m b/examples/matlab/LocalizationExample.m index 521c768ec..3a8282e3a 100644 --- a/examples/matlab/LocalizationExample.m +++ b/examples/matlab/LocalizationExample.m @@ -45,9 +45,9 @@ result.print(sprintf('\nFinal result:\n ')); %% Query the marginals marginals = graph.marginals(result); -x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key) -x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key) -x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key) +P{1}=marginals.marginalCovariance(1) +P{2}=marginals.marginalCovariance(2) +P{3}=marginals.marginalCovariance(3) %% Plot Trajectory figure(1) diff --git a/examples/vSLAMexample/Feature2D.h b/examples/vSLAMexample/Feature2D.h index aca5f9e4b..ea0f60e74 100644 --- a/examples/vSLAMexample/Feature2D.h +++ b/examples/vSLAMexample/Feature2D.h @@ -24,7 +24,7 @@ struct Feature2D { gtsam::Point2 m_p; - int m_idCamera; // id of the camera pose that makes this measurement + int m_idCamera; // id of the camera pose that makes this measurement int m_idLandmark; // id of the 3D landmark that it is associated with Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) : diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 71d5516a2..6b47c9633 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -11,14 +11,12 @@ /** * @file vISAMexample.cpp - * @brief An ISAM example for synthesis sequence - * single camera + * @brief An ISAM example for synthesis sequence, single camera * @author Duy-Nguyen Ta */ -#include -#include -using namespace boost; +#include "vSLAMutils.h" +#include "Feature2D.h" #include #include @@ -26,13 +24,12 @@ using namespace boost; #include #include -#include "vSLAMutils.h" -#include "Feature2D.h" +#include +#include +using namespace boost; using namespace std; using namespace gtsam; -using namespace visualSLAM; -using namespace boost; /* ************************************************************************* */ #define CALIB_FILE "calib.txt" @@ -45,7 +42,7 @@ string g_dataFolder; // Store groundtruth values, read from files shared_ptrK g_calib; -map g_landmarks; // map: +map g_landmarks; // map: map g_poses; // map: std::map > g_measurements; // feature sets detected at each frame @@ -76,31 +73,31 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements - newFactors = shared_ptr (new Graph()); + newFactors = shared_ptr (new visualSLAM::Graph()); for (size_t i = 0; i < measurements.size(); i++) { newFactors->addMeasurement( measurements[i].m_p, measurementSigma, - pose_id, - measurements[i].m_idLandmark, + Symbol('x',pose_id), + Symbol('l',measurements[i].m_idLandmark), calib); } // ... we need priors on the new pose and all new landmarks newFactors->addPosePrior(pose_id, pose, poseSigma); for (size_t i = 0; i < measurements.size(); i++) { - newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); + newFactors->addPointPrior(Symbol('x',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); } // Create initial values for all nodes in the newFactors initialValues = shared_ptr (new Values()); - initialValues->insert(PoseKey(pose_id), pose); + initialValues->insert(Symbol('x',pose_id), pose); for (size_t i = 0; i < measurements.size(); i++) { - initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); + initialValues->insert(Symbol('l',measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); } } @@ -127,8 +124,8 @@ int main(int argc, char* argv[]) { typedef std::map > FeatureMap; BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { const int poseId = features.first; - shared_ptr newFactors; - shared_ptr initialValues; + shared_ptr newFactors; + shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib); diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 8774b79f3..6eb3955f2 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -11,26 +11,23 @@ /** * @file vSFMexample.cpp - * @brief An vSFM example for synthesis sequence - * single camera + * @brief A visual SLAM example for simulated sequence * @author Duy-Nguyen Ta */ -#include -using namespace boost; +#include "vSLAMutils.h" +#include "Feature2D.h" #include #include #include #include -#include "vSLAMutils.h" -#include "Feature2D.h" +#include +using namespace boost; using namespace std; using namespace gtsam; -using namespace visualSLAM; -using namespace boost; /* ************************************************************************* */ #define CALIB_FILE "calib.txt" @@ -43,9 +40,9 @@ string g_dataFolder; // Store groundtruth values, read from files shared_ptrK g_calib; -map g_landmarks; // map: -map g_poses; // map: -std::vector g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position} +map g_landmarks; // map: +map g_poses; // map: +std::vector g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position} // Noise models SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); @@ -75,9 +72,9 @@ void readAllData() { * by adding and linking 2D features (measurements) detected in each captured image * with their corresponding landmarks. */ -Graph setupGraph(std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { +visualSLAM::Graph setupGraph(std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { - Graph g; + visualSLAM::Graph g; cout << "Built graph: " << endl; for (size_t i = 0; i < measurements.size(); i++) { @@ -86,8 +83,8 @@ Graph setupGraph(std::vector& measurements, SharedNoiseModel measurem g.addMeasurement( measurements[i].m_p, measurementSigma, - measurements[i].m_idCamera, - measurements[i].m_idLandmark, + Symbol('x',measurements[i].m_idCamera), + Symbol('l',measurements[i].m_idLandmark), calib); } @@ -101,15 +98,15 @@ Graph setupGraph(std::vector& measurements, SharedNoiseModel measurem */ Values initialize(std::map landmarks, std::map poses) { - Values initValues; + visualSLAM::Values initValues; // Initialize landmarks 3D positions. for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) - initValues.insert(PointKey(lmit->first), lmit->second); + initValues.insert(Symbol('l',lmit->first), lmit->second); // Initialize camera poses. for (map::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) - initValues.insert(PoseKey(poseit->first), poseit->second); + initValues.insert(Symbol('x',poseit->first), poseit->second); return initValues; } @@ -129,7 +126,7 @@ int main(int argc, char* argv[]) { readAllData(); // Create a graph using the 2D measurements (features) and the calibration data - Graph graph(setupGraph(g_measurements, measurementSigma, g_calib)); + visualSLAM::Graph graph(setupGraph(g_measurements, measurementSigma, g_calib)); // Create an initial Values structure using groundtruth values as the initial estimates Values initialEstimates(initialize(g_landmarks, g_poses)); @@ -139,7 +136,7 @@ int main(int argc, char* argv[]) { // Add prior factor for all poses in the graph map::iterator poseit = g_poses.begin(); for (; poseit != g_poses.end(); poseit++) - graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1)); + graph.addPosePrior(Symbol('x',poseit->first), poseit->second, noiseModel::Unit::Create(1)); // Optimize the graph cout << "*******************************************************" << endl; diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h index 52d492224..2365ef35c 100644 --- a/examples/vSLAMexample/vSLAMutils.h +++ b/examples/vSLAMexample/vSLAMutils.h @@ -17,13 +17,12 @@ #pragma once -#include -#include #include "Feature2D.h" #include "gtsam/geometry/Pose3.h" #include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Cal3_S2.h" - +#include +#include std::map readLandMarks(const std::string& landmarkFile); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 42c38514c..0d5724865 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -94,8 +94,8 @@ struct ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds[PoseKey::chr()] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds[PointKey::chr()] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5); // 0.1 rad rotation threshold, 0.5 m translation threshold + thresholds['l'] = Vector_(3, 1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index f45f560a7..2aec8e55a 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -95,7 +95,7 @@ pair load2D(const string& filename, is >> id >> x >> y >> yaw; // optional filter if (maxID && id >= maxID) continue; - poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw)); + poses->insert(id, Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); } @@ -132,10 +132,10 @@ pair load2D(const string& filename, l1Xl2 = l1Xl2.retract((*model)->sample()); // Insert vertices if pure odometry file - if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); - if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); + if (!poses->exists(id1)) poses->insert(id1, Pose2()); + if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model)); + pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index b490a76cb..e385cb66f 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -26,39 +26,39 @@ namespace planarSLAM { NonlinearFactorGraph(graph) {} /* ************************************************************************* */ - void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(PoseKey(i), p, model)); + void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(i, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPoseConstraint(Index i, const Pose2& p) { - sharedFactor factor(new Constraint(PoseKey(i), p)); + void Graph::addPoseConstraint(Key i, const Pose2& p) { + sharedFactor factor(new Constraint(i, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model)); + void Graph::addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model) { + sharedFactor factor(new Odometry(i1, i2, odometry, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) { - sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model)); + void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) { + sharedFactor factor(new Bearing(i, j, z, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) { - sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model)); + void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) { + sharedFactor factor(new Range(i, j, z, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addBearingRange(Index i, Index j, const Rot2& z1, + void Graph::addBearingRange(Key i, Key j, const Rot2& z1, double z2, const SharedNoiseModel& model) { - sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model)); + sharedFactor factor(new BearingRange(i, j, z1, z2, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 818879f26..f45c3bc86 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -11,7 +11,7 @@ /** * @file planarSLAM.h - * @brief: bearing/range measurements in 2D plane + * @brief bearing/range measurements in 2D plane * @author Frank Dellaert */ @@ -31,29 +31,25 @@ namespace planarSLAM { using namespace gtsam; - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a pose key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - /* * List of typedefs for factors */ - /// A hard constraint for PoseKeys to enforce particular values + + /// A hard constraint for poses to enforce particular values typedef NonlinearEquality Constraint; - /// A prior factor to bias the value of a PoseKey + /// A prior factor to bias the value of a pose typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 + /// A factor between two poses set with a Pose2 typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + /// A factor between a pose and a point to express difference in rotation (set with a Rot2) typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + /// A factor between a pose and a point to express distance between them (set with a double) typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location + /// A factor between a pose and a point to express difference in rotation and location typedef BearingRangeFactor BearingRange; - /** Values class, using specific PoseKeys and PointKeys + /** + * Values class, using specific poses and points * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods */ struct Values: public gtsam::Values { @@ -67,16 +63,16 @@ namespace planarSLAM { } /// get a pose - Pose2 pose(Index key) const { return at(PoseKey(key)); } + Pose2 pose(Key i) const { return at(i); } /// get a point - Point2 point(Index key) const { return at(PointKey(key)); } + Point2 point(Key j) const { return at(j); } /// insert a pose - void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } + void insertPose(Key i, const Pose2& pose) { insert(i, pose); } /// insert a point - void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); } + void insertPoint(Key j, const Point2& point) { insert(j, point); } }; /// Creates a NonlinearFactorGraph with the Values type @@ -88,23 +84,23 @@ namespace planarSLAM { /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph Graph(const NonlinearFactorGraph& graph); - /// Biases the value of PoseKey key with Pose2 p given a noise model - void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel); + /// Biases the value of pose key with Pose2 p given a noise model + void addPrior(Key i, const Pose2& pose, const SharedNoiseModel& noiseModel); - /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value - void addPoseConstraint(Index poseKey, const Pose2& pose); + /// Creates a hard constraint on the ith pose + void addPoseConstraint(Key i, const Pose2& pose); - /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) - void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model); + /// Creates an odometry factor between poses with keys i1 and i2 + void addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation - void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model); + /// Creates a bearing measurement from pose i to point j + void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location - void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model); + /// Creates a range measurement from pose i to point j + void addRange(Key i, Key k, double range, const SharedNoiseModel& model); - /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location - void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); + /// Creates a range/bearing measurement from pose i to point j + void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model); /// Optimize Values optimize(const Values& initialEstimate) const; diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index dd80b821b..b08f0c305 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -27,27 +27,26 @@ namespace pose2SLAM { Values x; double theta = 0, dtheta = 2 * M_PI / n; for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta)); + x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); return x; } /* ************************************************************************* */ - void Graph::addPrior(Index i, const Pose2& p, - const SharedNoiseModel& model) { - sharedFactor factor(new Prior(PoseKey(i), p, model)); + void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(i, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPoseConstraint(Index i, const Pose2& p) { - sharedFactor factor(new HardConstraint(PoseKey(i), p)); + void Graph::addPoseConstraint(Key i, const Pose2& p) { + sharedFactor factor(new HardConstraint(i, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addOdometry(Index i, Index j, const Pose2& z, + void Graph::addOdometry(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model)); + sharedFactor factor(new Odometry(i1, i2, z, model)); push_back(factor); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 50944cf2f..337ab9f7a 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -31,10 +31,7 @@ namespace pose2SLAM { using namespace gtsam; - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper + /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper struct Values: public gtsam::Values { /// Default constructor @@ -48,10 +45,10 @@ namespace pose2SLAM { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - Pose2 pose(Index key) const { return at(PoseKey(key)); } + Pose2 pose(Key i) const { return at(i); } /// insert a pose - void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } + void insertPose(Key i, const Pose2& pose) { insert(i, pose); } }; /** @@ -83,20 +80,18 @@ namespace pose2SLAM { /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph Graph(const NonlinearFactorGraph& graph); - /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph - void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model); + /// Adds a Pose2 prior with mean p and given noise model on pose i + void addPrior(Key i, const Pose2& p, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(Index i, const Pose2& p); + void addPoseConstraint(Key i, const Pose2& p); - /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph - void addOdometry(Index i, Index j, const Pose2& z, - const SharedNoiseModel& model); + /// Creates an odometry factor between poses with keys i1 and i2 + void addOdometry(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); /// AddConstraint adds a soft constraint between factor between keys i and j - void addConstraint(Index i, Index j, const Pose2& z, - const SharedNoiseModel& model) { - addOdometry(i,j,z,model); // same for now + void addConstraint(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) { + addOdometry(i1,i2,z,model); // same for now } /// Optimize diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 0ab962f1e..ffce28fe7 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -36,26 +36,26 @@ namespace pose3SLAM { Point3 gti(radius*cos(theta), radius*sin(theta), 0); Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! Pose3 gTi(gR0 * _0Ri, gti); - x.insert(PoseKey(i), gTi); + x.insert(i, gTi); } return x; } /* ************************************************************************* */ - void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(PoseKey(i), p, model)); + void Graph::addPrior(Key i, const Pose3& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(i, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model)); + void Graph::addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { + sharedFactor factor(new Constraint(i1, i2, z, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addHardConstraint(Index i, const Pose3& p) { - sharedFactor factor(new HardConstraint(PoseKey(i), p)); + void Graph::addHardConstraint(Key i, const Pose3& p) { + sharedFactor factor(new HardConstraint(i, p)); push_back(factor); } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 6b4efbe6c..e42238892 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -30,9 +30,6 @@ namespace pose3SLAM { using namespace gtsam; - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) * @param n number of poses @@ -52,13 +49,13 @@ namespace pose3SLAM { struct Graph: public NonlinearFactorGraph { /// Adds a factor between keys of the same type - void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model); + void addPrior(Key i, const Pose3& p, const SharedNoiseModel& model); /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph - void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model); + void addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose3 p. - void addHardConstraint(Index i, const Pose3& p); + void addHardConstraint(Key i, const Pose3& p); /// Optimize Values optimize(const Values& initialEstimate) { diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index e9b4f2bc9..403fac109 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -30,12 +30,6 @@ namespace simulated2D { // Simulated2D robots have no orientation, just a position - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a landmark key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - /** * Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper */ @@ -57,14 +51,14 @@ namespace simulated2D { } /// Insert a pose - void insertPose(Index j, const Point2& p) { - insert(PoseKey(j), p); + void insertPose(Key i, const Point2& p) { + insert(i, p); nrPoses_++; } /// Insert a point - void insertPoint(Index j, const Point2& p) { - insert(PointKey(j), p); + void insertPoint(Key j, const Point2& p) { + insert(j, p); nrPoints_++; } @@ -79,13 +73,13 @@ namespace simulated2D { } /// Return pose i - Point2 pose(Index j) const { - return at(PoseKey(j)); + Point2 pose(Key i) const { + return at(i); } /// Return point j - Point2 point(Index j) const { - return at(PointKey(j)); + Point2 point(Key j) const { + return at(j); } }; @@ -171,8 +165,8 @@ namespace simulated2D { Pose measured_; ///< odometry measurement /// Create odometry - GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) : - Base(model, key1, key2), measured_(measured) { + GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key i1, Key i2) : + Base(model, i1, i2), measured_(measured) { } /// Evaluate error and optionally return derivatives @@ -215,8 +209,8 @@ namespace simulated2D { Landmark measured_; ///< Measurement /// Create measurement factor - GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) : - Base(model, poseKey, landmarkKey), measured_(measured) { + GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key i, Key j) : + Base(model, i, j), measured_(measured) { } /// Evaluate error and optionally return derivatives diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index c7ebd466c..ed0d8ba1a 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -27,12 +28,6 @@ namespace simulated2DOriented { using namespace gtsam; - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a landmark key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - /// Specialized Values structure with syntactic sugar for /// compatibility with matlab class Values: public gtsam::Values { @@ -40,21 +35,23 @@ namespace simulated2DOriented { public: Values() : nrPoses_(0), nrPoints_(0) {} - void insertPose(Index j, const Pose2& p) { - insert(PoseKey(j), p); + /// insert a pose + void insertPose(Key i, const Pose2& p) { + insert(i, p); nrPoses_++; } - void insertPoint(Index j, const Point2& p) { - insert(PointKey(j), p); + /// insert a landmark + void insertPoint(Key j, const Point2& p) { + insert(j, p); nrPoints_++; } - int nrPoses() const { return nrPoses_; } - int nrPoints() const { return nrPoints_; } + int nrPoses() const { return nrPoses_; } ///< nr of poses + int nrPoints() const { return nrPoints_; } ///< nr of landmarks - Pose2 pose(Index j) const { return at(PoseKey(j)); } - Point2 point(Index j) const { return at(PointKey(j)); } + Pose2 pose(Key i) const { return at(i); } ///< get a pose + Point2 point(Key j) const { return at(j); } ///< get a landmark }; //TODO:: point prior is not implemented right now diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index fae2d2e22..2e9194466 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -17,28 +17,24 @@ * @author Frank dellaert */ -#include -#include +#include +#include +#include +#include +#include +#include + #include #include +#include +#include + using namespace std; -#include -#include -#include - -// template definitions -#include -#include -#include - namespace gtsam { namespace example { - using simulated2D::PoseKey; - using simulated2D::PointKey; - typedef boost::shared_ptr shared; static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); @@ -49,6 +45,7 @@ namespace example { static const Index _l1_=0, _x1_=1, _x2_=2; static const Index _x_=0, _y_=1, _z_=2; + // Convenience for named keys Key kx(size_t i) { return Symbol('x',i); } Key kl(size_t i) { return Symbol('l',i); } @@ -60,22 +57,22 @@ namespace example { // prior on x1 Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1))); + shared f1(new simulated2D::Prior(mu, sigma0_1, kx(1))); nlfg->push_back(f1); // odometry between x1 and x2 Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2))); + shared f2(new simulated2D::Odometry(z2, sigma0_1, kx(1), kx(2))); nlfg->push_back(f2); // measurement between x1 and l1 Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1))); + shared f3(new simulated2D::Measurement(z3, sigma0_2, kx(1), kl(1))); nlfg->push_back(f3); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1))); + shared f4(new simulated2D::Measurement(z4, sigma0_2, kx(2), kl(1))); nlfg->push_back(f4); return nlfg; @@ -89,9 +86,9 @@ namespace example { /* ************************************************************************* */ Values createValues() { Values c; - c.insert(PoseKey(1), Point2(0.0, 0.0)); - c.insert(PoseKey(2), Point2(1.5, 0.0)); - c.insert(PointKey(1), Point2(0.0, -1.0)); + c.insert(kx(1), Point2(0.0, 0.0)); + c.insert(kx(2), Point2(1.5, 0.0)); + c.insert(kl(1), Point2(0.0, -1.0)); return c; } @@ -107,9 +104,9 @@ namespace example { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { boost::shared_ptr c(new Values); - c->insert(PoseKey(1), Point2(0.1, 0.1)); - c->insert(PoseKey(2), Point2(1.4, 0.2)); - c->insert(PointKey(1), Point2(0.1, -1.1)); + c->insert(kx(1), Point2(0.1, 0.1)); + c->insert(kx(2), Point2(1.4, 0.2)); + c->insert(kl(1), Point2(0.1, -1.1)); return c; } @@ -223,7 +220,7 @@ namespace example { Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1))); + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), kx(1))); fg->push_back(factor); return fg; } @@ -241,23 +238,23 @@ namespace example { // prior on x1 Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1))); + shared prior(new simulated2D::Prior(x1, sigma1_0, kx(1))); nlfg.push_back(prior); - poses.insert(simulated2D::PoseKey(1), x1); + poses.insert(kx(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t))); + shared odometry(new simulated2D::Odometry(odo, sigma1_0, kx(t - 1), kx(t))); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t))); + shared measurement(new simulated2D::Prior(xt, sigma1_0, kx(t))); nlfg.push_back(measurement); // initial estimate - poses.insert(PoseKey(t), xt); + poses.insert(kx(t), xt); } return make_pair(nlfg, poses); @@ -418,7 +415,7 @@ namespace example { /* ************************************************************************* */ // Create key for simulated planar graph Symbol key(int x, int y) { - return PoseKey(1000*x+y); + return kx(1000*x+y); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 183b0c600..b73118352 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -27,8 +27,6 @@ using namespace std; using namespace gtsam; -using pose3SLAM::PoseKey; - /* ************************************************************************* */ TEST( AntiFactor, NegativeHessian) { @@ -42,17 +40,17 @@ TEST( AntiFactor, NegativeHessian) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(PoseKey(1), pose1); - values->insert(PoseKey(2), pose2); + values->insert(1, pose1); + values->insert(2, pose2); // Define an elimination ordering Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(PoseKey(1), 0); - ordering->insert(PoseKey(2), 1); + ordering->insert(1, 0); + ordering->insert(2, 1); // Create a "standard" factor - BetweenFactor::shared_ptr originalFactor(new BetweenFactor(PoseKey(1), PoseKey(2), z, sigma)); + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); // Linearize it into a Jacobian Factor GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); @@ -103,8 +101,8 @@ TEST( AntiFactor, EquivalentBayesNet) // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); - values->insert(PoseKey(1), pose1); - values->insert(PoseKey(2), pose2); + values->insert(1, pose1); + values->insert(2, pose2); // Define an elimination ordering Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); @@ -117,7 +115,7 @@ TEST( AntiFactor, EquivalentBayesNet) VectorValues expectedDeltas = optimize(*expectedBayesNet); // Add an additional factor between Pose1 and Pose2 - pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma)); + pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); graph->push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index b32bf9a9c..8859f7a08 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -28,6 +28,7 @@ using namespace planarSLAM; // some shared test values static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); +static Symbol i2('x',2), i3('x',3), j3('l',3); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(1,0.1)), @@ -37,7 +38,7 @@ SharedNoiseModel /* ************************************************************************* */ TEST( planarSLAM, PriorFactor_equals ) { - planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3); + planarSLAM::Prior factor1(i2, x1, I3), factor2(i2, x2, I3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -48,12 +49,12 @@ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); + planarSLAM::Bearing factor(i2, j3, z, sigma); // create config planarSLAM::Values c; - c.insert(PoseKey(2), x2); - c.insert(PointKey(3), l3); + c.insert(i2, x2); + c.insert(j3, l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -64,8 +65,8 @@ TEST( planarSLAM, BearingFactor ) TEST( planarSLAM, BearingFactor_equals ) { planarSLAM::Bearing - factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma), - factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma); + factor1(i2, j3, Rot2::fromAngle(0.1), sigma), + factor2(i2, j3, Rot2::fromAngle(2.3), sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -76,12 +77,12 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma); + planarSLAM::Range factor(i2, j3, z, sigma); // create config planarSLAM::Values c; - c.insert(PoseKey(2), x2); - c.insert(PointKey(3), l3); + c.insert(i2, x2); + c.insert(j3, l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -91,7 +92,7 @@ TEST( planarSLAM, RangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, RangeFactor_equals ) { - planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma); + planarSLAM::Range factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -103,12 +104,12 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); + planarSLAM::BearingRange factor(i2, j3, r, b, sigma2); // create config planarSLAM::Values c; - c.insert(PoseKey(2), x2); - c.insert(PointKey(3), l3); + c.insert(i2, x2); + c.insert(j3, l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -119,8 +120,8 @@ TEST( planarSLAM, BearingRangeFactor ) TEST( planarSLAM, BearingRangeFactor_equals ) { planarSLAM::BearingRange - factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2); + factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2), + factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -130,13 +131,13 @@ TEST( planarSLAM, BearingRangeFactor_equals ) TEST( planarSLAM, BearingRangeFactor_poses ) { typedef BearingRangeFactor PoseBearingRange; - PoseBearingRange actual(PoseKey(2), PoseKey(3), Rot2::fromDegrees(60.0), 12.3, sigma2); + PoseBearingRange actual(2, 3, Rot2::fromDegrees(60.0), 12.3, sigma2); } /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { - planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3); + planarSLAM::Constraint factor1(i2, x2), factor2(i2, x3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -147,26 +148,26 @@ TEST( planarSLAM, constructor ) { // create config planarSLAM::Values c; - c.insert(PoseKey(2), x2); - c.insert(PoseKey(3), x3); - c.insert(PointKey(3), l3); + c.insert(i2, x2); + c.insert(i3, x3); + c.insert(j3, l3); // create graph planarSLAM::Graph G; // Add pose constraint - G.addPoseConstraint(2, x2); // make it feasible :-) + G.addPoseConstraint(i2, x2); // make it feasible :-) // Add odometry - G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3); + G.addOdometry(i2, i3, Pose2(0, 0, M_PI_4), I3); // Create bearing factor Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - G.addBearing(2, 3, z1, sigma); + G.addBearing(i2, j3, z1, sigma); // Create range factor double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 - G.addRange(2, 3, z2, sigma); + G.addRange(i2, j3, z2, sigma); Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); Vector expected1 = Vector_(3, 0.0, 0.0, 0.0); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index a2d99ca88..580d40e8a 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -32,7 +32,6 @@ using namespace boost::assign; using namespace std; typedef pose2SLAM::Odometry Pose2Factor; -using pose2SLAM::PoseKey; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; @@ -44,21 +43,19 @@ static Matrix cov(Matrix_(3, 3, static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3)); -const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1); - /* ************************************************************************* */ // Test constraint, small displacement Vector f1(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); + Pose2Factor constraint(1, 2, z, covariance); return constraint.evaluateError(pose1, pose2); } -TEST( Pose2SLAM, constraint1 ) +TEST_UNSAFE( Pose2SLAM, constraint1 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); + Pose2Factor constraint(1, 2, pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -73,15 +70,15 @@ TEST( Pose2SLAM, constraint1 ) // Test constraint, large displacement Vector f2(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); + Pose2Factor constraint(1, 2, z, covariance); return constraint.evaluateError(pose1, pose2); } -TEST( Pose2SLAM, constraint2 ) +TEST_UNSAFE( Pose2SLAM, constraint2 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); + Pose2Factor constraint(1, 2, pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -93,7 +90,7 @@ TEST( Pose2SLAM, constraint2 ) } /* ************************************************************************* */ -TEST( Pose2SLAM, constructor ) +TEST_UNSAFE( Pose2SLAM, constructor ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); @@ -107,11 +104,11 @@ TEST( Pose2SLAM, constructor ) } /* ************************************************************************* */ -TEST( Pose2SLAM, linearization ) +TEST_UNSAFE( Pose2SLAM, linearization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); + Pose2Factor constraint(1, 2, measured, covariance); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); @@ -119,8 +116,8 @@ TEST( Pose2SLAM, linearization ) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) pose2SLAM::Values config; - config.insert(pose2SLAM::PoseKey(1),p1); - config.insert(pose2SLAM::PoseKey(2),p2); + config.insert(1,p1); + config.insert(2,p2); // Linearize Ordering ordering(*config.orderingArbitrary()); boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); @@ -140,13 +137,13 @@ TEST( Pose2SLAM, linearization ) Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[kx1], A1, ordering[kx2], A2, b, probModel1))); + lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[1], A1, ordering[2], A2, b, probModel1))); CHECK(assert_equal(lfg_expected, *lfg_linearized)); } /* ************************************************************************* */ -TEST(Pose2SLAM, optimize) { +TEST_UNSAFE(Pose2SLAM, optimize) { // create a Pose graph with one equality constraint and one measurement pose2SLAM::Graph fg; @@ -155,12 +152,12 @@ TEST(Pose2SLAM, optimize) { // Create initial config Values initial; - initial.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); - initial.insert(pose2SLAM::PoseKey(1), Pose2(0,0,0)); + initial.insert(0, Pose2(0,0,0)); + initial.insert(1, Pose2(0,0,0)); // Choose an ordering and optimize Ordering ordering; - ordering += kx0, kx1; + ordering += 0, 1; LevenbergMarquardtParams params; params.relativeErrorTol = 1e-15; @@ -169,23 +166,23 @@ TEST(Pose2SLAM, optimize) { // Check with expected config Values expected; - expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); - expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); + expected.insert(0, Pose2(0,0,0)); + expected.insert(1, Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, actual)); // Check marginals Marginals marginals = fg.marginals(actual); // Matrix expectedP0 = Infinity, as we have a pose constraint !? - // Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0)); + // Matrix actualP0 = marginals.marginalCovariance(0); // EQUALITY(expectedP0, actualP0); Matrix expectedP1 = cov; // the second pose really should have just the noise covariance - Matrix actualP1 = marginals.marginalCovariance(pose2SLAM::PoseKey(1)); + Matrix actualP1 = marginals.marginalCovariance(1); EQUALITY(expectedP1, actualP1); } /* ************************************************************************* */ -// test optimization with 3 poses -TEST(Pose2SLAM, optimizeThreePoses) { +// test optimization with 3 poses, note we use plain Keys here, not symbols +TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) { // Create a hexagon of poses pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); @@ -207,7 +204,7 @@ TEST(Pose2SLAM, optimizeThreePoses) { // Choose an ordering Ordering ordering; - ordering += kx0,kx1,kx2; + ordering += 0,1,2; // optimize LevenbergMarquardtParams params; @@ -231,12 +228,12 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { pose2SLAM::Graph fg; fg.addPoseConstraint(0, p0); Pose2 delta = p0.between(p1); - fg.addOdometry(0, 1, delta, covariance); + fg.addOdometry(0,1, delta, covariance); fg.addOdometry(1,2, delta, covariance); fg.addOdometry(2,3, delta, covariance); fg.addOdometry(3,4, delta, covariance); fg.addOdometry(4,5, delta, covariance); - fg.addOdometry(5, 0, delta, covariance); + fg.addOdometry(5,0, delta, covariance); // Create initial config pose2SLAM::Values initial; @@ -249,7 +246,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Choose an ordering Ordering ordering; - ordering += kx0,kx1,kx2,kx3,kx4,kx5; + ordering += 0,1,2,3,4,5; // optimize LevenbergMarquardtParams params; @@ -261,7 +258,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { CHECK(assert_equal((const Values&)hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))))); + CHECK(assert_equal(delta, actual.at(5).between(actual.at(0)))); // Pose2SLAMOptimizer myOptimizer("3"); @@ -299,7 +296,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { } /* ************************************************************************* */ -TEST(Pose2SLAM, optimize2) { +TEST_UNSAFE(Pose2SLAM, optimize2) { // Pose2SLAMOptimizer myOptimizer("100"); // Matrix A1 = myOptimizer.a1(); // Matrix A2 = myOptimizer.a2(); @@ -317,7 +314,7 @@ TEST(Pose2SLAM, optimize2) { } ///* ************************************************************************* */ -// SL-NEEDED? TEST(Pose2SLAM, findMinimumSpanningTree) { +// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) { // pose2SLAM::Graph G, T, C; // G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); @@ -331,7 +328,7 @@ TEST(Pose2SLAM, optimize2) { //} // ///* ************************************************************************* */ -// SL-NEEDED? TEST(Pose2SLAM, split) { +// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) { // pose2SLAM::Graph G, T, C; // G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); @@ -350,37 +347,37 @@ TEST(Pose2SLAM, optimize2) { using namespace pose2SLAM; /* ************************************************************************* */ -TEST(Pose2Values, pose2Circle ) +TEST_UNSAFE(Pose2Values, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m pose2SLAM::Values expected; - expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); - expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); - expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); - expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); + expected.insert(0, Pose2( 1, 0, M_PI_2)); + expected.insert(1, Pose2( 0, 1, - M_PI )); + expected.insert(2, Pose2(-1, 0, - M_PI_2)); + expected.insert(3, Pose2( 0, -1, 0 )); pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST(Pose2SLAM, expmap ) +TEST_UNSAFE(Pose2SLAM, expmap ) { // expected is circle shifted to right pose2SLAM::Values expected; - expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); - expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); - expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); - expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); + expected.insert(0, Pose2( 1.1, 0, M_PI_2)); + expected.insert(1, Pose2( 0.1, 1, - M_PI )); + expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); + expected.insert(3, Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); Ordering ordering(*circle.orderingArbitrary()); VectorValues delta(circle.dims(ordering)); - delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); + delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0); + delta[ordering[1]] = Vector_(3, -0.1,0.0,0.0); + delta[ordering[2]] = Vector_(3, 0.0,0.1,0.0); + delta[ordering[3]] = Vector_(3, 0.1,0.0,0.0); pose2SLAM::Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -390,15 +387,15 @@ static SharedNoiseModel sigmas = sharedSigmas(Vector_(3,sx,sy,st)); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) -TEST( Pose2Prior, error ) +TEST_UNSAFE( Pose2Prior, error ) { // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(PoseKey(1), p1); + x0.insert(1, p1); // Create factor - pose2SLAM::Prior factor(PoseKey(1), p1, sigmas); + pose2SLAM::Prior factor(1, p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -407,14 +404,14 @@ TEST( Pose2Prior, error ) // Check error at x0, i.e. delta = zero ! VectorValues delta(VectorValues::Zero(x0.dims(ordering))); - delta[ordering[kx1]] = zero(3); + delta[ordering[1]] = zero(3); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 VectorValues addition(VectorValues::Zero(x0.dims(ordering))); - addition[ordering[kx1]] = Vector_(3, 0.1, 0.0, 0.0); + addition[ordering[1]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; pose2SLAM::Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! @@ -425,7 +422,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); +static pose2SLAM::Prior priorFactor(1, priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector @@ -435,7 +432,7 @@ LieVector hprior(const Pose2& p1) { } /* ************************************************************************* */ -TEST( Pose2Prior, linearize ) +TEST_UNSAFE( Pose2Prior, linearize ) { // Choose a linearization point at ground truth pose2SLAM::Values x0; @@ -448,12 +445,12 @@ TEST( Pose2Prior, linearize ) // Test with numerical derivative Matrix numericalH = numericalDerivative11(hprior, priorVal); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[kx1])))); + CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[1])))); } /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) -TEST( Pose2Factor, error ) +TEST_UNSAFE( Pose2Factor, error ) { // Choose a linearization point Pose2 p1; // robot at origin @@ -464,7 +461,7 @@ TEST( Pose2Factor, error ) // Create factor Pose2 z = p1.between(p2); - Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance); + Pose2Factor factor(1, 2, z, covariance); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -473,15 +470,15 @@ TEST( Pose2Factor, error ) // Check error at x0, i.e. delta = zero ! VectorValues delta(x0.dims(ordering)); - delta[ordering[kx1]] = zero(3); - delta[ordering[kx2]] = zero(3); + delta[ordering[1]] = zero(3); + delta[ordering[2]] = zero(3); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); // Check error after increasing p2 VectorValues plus = delta; - plus[ordering[kx2]] = Vector_(3, 0.1, 0.0, 0.0); + plus[ordering[2]] = Vector_(3, 0.1, 0.0, 0.0); pose2SLAM::Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); @@ -491,17 +488,17 @@ TEST( Pose2Factor, error ) /* ************************************************************************* */ // common Pose2Factor for tests below static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); +static Pose2Factor factor(1,2,measured, covariance); /* ************************************************************************* */ -TEST( Pose2Factor, rhs ) +TEST_UNSAFE( Pose2Factor, rhs ) { // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1),p1); - x0.insert(pose2SLAM::PoseKey(2),p2); + x0.insert(1,p1); + x0.insert(2,p2); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -524,14 +521,14 @@ LieVector h(const Pose2& p1,const Pose2& p2) { } /* ************************************************************************* */ -TEST( Pose2Factor, linearize ) +TEST_UNSAFE( Pose2Factor, linearize ) { // Choose a linearization point at ground truth Pose2 p1(1,2,M_PI_2); Pose2 p2(-1,4,M_PI); pose2SLAM::Values x0; - x0.insert(pose2SLAM::PoseKey(1),p1); - x0.insert(pose2SLAM::PoseKey(2),p2); + x0.insert(1,p1); + x0.insert(2,p2); // expected linearization Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, @@ -549,7 +546,7 @@ TEST( Pose2Factor, linearize ) // expected linear factor Ordering ordering(*x0.orderingArbitrary()); SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering[kx1], expectedH1, ordering[kx2], expectedH2, expected_b, probModel1); + JacobianFactor expected(ordering[1], expectedH1, ordering[2], expectedH2, expected_b, probModel1); // Actual linearization boost::shared_ptr actual = diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index d4ab3afdb..4ebe7d27f 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -10,39 +10,33 @@ * -------------------------------------------------------------------------- */ /** - * @file testPose3Graph.cpp - * @author Frank Dellaert, Viorela Ila + * @file testpose3SLAM.cpp + * @author Frank Dellaert + * @author Viorela Ila **/ -#include +#include +#include +#include + +#include -#include #include #include #include using namespace boost; using namespace boost::assign; -#include - -// TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 6 - -#include -#include -#include +#include using namespace std; using namespace gtsam; -using namespace pose3SLAM; // common measurement covariance -static Matrix covariance = eye(6); +static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Unit::Create(6)); const double tol=1e-5; -const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5); - /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose3Graph, optimizeCircle) { @@ -50,7 +44,7 @@ TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; Values hexagon = pose3SLAM::circle(6,radius); - Pose3 gT0 = hexagon.at(PoseKey(0)), gT1 = hexagon.at(PoseKey(1)); + Pose3 gT0 = hexagon.at(0), gT1 = hexagon.at(1); // create a Pose graph with one equality constraint and one measurement pose3SLAM::Graph fg; @@ -67,16 +61,16 @@ TEST(Pose3Graph, optimizeCircle) { // Create initial config Values initial; - initial.insert(PoseKey(0), gT0); - initial.insert(PoseKey(1), hexagon.at(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(PoseKey(2), hexagon.at(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(PoseKey(3), hexagon.at(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial.insert(PoseKey(4), hexagon.at(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial.insert(PoseKey(5), hexagon.at(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial.insert(0, gT0); + initial.insert(1, hexagon.at(1).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial.insert(2, hexagon.at(2).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial.insert(3, hexagon.at(3).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial.insert(4, hexagon.at(4).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial.insert(5, hexagon.at(5).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize Ordering ordering; - ordering += kx0,kx1,kx2,kx3,kx4,kx5; + ordering += 0,1,2,3,4,5; Values actual = LevenbergMarquardtOptimizer(fg, initial, ordering).optimize(); @@ -84,7 +78,7 @@ TEST(Pose3Graph, optimizeCircle) { CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(_0T1, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))),1e-5)); + CHECK(assert_equal(_0T1, actual.at(5).between(actual.at(0)),1e-5)); } /* ************************************************************************* */ @@ -94,11 +88,10 @@ TEST(Pose3Graph, partial_prior_height) { // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // height prior - single element interface - Symbol key = PoseKey(1); double exp_height = 5.0; SharedDiagonal model = noiseModel::Unit::Create(1); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); - Partial height(key, 5, exp_height, model); + Partial height(1, 5, exp_height, model); Matrix actA; EXPECT(assert_equal(Vector_(1,-2.0), height.evaluateError(init, actA), tol)); Matrix expA = Matrix_(1, 6, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); @@ -108,13 +101,13 @@ TEST(Pose3Graph, partial_prior_height) { graph.add(height); Values values; - values.insert(key, init); + values.insert(1, init); // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(key), tol)); + EXPECT(assert_equal(expected, actual.at(1), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -128,12 +121,12 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - pose3SLAM::Constraint factor(PoseKey(1), PoseKey(2), z, I6); + pose3SLAM::Constraint factor(1, 2, z, I6); // Create config Values x; - x.insert(PoseKey(1),t1); - x.insert(PoseKey(2),t2); + x.insert(1,t1); + x.insert(2,t2); // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); @@ -146,12 +139,11 @@ TEST(Pose3Graph, partial_prior_xy) { typedef PartialPriorFactor Partial; // XY prior - full mask interface - Symbol key = PoseKey(1); Vector exp_xy = Vector_(2, 3.0, 4.0); SharedDiagonal model = noiseModel::Unit::Create(2); Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); vector mask; mask += 3, 4; - Partial priorXY(key, mask, exp_xy, model); + Partial priorXY(1, mask, exp_xy, model); Matrix actA; EXPECT(assert_equal(Vector_(2,-2.0,-6.0), priorXY.evaluateError(init, actA), tol)); Matrix expA = Matrix_(2, 6, @@ -163,10 +155,10 @@ TEST(Pose3Graph, partial_prior_xy) { graph.add(priorXY); Values values; - values.insert(key, init); + values.insert(1, init); Values actual = LevenbergMarquardtOptimizer(graph, values).optimize(); - EXPECT(assert_equal(expected, actual.at(key), tol)); + EXPECT(assert_equal(expected, actual.at(1), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -182,10 +174,10 @@ TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m Values expected; - expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0))); - expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0))); - expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0))); - expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0))); + expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); + expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); + expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); + expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); @@ -195,10 +187,10 @@ TEST( Values, pose3Circle ) TEST( Values, expmap ) { Values expected; - expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0))); + expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering)); diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 8b4ecbf21..80b35f269 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -16,13 +16,11 @@ * @date Nov 2009 */ -#include - #include +#include using namespace std; using namespace gtsam; -using namespace visualSLAM; // make cube static Point3 @@ -37,23 +35,23 @@ static Cal3_S2 K(fov,w,h); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static shared_ptrK sK(new Cal3_S2(K)); -const Key kx1 = Symbol('x',1), kl1 = Symbol('l',1); - -// make cameras +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } /* ************************************************************************* */ TEST( ProjectionFactor, error ) { // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); - int cameraFrameNumber=1, landmarkNumber=1; + int i=1, j=1; boost::shared_ptr - factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); + factor(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); // For the following values structure, the factor predicts 320,240 Values config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1); - Point3 l1; config.insert(PointKey(1), l1); + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(kx(1), x1); + Point3 l1; config.insert(kl(1), l1); // Point should project to Point2(320.,240.) CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config))); @@ -61,12 +59,12 @@ TEST( ProjectionFactor, error ) DOUBLES_EQUAL(4.5,factor->error(config),1e-9); // Check linearize - Ordering ordering; ordering += kx1,kl1; + Ordering ordering; ordering += kx(1),kl(1); Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); Vector b = Vector_(2,3.,0.); SharedDiagonal probModel1 = noiseModel::Unit::Create(2); - JacobianFactor expected(ordering[kx1], Ax1, ordering[kl1], Al1, b, probModel1); + JacobianFactor expected(ordering[kx(1)], Ax1, ordering[kl(1)], Al1, b, probModel1); JacobianFactor::shared_ptr actual = boost::dynamic_pointer_cast(factor->linearize(config, ordering)); CHECK(assert_equal(expected,*actual,1e-3)); @@ -81,11 +79,11 @@ TEST( ProjectionFactor, error ) // expmap on a config Values expected_config; - Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2); - Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2); + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(kx(1), x2); + Point3 l2(1,2,3); expected_config.insert(kl(1), l2); VectorValues delta(expected_config.dims(ordering)); - delta[ordering[kx1]] = Vector_(6, 0.,0.,0., 1.,1.,1.); - delta[ordering[kl1]] = Vector_(3, 1.,2.,3.); + delta[ordering[kx(1)]] = Vector_(6, 0.,0.,0., 1.,1.,1.); + delta[ordering[kl(1)]] = Vector_(3, 1.,2.,3.); Values actual_config = config.retract(delta, ordering); CHECK(assert_equal(expected_config,actual_config,1e-9)); } @@ -95,12 +93,12 @@ TEST( ProjectionFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - int cameraFrameNumber=1, landmarkNumber=1; + int i=1, j=1; boost::shared_ptr - factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); + factor1(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); boost::shared_ptr - factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); + factor2(new visualSLAM::ProjectionFactor(z, sigma, kx(i), kl(j), sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp index 9cfb8bb72..4442d73fc 100644 --- a/gtsam/slam/tests/testSerializationSLAM.cpp +++ b/gtsam/slam/tests/testSerializationSLAM.cpp @@ -11,7 +11,7 @@ /** * @file testSerializationSLAM.cpp - * @brief + * @brief test serialization * @author Richard Roberts * @date Feb 7, 2012 */ @@ -109,19 +109,20 @@ BOOST_CLASS_EXPORT(gtsam::Pose2) TEST (Serialization, planar_system) { using namespace planarSLAM; planarSLAM::Values values; - values.insert(PointKey(3), Point2(1.0, 2.0)); - values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); + Symbol i2('x',2), i3('x',3), i4('x',4), i9('x',9), j3('l',3), j5('l',5), j9('l',9); + values.insert(j3, Point2(1.0, 2.0)); + values.insert(i4, Pose2(1.0, 2.0, 0.3)); SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); - Range range(PoseKey(2), PointKey(9), 7.0, model1); - BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); + Prior prior(i3, Pose2(0.1,-0.3, 0.2), model1); + Bearing bearing(i3, j5, Rot2::fromDegrees(0.5), model1); + Range range(i2, j9, 7.0, model1); + BearingRange bearingRange(i2, j3, Rot2::fromDegrees(0.6), 2.0, model2); + Odometry odometry(i2, i3, Pose2(1.0, 2.0, 0.3), model3); + Constraint constraint(i9, Pose2(2.0,-1.0, 0.2)); Graph graph; graph.add(prior); @@ -132,8 +133,8 @@ TEST (Serialization, planar_system) { graph.add(constraint); // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(i2)); + EXPECT(equalsObj(j3)); EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); @@ -144,8 +145,8 @@ TEST (Serialization, planar_system) { EXPECT(equalsObj(graph)); // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(i2)); + EXPECT(equalsXML(j3)); EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); @@ -189,11 +190,11 @@ TEST (Serialization, visual_system) { boost::shared_ptr K(new Cal3_S2(cal1)); Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); - graph.addPointConstraint(1, pt1); - graph.addPointPrior(1, pt2, model3); - graph.addPoseConstraint(1, pose1); - graph.addPosePrior(1, pose2, model6); + graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); + graph.addPointConstraint(l1, pt1); + graph.addPointPrior(l1, pt2, model3); + graph.addPoseConstraint(x1, pose1); + graph.addPosePrior(x1, pose2, model6); EXPECT(equalsObj(values)); EXPECT(equalsObj(graph)); diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 3513927a7..b72a8f4c0 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -31,8 +31,8 @@ using namespace simulated2D; TEST( simulated2D, Simulated2DValues ) { simulated2D::Values actual; - actual.insert(simulated2D::PoseKey(1),Point2(1,1)); - actual.insert(simulated2D::PointKey(2),Point2(2,2)); + actual.insert(1,Point2(1,1)); + actual.insert(2,Point2(2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index 64844cb2a..b3d6e8d9e 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -28,6 +28,10 @@ using namespace std; using namespace gtsam; using namespace simulated2DOriented; +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ TEST( simulated2DOriented, Dprior ) { @@ -55,11 +59,11 @@ TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); SharedDiagonal model(Vector_(3, 1., 1., 1.)); - simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2)); + simulated2DOriented::Odometry factor(measurement, model, kx(1), kx(2)); simulated2DOriented::Values config; - config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2)); - config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1)); + config.insert(kx(1), Pose2(1., 0., 0.2)); + config.insert(kx(2), Pose2(2., 0., 0.1)); boost::shared_ptr lf = factor.linearize(config, *config.orderingArbitrary()); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 25cd6e16f..ffa512d96 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -12,24 +12,20 @@ /** * @file testStereoFactor.cpp * @brief Unit test for StereoFactor - * single camera * @author Chris Beall */ -#include - -#include -#include +#include +#include #include #include -#include +#include +#include -#include +#include using namespace std; using namespace gtsam; -using namespace boost; -using namespace visualSLAM; Pose3 camera1(Matrix_(3,3, 1., 0., 0., @@ -45,6 +41,10 @@ StereoCamera stereoCam(Pose3(), K); Point3 p(0, 0, 5); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { @@ -52,18 +52,18 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); + graph.add(visualSLAM::PoseConstraint(kx(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); + graph.add(visualSLAM::StereoFactor(z14,sigma, kx(1), kl(1), K)); // Create a configuration corresponding to the ground truth Values values; - values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin + values.insert(kx(1), camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); - values.insert(PointKey(1), l1); // add point at origin; + values.insert(kl(1), l1); // add point at origin; GaussNewtonOptimizer optimizer(graph, values); diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp similarity index 65% rename from gtsam/slam/tests/testVSLAM.cpp rename to gtsam/slam/tests/testVisualSLAM.cpp index bb7623954..27b930022 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -10,43 +10,45 @@ * -------------------------------------------------------------------------- */ /** - * @file testGraph.cpp - * @brief Unit test for two cameras and four landmarks - * single camera + * @file testVisualSLAM.cpp + * @brief Unit test for two cameras and four landmarks, single camera * @author Chris Beall * @author Frank Dellaert * @author Viorela Ila */ +#include +#include +#include + #include #include using namespace boost; -#include -#include -#include - using namespace std; using namespace gtsam; -using namespace boost; -using namespace visualSLAM; + +/* ************************************************************************* */ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); -/* ************************************************************************* */ -Point3 landmark1(-1.0,-1.0, 0.0); -Point3 landmark2(-1.0, 1.0, 0.0); -Point3 landmark3( 1.0, 1.0, 0.0); -Point3 landmark4( 1.0,-1.0, 0.0); +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } -Pose3 camera1(Matrix_(3,3, +static Point3 landmark1(-1.0,-1.0, 0.0); +static Point3 landmark2(-1.0, 1.0, 0.0); +static Point3 landmark3( 1.0, 1.0, 0.0); +static Point3 landmark4( 1.0,-1.0, 0.0); + +static Pose3 camera1(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. ), Point3(0,0,6.25)); -Pose3 camera2(Matrix_(3,3, +static Pose3 camera2(Matrix_(3,3, 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -66,14 +68,14 @@ visualSLAM::Graph testGraph() { shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0)); visualSLAM::Graph g; - g.addMeasurement(z11, sigma, 1, 1, sK); - g.addMeasurement(z12, sigma, 1, 2, sK); - g.addMeasurement(z13, sigma, 1, 3, sK); - g.addMeasurement(z14, sigma, 1, 4, sK); - g.addMeasurement(z21, sigma, 2, 1, sK); - g.addMeasurement(z22, sigma, 2, 2, sK); - g.addMeasurement(z23, sigma, 2, 3, sK); - g.addMeasurement(z24, sigma, 2, 4, sK); + g.addMeasurement(z11, sigma, kx(1), kl(1), sK); + g.addMeasurement(z12, sigma, kx(1), kl(2), sK); + g.addMeasurement(z13, sigma, kx(1), kl(3), sK); + g.addMeasurement(z14, sigma, kx(1), kl(4), sK); + g.addMeasurement(z21, sigma, kx(2), kl(1), sK); + g.addMeasurement(z22, sigma, kx(2), kl(2), sK); + g.addMeasurement(z23, sigma, kx(2), kl(3), sK); + g.addMeasurement(z24, sigma, kx(2), kl(4), sK); return g; } @@ -83,22 +85,22 @@ TEST( Graph, optimizeLM) // build a graph visualSLAM::Graph graph(testGraph()); // add 3 landmark constraints - graph.addPointConstraint(1, landmark1); - graph.addPointConstraint(2, landmark2); - graph.addPointConstraint(3, landmark3); + graph.addPointConstraint(kl(1), landmark1); + graph.addPointConstraint(kl(2), landmark2); + graph.addPointConstraint(kl(3), landmark3); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(PoseKey(1), camera1); - initialEstimate.insert(PoseKey(2), camera2); - initialEstimate.insert(PointKey(1), landmark1); - initialEstimate.insert(PointKey(2), landmark2); - initialEstimate.insert(PointKey(3), landmark3); - initialEstimate.insert(PointKey(4), landmark4); + initialEstimate.insert(kx(1), camera1); + initialEstimate.insert(kx(2), camera2); + initialEstimate.insert(kl(1), landmark1); + initialEstimate.insert(kl(2), landmark2); + initialEstimate.insert(kl(3), landmark3); + initialEstimate.insert(kl(4), landmark4); // Create an ordering of the variables Ordering ordering; - ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); + ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -121,21 +123,21 @@ TEST( Graph, optimizeLM2) // build a graph visualSLAM::Graph graph(testGraph()); // add 2 camera constraints - graph.addPoseConstraint(1, camera1); - graph.addPoseConstraint(2, camera2); + graph.addPoseConstraint(kx(1), camera1); + graph.addPoseConstraint(kx(2), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(PoseKey(1), camera1); - initialEstimate.insert(PoseKey(2), camera2); - initialEstimate.insert(PointKey(1), landmark1); - initialEstimate.insert(PointKey(2), landmark2); - initialEstimate.insert(PointKey(3), landmark3); - initialEstimate.insert(PointKey(4), landmark4); + initialEstimate.insert(kx(1), camera1); + initialEstimate.insert(kx(2), camera2); + initialEstimate.insert(kl(1), landmark1); + initialEstimate.insert(kl(2), landmark2); + initialEstimate.insert(kl(3), landmark3); + initialEstimate.insert(kl(4), landmark4); // Create an ordering of the variables Ordering ordering; - ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); + ordering += kl(1),kl(2),kl(3),kl(4),kx(1),kx(2); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -158,17 +160,17 @@ TEST( Graph, CHECK_ORDERING) // build a graph visualSLAM::Graph graph = testGraph(); // add 2 camera constraints - graph.addPoseConstraint(1, camera1); - graph.addPoseConstraint(2, camera2); + graph.addPoseConstraint(kx(1), camera1); + graph.addPoseConstraint(kx(2), camera2); // Create an initial values structure corresponding to the ground truth Values initialEstimate; - initialEstimate.insert(PoseKey(1), camera1); - initialEstimate.insert(PoseKey(2), camera2); - initialEstimate.insert(PointKey(1), landmark1); - initialEstimate.insert(PointKey(2), landmark2); - initialEstimate.insert(PointKey(3), landmark3); - initialEstimate.insert(PointKey(4), landmark4); + initialEstimate.insert(kx(1), camera1); + initialEstimate.insert(kx(2), camera2); + initialEstimate.insert(kl(1), landmark1); + initialEstimate.insert(kl(2), landmark2); + initialEstimate.insert(kl(3), landmark3); + initialEstimate.insert(kl(4), landmark4); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -189,21 +191,21 @@ TEST( Values, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables Values init; - init.insert(PoseKey(1), Pose3()); - init.insert(PointKey(1), Point3(1.0, 2.0, 3.0)); + init.insert(kx(1), Pose3()); + init.insert(kl(1), Point3(1.0, 2.0, 3.0)); Values expected; - expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1)); + expected.insert(kx(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(kl(1), Point3(1.1, 2.1, 3.1)); Ordering largeOrdering; Values largeValues = init; - largeValues.insert(PoseKey(2), Pose3()); - largeOrdering += PoseKey(1),PointKey(1),PoseKey(2); + largeValues.insert(kx(2), Pose3()); + largeOrdering += kx(1),kl(1),kx(2); VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering[PoseKey(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering[PointKey(1)]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering[PoseKey(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); + delta[largeOrdering[kx(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); + delta[largeOrdering[kl(1)]] = Vector_(3, 0.1, 0.1, 0.1); + delta[largeOrdering[kx(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index bc2ee199b..cc37e8bff 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -22,38 +22,38 @@ namespace visualSLAM { /* ************************************************************************* */ void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Index poseKey, Index pointKey, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); + Key poseKey, Key pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPoseConstraint(Index poseKey, const Pose3& p) { - boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); + void Graph::addPoseConstraint(Key poseKey, const Pose3& p) { + boost::shared_ptr factor(new PoseConstraint(poseKey, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPointConstraint(Index pointKey, const Point3& p) { - boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); + void Graph::addPointConstraint(Key pointKey, const Point3& p) { + boost::shared_ptr factor(new PointConstraint(pointKey, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) { - boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); + void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) { + boost::shared_ptr factor(new PosePrior(poseKey, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) { - boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); + void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { + boost::shared_ptr factor(new PointPrior(pointKey, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model) { - push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); + void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { + push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 38d3312a7..9933c4d9f 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -34,12 +34,6 @@ namespace visualSLAM { using namespace gtsam; - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a pose key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - /** * Typedefs that make up the visualSLAM namespace. */ @@ -85,21 +79,21 @@ namespace visualSLAM { * @param K shared pointer to calibration object */ void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Index poseKey, Index pointKey, const shared_ptrK& K); + Key poseKey, Key pointKey, const shared_ptrK& K); /** * Add a constraint on a pose (for now, *must* be satisfied in any Values) * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()); + void addPoseConstraint(Key poseKey, const Pose3& p = Pose3()); /** * Add a constraint on a point (for now, *must* be satisfied in any Values) * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(Index pointKey, const Point3& p = Point3()); + void addPointConstraint(Key pointKey, const Point3& p = Point3()); /** * Add a prior on a pose @@ -107,7 +101,7 @@ namespace visualSLAM { * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); + void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); /** * Add a prior on a landmark @@ -115,7 +109,7 @@ namespace visualSLAM { * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)); + void addPointPrior(Key pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)); /** * Add a range prior to a landmark @@ -124,7 +118,7 @@ namespace visualSLAM { * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); /** * Optimize the graph diff --git a/gtsam_unstable/slam/simulated3D.h b/gtsam_unstable/slam/simulated3D.h index 7b4dfce37..66552b926 100644 --- a/gtsam_unstable/slam/simulated3D.h +++ b/gtsam_unstable/slam/simulated3D.h @@ -36,12 +36,6 @@ namespace simulated3D { * the simulated2D domain. */ - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /// Convenience function for constructing a pose key - inline Symbol PointKey(Index j) { return Symbol('l', j); } - /** * Prior on a single pose */ @@ -105,9 +99,8 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { * @param poseKey is the pose key of the robot * @param pointKey is the point key for the landmark */ - Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey) : - NoiseModelFactor2(model, poseKey, pointKey), measured_(measured) {} + Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) : + NoiseModelFactor2(model, i, j), measured_(measured) {} /** * Error function with optional derivatives diff --git a/gtsam_unstable/slam/tests/testSimulated3D.cpp b/gtsam_unstable/slam/tests/testSimulated3D.cpp index 50171bc76..5aca0ce3a 100644 --- a/gtsam_unstable/slam/tests/testSimulated3D.cpp +++ b/gtsam_unstable/slam/tests/testSimulated3D.cpp @@ -30,8 +30,8 @@ using namespace simulated3D; TEST( simulated3D, Values ) { Values actual; - actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); - actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); + actual.insert(Symbol('l',1),Point3(1,1,1)); + actual.insert(Symbol('x',2),Point3(2,2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index e1b685fc8..35b1833ee 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -32,7 +32,7 @@ SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); // some simple inequality constraints -Symbol key(simulated2D::PoseKey(1)); +Symbol key('x',1); double mu = 10.0; // greater than iq2D::PoseXInequality constraint1(key, 1.0, true, mu); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index b16ac0cdc..ae8c6e00e 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -387,11 +387,11 @@ TEST(DoglegOptimizer, Iterate) { // config far from minimum Point2 x0(3,0); boost::shared_ptr config(new Values); - config->insert(simulated2D::PoseKey(1), x0); + config->insert(Symbol('x',1), x0); // ordering shared_ptr ord(new Ordering()); - ord->push_back(simulated2D::PoseKey(1)); + ord->push_back(Symbol('x',1)); double Delta = 1.0; for(size_t it=0; it<10; ++it) { diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 40e1ca4e1..cf2842af9 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -4,13 +4,6 @@ * @author Michael Kaess */ -#include -#include // for operator += -#include -using namespace boost::assign; - -#include - #include #include #include @@ -21,26 +14,32 @@ using namespace boost::assign; #include #include +#include + +#include +#include // for operator += +#include +using namespace boost::assign; + using namespace std; using namespace gtsam; -using namespace example; using boost::shared_ptr; const double tol = 1e-4; +// SETDEBUG("ISAM2 update", true); +// SETDEBUG("ISAM2 update verbose", true); +// SETDEBUG("ISAM2 recalculate", true); + +// Set up parameters +SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); +SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); + ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, boost::optional full_graph = boost::none, const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // These variables will be reused and accumulate factors and values ISAM2 isam(params); // ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); @@ -57,8 +56,8 @@ ISAM2 createSlamlikeISAM2( fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -70,8 +69,8 @@ ISAM2 createSlamlikeISAM2( fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -80,17 +79,17 @@ ISAM2 createSlamlikeISAM2( { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -103,8 +102,8 @@ ISAM2 createSlamlikeISAM2( fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -113,13 +112,13 @@ ISAM2 createSlamlikeISAM2( { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -139,10 +138,10 @@ TEST_UNSAFE(ISAM2, AddVariables) { // Create initial state Values theta; - theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); - theta.insert(planarSLAM::PointKey(0), Point2(.4, .5)); + theta.insert((0), Pose2(.1, .2, .3)); + theta.insert(100, Point2(.4, .5)); Values newTheta; - newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); + newTheta.insert((1), Pose2(.6, .7, .8)); VectorValues deltaUnpermuted; deltaUnpermuted.insert(0, Vector_(3, .1, .2, .3)); @@ -176,21 +175,21 @@ TEST_UNSAFE(ISAM2, AddVariables) { vector replacedKeys(2, false); - Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); + Ordering ordering; ordering += 100, (0); ISAM2::Nodes nodes(2); // Verify initial state - LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); - LONGS_EQUAL(1, ordering[planarSLAM::PoseKey(0)]); - EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[planarSLAM::PointKey(0)]])); - EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]])); + LONGS_EQUAL(0, ordering[100]); + LONGS_EQUAL(1, ordering[(0)]); + EXPECT(assert_equal(deltaUnpermuted[1], delta[ordering[100]])); + EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[(0)]])); // Create expected state Values thetaExpected; - thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); - thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5)); - thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); + thetaExpected.insert((0), Pose2(.1, .2, .3)); + thetaExpected.insert(100, Point2(.4, .5)); + thetaExpected.insert((1), Pose2(.6, .7, .8)); VectorValues deltaUnpermutedExpected; deltaUnpermutedExpected.insert(0, Vector_(3, .1, .2, .3)); @@ -230,7 +229,7 @@ TEST_UNSAFE(ISAM2, AddVariables) { vector replacedKeysExpected(3, false); - Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); + Ordering orderingExpected; orderingExpected += 100, (0), (1); ISAM2::Nodes nodesExpected( 3, ISAM2::sharedClique()); @@ -255,10 +254,10 @@ TEST_UNSAFE(ISAM2, AddVariables) { // using namespace gtsam::planarSLAM; // typedef GaussianISAM2::Impl Impl; // -// Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1); +// Ordering ordering; ordering += (0), (0), (1); // planarSLAM::Graph graph; -// graph.addPrior(PoseKey(0), Pose2(), sharedUnit(Pose2::dimension)); -// graph.addRange(PoseKey(0), PointKey(0), 1.0, sharedUnit(1)); +// graph.addPrior((0), Pose2(), sharedUnit(Pose2::dimension)); +// graph.addRange((0), (0), 1.0, sharedUnit(1)); // // FastSet expected; // expected.insert(0); @@ -307,7 +306,7 @@ TEST(ISAM2, optimize2) { // Create initialization Values theta; - theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01)); + theta.insert((0), Pose2(0.01, 0.01, 0.01)); // Create conditional Vector d(3); d << -0.1, -0.1, -0.31831; @@ -318,7 +317,7 @@ TEST(ISAM2, optimize2) { GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3))); // Create ordering - Ordering ordering; ordering += planarSLAM::PoseKey(0); + Ordering ordering; ordering += (0); // Expected vector VectorValues expected(1, 3); @@ -353,18 +352,6 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons TEST(ISAM2, slamlike_solution_gaussnewton) { -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; @@ -380,8 +367,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -395,8 +382,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -405,17 +392,17 @@ TEST(ISAM2, slamlike_solution_gaussnewton) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -428,8 +415,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -438,13 +425,13 @@ TEST(ISAM2, slamlike_solution_gaussnewton) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -485,19 +472,6 @@ TEST(ISAM2, slamlike_solution_gaussnewton) /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); Values fullinit; @@ -513,8 +487,8 @@ TEST(ISAM2, slamlike_solution_dogleg) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -528,8 +502,8 @@ TEST(ISAM2, slamlike_solution_dogleg) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -538,17 +512,17 @@ TEST(ISAM2, slamlike_solution_dogleg) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -561,8 +535,8 @@ TEST(ISAM2, slamlike_solution_dogleg) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -571,13 +545,13 @@ TEST(ISAM2, slamlike_solution_dogleg) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -618,19 +592,6 @@ TEST(ISAM2, slamlike_solution_dogleg) /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // 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; @@ -646,8 +607,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -661,8 +622,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -671,17 +632,17 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -694,8 +655,8 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -704,13 +665,13 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -751,19 +712,6 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg_qr) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // 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; @@ -779,8 +727,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -794,8 +742,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -804,17 +752,17 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -827,8 +775,8 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -837,13 +785,13 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -894,8 +842,8 @@ TEST(ISAM2, clone) { // Modify original isam NonlinearFactorGraph factors; - factors.add(BetweenFactor(Symbol('x',0), Symbol('x',10), - isam.calculateEstimate(Symbol('x',0)).between(isam.calculateEstimate(Symbol('x',10))), sharedUnit(3))); + factors.add(BetweenFactor(0, 10, + isam.calculateEstimate(0).between(isam.calculateEstimate(10)), sharedUnit(3))); isam.update(factors); CHECK(assert_equal(createSlamlikeISAM2(), clone2)); @@ -978,22 +926,9 @@ TEST(ISAM2, permute_cached) { /* ************************************************************************* */ TEST(ISAM2, removeFactors) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - // This test builds a graph in the same way as the "slamlike" test above, but // then removes the 2nd-to-last landmark measurement - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; @@ -1009,8 +944,8 @@ TEST(ISAM2, removeFactors) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -1024,8 +959,8 @@ TEST(ISAM2, removeFactors) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -1034,17 +969,17 @@ TEST(ISAM2, removeFactors) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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; @@ -1057,8 +992,8 @@ TEST(ISAM2, removeFactors) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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); } @@ -1067,14 +1002,14 @@ TEST(ISAM2, removeFactors) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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; @@ -1119,24 +1054,11 @@ TEST(ISAM2, removeFactors) } /* ************************************************************************* */ -TEST(ISAM2, swapFactors) +TEST_UNSAFE(ISAM2, swapFactors) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - // This test builds a graph in the same way as the "slamlike" test above, but // then swaps the 2nd-to-last landmark measurement with a different one - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - Values fullinit; planarSLAM::Graph fullgraph; ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph); @@ -1149,8 +1071,8 @@ TEST(ISAM2, swapFactors) fullgraph.remove(swap_idx); planarSLAM::Graph swapfactors; -// swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); +// 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); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } @@ -1191,19 +1113,6 @@ TEST(ISAM2, swapFactors) /* ************************************************************************* */ TEST(ISAM2, constrained_ordering) { - -// SETDEBUG("ISAM2 update", true); -// SETDEBUG("ISAM2 update verbose", true); -// SETDEBUG("ISAM2 recalculate", true); - - // Pose and landmark key types from planarSLAM - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - - // Set up parameters - SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); - // These variables will be reused and accumulate factors and values ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); Values fullinit; @@ -1211,8 +1120,8 @@ TEST(ISAM2, constrained_ordering) // We will constrain x3 and x4 to the end FastMap constrained; - constrained.insert(make_pair(planarSLAM::PoseKey(3), 1)); - constrained.insert(make_pair(planarSLAM::PoseKey(4), 2)); + constrained.insert(make_pair((3), 1)); + constrained.insert(make_pair((4), 2)); // i keeps track of the time step size_t i = 0; @@ -1224,8 +1133,8 @@ TEST(ISAM2, constrained_ordering) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); - fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); + init.insert((0), Pose2(0.01, 0.01, 0.01)); + fullinit.insert((0), Pose2(0.01, 0.01, 0.01)); isam.update(newfactors, init); } @@ -1239,8 +1148,8 @@ TEST(ISAM2, constrained_ordering) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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)); if(i >= 3) isam.update(newfactors, init, FastVector(), constrained); @@ -1252,17 +1161,17 @@ TEST(ISAM2, constrained_ordering) { planarSLAM::Graph newfactors; 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); + 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(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))); - fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); - fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); + 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, FastVector(), constrained); ++ i; @@ -1275,8 +1184,8 @@ TEST(ISAM2, constrained_ordering) fullgraph.push_back(newfactors); Values init; - init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); + 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, FastVector(), constrained); } @@ -1285,13 +1194,13 @@ TEST(ISAM2, constrained_ordering) { planarSLAM::Graph newfactors; 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); + 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(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); - fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); + 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, FastVector(), constrained); ++ i; @@ -1301,7 +1210,7 @@ TEST(ISAM2, constrained_ordering) EXPECT(isam_check(fullgraph, fullinit, isam)); // Check that x3 and x4 are last, but either can come before the other - EXPECT(isam.getOrdering()[planarSLAM::PoseKey(3)] == 12 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 13); + EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); // Check gradient at each node typedef ISAM2::sharedClique sharedClique; diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 074693123..6ec160a5d 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -125,9 +125,6 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTree, slamlike) { - using planarSLAM::PoseKey; - using planarSLAM::PointKey; - Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; @@ -137,39 +134,39 @@ TEST(GaussianJunctionTree, slamlike) { 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)); + newfactors.addPrior(kx(0), Pose2(0.0, 0.0, 0.0), odoNoise); + init.insert(kx(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)); + newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + init.insert(kx(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))); + newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + init.insert(kx(i+1), Pose2(1.01, 0.01, 0.01)); + init.insert(kl(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); + init.insert(kl(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)); + newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + init.insert(kx(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)); + newfactors.addOdometry(kx(i), kx(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addBearingRange(kx(i), kl(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.addBearingRange(kx(i), kl(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + init.insert(kx(i+1), Pose2(6.9, 0.1, 0.01)); fullgraph.push_back(newfactors); ++ i; @@ -195,12 +192,12 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(0, Pose2(), sharedSigma(3, 10.0)); - fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); + fg.addPrior(kx(0), Pose2(), sharedSigma(3, 10.0)); + fg.addOdometry(kx(0), kx(1), Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); Values init; - init.insert(pose2SLAM::PoseKey(0), Pose2()); - init.insert(pose2SLAM::PoseKey(1), Pose2(1.0, 0.0, 0.0)); + init.insert(kx(0), Pose2()); + init.insert(kx(1), Pose2(1.0, 0.0, 0.0)); Ordering ordering; ordering += kx(1), kx(0); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index f56efae82..c75c42b49 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -16,38 +16,34 @@ * @brief unit test for graph-inl.h */ -#include +#include +#include + +#include + #include #include // for operator += using namespace boost::assign; -#include - -// TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 3 - -#include -#include +#include using namespace std; using namespace gtsam; -Key kx(size_t i) { return Symbol('x',i); } - /* ************************************************************************* */ // x1 -> x2 // -> x3 -> x4 // -> x5 TEST ( Ordering, predecessorMap2Keys ) { PredecessorMap p_map; - p_map.insert(kx(1),kx(1)); - p_map.insert(kx(2),kx(1)); - p_map.insert(kx(3),kx(1)); - p_map.insert(kx(4),kx(3)); - p_map.insert(kx(5),kx(1)); + p_map.insert(1,1); + p_map.insert(2,1); + p_map.insert(3,1); + p_map.insert(4,3); + p_map.insert(5,1); list expected; - expected += kx(4),kx(5),kx(3),kx(2),kx(1);//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1); + expected += 4,5,3,2,1; list actual = predecessorMap2Keys(p_map); LONGS_EQUAL(expected.size(), actual.size()); @@ -67,20 +63,20 @@ TEST( Graph, predecessorMap2Graph ) map key2vertex; PredecessorMap p_map; - p_map.insert(kx(1), kx(2)); - p_map.insert(kx(2), kx(2)); - p_map.insert(kx(3), kx(2)); + p_map.insert(1, 2); + p_map.insert(2, 2); + p_map.insert(3, 2); tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); LONGS_EQUAL(3, boost::num_vertices(graph)); - CHECK(root == key2vertex[kx(2)]); + CHECK(root == key2vertex[2]); } /* ************************************************************************* */ TEST( Graph, composePoses ) { pose2SLAM::Graph graph; - Matrix cov = eye(3); + SharedNoiseModel cov = SharedNoiseModel::Unit(3); Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9); Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3); graph.addOdometry(1,2, p12, cov); @@ -88,10 +84,10 @@ TEST( Graph, composePoses ) graph.addOdometry(4,3, p43, cov); PredecessorMap tree; - tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2)); - tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2)); - tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2)); - tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3)); + tree.insert(1,2); + tree.insert(2,2); + tree.insert(3,2); + tree.insert(4,3); Pose2 rootPose = p2; @@ -99,10 +95,10 @@ TEST( Graph, composePoses ) Pose2, Key> (graph, tree, rootPose); Values expected; - expected.insert(pose2SLAM::PoseKey(1), p1); - expected.insert(pose2SLAM::PoseKey(2), p2); - expected.insert(pose2SLAM::PoseKey(3), p3); - expected.insert(pose2SLAM::PoseKey(4), p4); + expected.insert(1, p1); + expected.insert(2, p2); + expected.insert(3, p3); + expected.insert(4, p4); LONGS_EQUAL(4, actual->size()); CHECK(assert_equal(expected, *actual)); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 6ad070698..05573c686 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -25,6 +25,10 @@ using namespace std; using namespace gtsam; +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ // The tests below test the *generic* inference algorithms. Some of these have // specialized versions in the derived classes GaussianFactorGraph etc... @@ -52,23 +56,23 @@ TEST( inference, marginals2) SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1)); - fg.addPrior(0, Pose2(), poseModel); - fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel); - fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel); - fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel); + fg.addPrior(kx(0), Pose2(), poseModel); + fg.addOdometry(kx(0), kx(1), Pose2(1.0,0.0,0.0), poseModel); + fg.addOdometry(kx(1), kx(2), Pose2(1.0,0.0,0.0), poseModel); + fg.addBearingRange(kx(0), kl(0), Rot2(), 1.0, pointModel); + fg.addBearingRange(kx(1), kl(0), Rot2(), 1.0, pointModel); + fg.addBearingRange(kx(2), kl(0), Rot2(), 1.0, pointModel); Values init; - init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); - init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0)); - init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0)); - init.insert(planarSLAM::PointKey(0), Point2(1.0,1.0)); + init.insert(kx(0), Pose2(0.0,0.0,0.0)); + init.insert(kx(1), Pose2(1.0,0.0,0.0)); + init.insert(kx(2), Pose2(2.0,0.0,0.0)); + init.insert(kl(0), Point2(1.0,1.0)); Ordering ordering(*fg.orderingCOLAMD(init)); FactorGraph::shared_ptr gfg(fg.linearize(init, ordering)); GaussianMultifrontalSolver solver(*gfg); - solver.marginalFactor(ordering[planarSLAM::PointKey(0)]); + solver.marginalFactor(ordering[kl(0)]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 94ab58831..639e67591 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -535,13 +535,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VGraph graph; // create equality constraints for poses - graph.addPoseConstraint(1, camera1.pose()); - graph.addPoseConstraint(2, camera2.pose()); + graph.addPoseConstraint(x1, camera1.pose()); + graph.addPoseConstraint(x2, camera2.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(3); - graph.addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK); - graph.addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK); + graph.addMeasurement(camera1.project(landmark), vmodel, x1, l1, shK); + graph.addMeasurement(camera2.project(landmark), vmodel, x2, l2, shK); // add equality constraint graph.add(Point3Equality(l1, l2)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index d5e91d0d7..d1a891356 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -37,8 +37,9 @@ using namespace std; using namespace gtsam; using namespace example; -using simulated2D::PoseKey; -using simulated2D::PointKey; +// Convenience for named keys +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } typedef boost::shared_ptr shared_nlf; @@ -49,11 +50,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors Point2 z3(0.,-1.); - simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1)); + simulated2D::Measurement f0(z3, sigma, kx(1),kl(1)); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1)); + simulated2D::Measurement f1(z4, sigma, kx(2),kl(1)); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -199,16 +200,16 @@ TEST( NonlinearFactor, linearize_constraint1 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); - Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1))); + Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, kx(1))); Values config; - config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); + config.insert(kx(1), Point2(1.0, 2.0)); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, 0., -3.); - JacobianFactor expected(ord[PoseKey(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); + JacobianFactor expected(ord[kx(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -219,18 +220,18 @@ TEST( NonlinearFactor, linearize_constraint2 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); - simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1)); + simulated2D::Measurement f0(z3, constraint, kx(1),kl(1)); Values config; - config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); - config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); + config.insert(kx(1), Point2(1.0, 2.0)); + config.insert(kl(1), Point2(5.0, 4.0)); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); Vector b = Vector_(2, -15., -3.); - JacobianFactor expected(ord[PoseKey(1)], -1*A, ord[PointKey(1)], A, b, constraint); + JacobianFactor expected(ord[kx(1)], -1*A, ord[kl(1)], A, b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -238,7 +239,7 @@ TEST( NonlinearFactor, linearize_constraint2 ) class TestFactor4 : public NoiseModelFactor4 { public: typedef NoiseModelFactor4 Base; - TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4)) {} + TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4)) {} virtual Vector evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -262,13 +263,13 @@ public: TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; Values tv; - tv.insert(PoseKey(1), LieVector(1, 1.0)); - tv.insert(PoseKey(2), LieVector(1, 2.0)); - tv.insert(PoseKey(3), LieVector(1, 3.0)); - tv.insert(PoseKey(4), LieVector(1, 4.0)); + tv.insert(kx(1), LieVector(1, 1.0)); + tv.insert(kx(2), LieVector(1, 2.0)); + tv.insert(kx(3), LieVector(1, 3.0)); + tv.insert(kx(4), LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4); + Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -285,7 +286,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { class TestFactor5 : public NoiseModelFactor5 { public: typedef NoiseModelFactor5 Base; - TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5)) {} + TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -311,14 +312,14 @@ public: TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; Values tv; - tv.insert(PoseKey(1), LieVector(1, 1.0)); - tv.insert(PoseKey(2), LieVector(1, 2.0)); - tv.insert(PoseKey(3), LieVector(1, 3.0)); - tv.insert(PoseKey(4), LieVector(1, 4.0)); - tv.insert(PoseKey(5), LieVector(1, 5.0)); + tv.insert(kx(1), LieVector(1, 1.0)); + tv.insert(kx(2), LieVector(1, 2.0)); + tv.insert(kx(3), LieVector(1, 3.0)); + tv.insert(kx(4), LieVector(1, 4.0)); + tv.insert(kx(5), LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5); + Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -337,7 +338,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { class TestFactor6 : public NoiseModelFactor6 { public: typedef NoiseModelFactor6 Base; - TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6)) {} + TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), kx(1), kx(2), kx(3), kx(4), kx(5), kx(6)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -365,15 +366,15 @@ public: TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; Values tv; - tv.insert(PoseKey(1), LieVector(1, 1.0)); - tv.insert(PoseKey(2), LieVector(1, 2.0)); - tv.insert(PoseKey(3), LieVector(1, 3.0)); - tv.insert(PoseKey(4), LieVector(1, 4.0)); - tv.insert(PoseKey(5), LieVector(1, 5.0)); - tv.insert(PoseKey(6), LieVector(1, 6.0)); + tv.insert(kx(1), LieVector(1, 1.0)); + tv.insert(kx(2), LieVector(1, 2.0)); + tv.insert(kx(3), LieVector(1, 3.0)); + tv.insert(kx(4), LieVector(1, 4.0)); + tv.insert(kx(5), LieVector(1, 5.0)); + tv.insert(kx(6), LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6); + Ordering ordering; ordering += kx(1), kx(2), kx(3), kx(4), kx(5), kx(6); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -395,10 +396,10 @@ TEST(NonlinearFactor, NoiseModelFactor6) { TEST( NonlinearFactor, clone_rekey ) { shared_nlf init(new TestFactor4()); - EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]); + EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); + EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); + EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); + EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); // Standard clone shared_nlf actClone = init->clone(); @@ -407,24 +408,24 @@ TEST( NonlinearFactor, clone_rekey ) // Re-key factor - clones with different keys std::vector new_keys(4); - new_keys[0] = PoseKey(5); - new_keys[1] = PoseKey(6); - new_keys[2] = PoseKey(7); - new_keys[3] = PoseKey(8); + new_keys[0] = kx(5); + new_keys[1] = kx(6); + new_keys[2] = kx(7); + new_keys[3] = kx(8); shared_nlf actRekey = init->rekey(new_keys); EXPECT(actRekey.get() != init.get()); // Ensure different pointers // Ensure init is unchanged - EXPECT_LONGS_EQUAL(PoseKey(1), init->keys()[0]); - EXPECT_LONGS_EQUAL(PoseKey(2), init->keys()[1]); - EXPECT_LONGS_EQUAL(PoseKey(3), init->keys()[2]); - EXPECT_LONGS_EQUAL(PoseKey(4), init->keys()[3]); + EXPECT_LONGS_EQUAL(kx(1), init->keys()[0]); + EXPECT_LONGS_EQUAL(kx(2), init->keys()[1]); + EXPECT_LONGS_EQUAL(kx(3), init->keys()[2]); + EXPECT_LONGS_EQUAL(kx(4), init->keys()[3]); // Check new keys - EXPECT_LONGS_EQUAL(PoseKey(5), actRekey->keys()[0]); - EXPECT_LONGS_EQUAL(PoseKey(6), actRekey->keys()[1]); - EXPECT_LONGS_EQUAL(PoseKey(7), actRekey->keys()[2]); - EXPECT_LONGS_EQUAL(PoseKey(8), actRekey->keys()[3]); + EXPECT_LONGS_EQUAL(kx(5), actRekey->keys()[0]); + EXPECT_LONGS_EQUAL(kx(6), actRekey->keys()[1]); + EXPECT_LONGS_EQUAL(kx(7), actRekey->keys()[2]); + EXPECT_LONGS_EQUAL(kx(8), actRekey->keys()[3]); } /* ************************************************************************* */ diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 80ed7b6b6..cf8782e09 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -54,7 +54,7 @@ TEST(testNonlinearISAM, markov_chain ) { ordering.push_back(secondLast); isam.setOrdering(ordering); - Ordering expected; expected += PoseKey(0), PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(3); + Ordering expected; expected += (0), (1), (2), (4), (3); EXPECT(assert_equal(expected, isam.getOrdering())); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index eb6357fbd..1a134b8a3 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -50,7 +50,7 @@ TEST( NonlinearOptimizer, iterateLM ) // config far from minimum Point2 x0(3,0); Values config; - config.insert(simulated2D::PoseKey(1), x0); + config.insert(kx(1), x0); // normal iterate GaussNewtonParams gnParams; @@ -74,13 +74,13 @@ TEST( NonlinearOptimizer, optimize ) // test error at minimum Point2 xstar(0,0); Values cstar; - cstar.insert(simulated2D::PoseKey(1), xstar); + cstar.insert(kx(1), xstar); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters @@ -113,7 +113,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); Values actual = LevenbergMarquardtOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -126,7 +126,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); Values actual = GaussNewtonOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -139,7 +139,7 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); Values actual = DoglegOptimizer(fg, c0).optimize(); DOUBLES_EQUAL(0,fg.error(actual),tol); @@ -157,7 +157,7 @@ TEST( NonlinearOptimizer, optimization_method ) Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); @@ -170,23 +170,23 @@ TEST( NonlinearOptimizer, optimization_method ) TEST( NonlinearOptimizer, Factorization ) { Values config; - config.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); - config.insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.)); + config.insert(kx(1), Pose2(0.,0.,0.)); + config.insert(kx(2), Pose2(1.5,0.,0.)); pose2SLAM::Graph graph; - graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.addPrior(kx(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); + graph.addOdometry(kx(1),kx(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; - ordering.push_back(pose2SLAM::PoseKey(1)); - ordering.push_back(pose2SLAM::PoseKey(2)); + ordering.push_back(kx(1)); + ordering.push_back(kx(2)); LevenbergMarquardtOptimizer optimizer(graph, config, ordering); optimizer.iterate(); Values expected; - expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); - expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.)); + expected.insert(kx(1), Pose2(0.,0.,0.)); + expected.insert(kx(2), Pose2(1.,0.,0.)); CHECK(assert_equal(expected, optimizer.values(), 1e-5)); } @@ -201,13 +201,13 @@ TEST(NonlinearOptimizer, NullFactor) { // test error at minimum Point2 xstar(0,0); Values cstar; - cstar.insert(simulated2D::PoseKey(1), xstar); + cstar.insert(kx(1), xstar); DOUBLES_EQUAL(0.0,fg.error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); Values c0; - c0.insert(simulated2D::PoseKey(1), x0); + c0.insert(kx(1), x0); DOUBLES_EQUAL(199.0,fg.error(c0),1e-3); // optimize parameters