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