diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 703f6a47d..3ad5f4d7f 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -34,7 +34,7 @@ using namespace gtsam::pose2SLAM; int main(int argc, char** argv) { // create keys for robot positions - Key x1(1), x2(2), x3(3); + PoseKey x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ shared_ptr graph(new Graph); @@ -51,8 +51,8 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph->addConstraint(x1, x2, odom_measurement, odom_model); - graph->addConstraint(x2, x3, odom_measurement, odom_model); + graph->addOdometry(x1, x2, odom_measurement, odom_model); + graph->addOdometry(x2, x3, odom_measurement, odom_model); graph->print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 68ece651b..afc7a217e 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -32,7 +32,7 @@ using namespace gtsam::pose2SLAM; int main(int argc, char** argv) { // create keys for robot positions - Key x1(1), x2(2), x3(3); + PoseKey x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ Graph graph ; @@ -49,8 +49,8 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addConstraint(x1, x2, odom_measurement, odom_model); - graph.addConstraint(x2, x3, odom_measurement, odom_model); + graph.addOdometry(x1, x2, odom_measurement, odom_model); + graph.addOdometry(x2, x3, odom_measurement, odom_model); graph.print("full graph"); /* 3. Create the data structure to hold the initial estinmate to the solution diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d24058204..af964c0ab 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -28,8 +28,9 @@ using namespace gtsam; #define LINESIZE 81920 -typedef boost::shared_ptr sharedPose2Graph; -typedef boost::shared_ptr sharedPose2Values; +typedef boost::shared_ptr sharedPose2Graph; +typedef boost::shared_ptr sharedPose2Values; +typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility namespace gtsam { @@ -80,8 +81,8 @@ pair load2D(const string& filename, exit(-1); } - sharedPose2Values poses(new Pose2Values); - sharedPose2Graph graph(new Pose2Graph); + sharedPose2Values poses(new pose2SLAM::Values); + sharedPose2Graph graph(new pose2SLAM::Graph); string tag; @@ -135,7 +136,7 @@ pair load2D(const string& filename, if (!poses->exists(id1)) poses->insert(id1, Pose2()); if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); + pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); @@ -148,14 +149,14 @@ pair load2D(const string& filename, } /* ************************************************************************* */ -void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model, const string& filename) { - typedef Pose2Values::Key Key; + typedef pose2SLAM::Values::Key Key; fstream stream(filename.c_str(), fstream::out); // save poses - Pose2Values::Key key; + pose2SLAM::Values::Key key; Pose2 pose; BOOST_FOREACH(boost::tie(key, pose), config) stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; @@ -163,7 +164,7 @@ void save2D(const Pose2Graph& graph, const Pose2Values& config, const SharedDiag // save edges Matrix R = model->R(); Matrix RR = trans(R)*R;//prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr > factor_, graph) { + BOOST_FOREACH(boost::shared_ptr > factor_, graph) { boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); if (!factor) continue; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index e221f9da3..99e7f7eef 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -40,16 +40,16 @@ std::pair > * @param maxID, if non-zero cut out vertices >= maxID * @param smart: try to reduce complexity of covariance to cheapest model */ -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( std::pair > dataset, int maxID = 0, bool addNoise=false, bool smart=true); -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( const std::string& filename, boost::optional model = boost::optional(), int maxID = 0, bool addNoise=false, bool smart=true); /** save 2d graph */ -void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Values& config, const gtsam::SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model, const std::string& filename); /** diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 86dd3ea16..228ca3eed 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -11,7 +11,7 @@ /** * @file pose2SLAM.cpp - * @brief: bearing/range measurements in 2D plane + * @brief: Odometry measurements in 2D plane * @author Frank Dellaert **/ @@ -36,20 +36,20 @@ namespace gtsam { } /* ************************************************************************* */ - void Graph::addPrior(const Key& i, const Pose2& p, + void Graph::addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model) { sharedFactor factor(new Prior(i, p, model)); push_back(factor); } - void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(i, j, z, model)); - push_back(factor); - } + void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { + sharedFactor factor(new HardConstraint(i, p)); + push_back(factor); + } - void Graph::addHardConstraint(const Key& i, const Pose2& p) { - sharedFactor factor(new HardConstraint(i, p)); + void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + const SharedNoiseModel& model) { + sharedFactor factor(new Odometry(i, j, z, model)); push_back(factor); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 45017a81b..6f1b35386 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -17,7 +17,6 @@ #pragma once -#include #include #include #include @@ -31,67 +30,81 @@ namespace gtsam { - // Use pose2SLAM namespace for specific SLAM instance - namespace pose2SLAM { +// Use pose2SLAM namespace for specific SLAM instance +namespace pose2SLAM { - /// Keys with Pose2 and symbol 'x' - typedef TypedSymbol Key; + /// Keys with Pose2 and symbol 'x' + typedef TypedSymbol PoseKey; - /// Values with type 'Key' - typedef Values Values; + /// Values class, inherited from Values, using PoseKeys + struct Values: public gtsam::Values { - /** - * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @param c character to use for keys - * @return circle of n 2D poses - */ - Values circle(size_t n, double R); + /// Default constructor + Values() {} - /// A prior factor on Key with Pose2 data type. - typedef PriorFactor Prior; + /// Copy constructor + Values(const gtsam::Values& values) : + gtsam::Values(values) { + } - /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; + // Convenience for MATLAB wrapper, which does not allow for identically named methods - /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + /// get a pose + Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } - /// Graph - struct Graph: public NonlinearFactorGraph { + /// insert a pose + void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); } + }; - /// Adds a factor between keys of the same type - typedef BetweenFactor Constraint; + /** + * Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0) + * @param n number of poses + * @param R radius of circle + * @param c character to use for keys + * @return circle of n 2D poses + */ + Values circle(size_t n, double R); - /// A simple typedef from Pose2 to Pose - typedef Pose2 Pose; + /** + * List of typedefs for factors + */ - /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph - void addPrior(const Key& i, const Pose2& p, const SharedNoiseModel& model); + /// A hard constraint to enforce a specific value for a pose + typedef NonlinearEquality HardConstraint; + /// A prior factor on a pose with Pose2 data type. + typedef PriorFactor Prior; + /// A factor to add an odometry measurement between two poses. + typedef BetweenFactor Odometry; - /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph - void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedNoiseModel& model); + /// Graph + struct Graph: public NonlinearFactorGraph { - /// Creates a hard constraint for key i with the given Pose2 p. - void addHardConstraint(const Key& i, const Pose2& p); - }; + /// Default constructor for a NonlinearFactorGraph + Graph(){} - /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph); - /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; + /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph + void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); - } // pose2SLAM + /// Creates a hard constraint for key i with the given Pose2 p. + void addPoseConstraint(const PoseKey& i, const Pose2& p); - /** - * Backwards compatibility - */ - typedef pose2SLAM::Values Pose2Values; ///< Typedef for Values class for backwards compatibility - typedef pose2SLAM::Prior Pose2Prior; ///< Typedef for Prior class for backwards compatibility - typedef pose2SLAM::Constraint Pose2Factor; ///< Typedef for Constraint class for backwards compatibility - typedef pose2SLAM::Graph Pose2Graph; ///< Typedef for Graph class for backwards compatibility + /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph + void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + const SharedNoiseModel& model); + }; + + /// The sequential optimizer + typedef NonlinearOptimizer OptimizerSequential; + + /// The multifrontal optimizer + typedef NonlinearOptimizer Optimizer; + + } // pose2SLAM } // namespace gtsam diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index c2c11b937..f47339233 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -33,6 +33,8 @@ using namespace boost::assign; #include using namespace std; +typedef pose2SLAM::Odometry Pose2Factor; + // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; static noiseModel::Gaussian::shared_ptr covariance( @@ -94,8 +96,8 @@ TEST( Pose2SLAM, constructor ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Graph graph; - graph.addConstraint(1,2,measured, covariance); + pose2SLAM::Graph graph; + graph.addOdometry(1,2,measured, covariance); // get the size of the graph size_t actual = graph.size(); // verify @@ -109,13 +111,13 @@ TEST( Pose2SLAM, linearization ) // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); Pose2Factor constraint(1,2,measured, covariance); - Pose2Graph graph; - graph.addConstraint(1,2,measured, covariance); + pose2SLAM::Graph graph; + graph.addOdometry(1,2,measured, covariance); // 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) looking at negative (ground truth is at 4.1,2) - Pose2Values config; + pose2SLAM::Values config; config.insert(1,p1); config.insert(2,p2); // Linearize @@ -146,26 +148,26 @@ TEST( Pose2SLAM, linearization ) TEST(Pose2Graph, optimize) { // create a Pose graph with one equality constraint and one measurement - shared_ptr fg(new Pose2Graph); - fg->addHardConstraint(0, Pose2(0,0,0)); - fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance); + shared_ptr fg(new pose2SLAM::Graph); + fg->addPoseConstraint(0, Pose2(0,0,0)); + fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config - boost::shared_ptr initial(new Pose2Values()); + boost::shared_ptr initial(new pose2SLAM::Values()); initial->insert(0, Pose2(0,0,0)); initial->insert(1, Pose2(0,0,0)); // Choose an ordering and optimize shared_ptr ordering(new Ordering); *ordering += "x0","x1"; - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; NonlinearOptimizationParameters::sharedThis params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); Optimizer optimizer0(fg, initial, ordering, params); Optimizer optimizer = optimizer0.levenbergMarquardt(); // Check with expected config - Pose2Values expected; + pose2SLAM::Values expected; expected.insert(0, Pose2(0,0,0)); expected.insert(1, Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.values())); @@ -176,19 +178,19 @@ TEST(Pose2Graph, optimize) { TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses - Pose2Values hexagon = pose2SLAM::circle(3,1.0); + pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); Pose2 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement - shared_ptr fg(new Pose2Graph); - fg->addHardConstraint(0, p0); + shared_ptr fg(new pose2SLAM::Graph); + fg->addPoseConstraint(0, p0); Pose2 delta = p0.between(p1); - fg->addConstraint(0, 1, delta, covariance); - fg->addConstraint(1, 2, delta, covariance); - fg->addConstraint(2, 0, delta, covariance); + fg->addOdometry(0, 1, delta, covariance); + fg->addOdometry(1, 2, delta, covariance); + fg->addOdometry(2, 0, delta, covariance); // Create initial config - boost::shared_ptr initial(new Pose2Values()); + boost::shared_ptr initial(new pose2SLAM::Values()); initial->insert(0, p0); initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); @@ -202,7 +204,7 @@ TEST(Pose2Graph, optimizeThreePoses) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - Pose2Values actual = *optimizer.values(); + pose2SLAM::Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -210,25 +212,25 @@ TEST(Pose2Graph, optimizeThreePoses) { /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure -TEST_UNSAFE(Pose2Graph, optimizeCircle) { +TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create a hexagon of poses - Pose2Values hexagon = pose2SLAM::circle(6,1.0); + pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0); Pose2 p0 = hexagon[0], p1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement - shared_ptr fg(new Pose2Graph); - fg->addHardConstraint(0, p0); + shared_ptr fg(new pose2SLAM::Graph); + fg->addPoseConstraint(0, p0); Pose2 delta = p0.between(p1); - fg->addConstraint(0, 1, delta, covariance); - fg->addConstraint(1,2, delta, covariance); - fg->addConstraint(2,3, delta, covariance); - fg->addConstraint(3,4, delta, covariance); - fg->addConstraint(4,5, delta, covariance); - fg->addConstraint(5, 0, 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); // Create initial config - boost::shared_ptr initial(new Pose2Values()); + boost::shared_ptr initial(new pose2SLAM::Values()); initial->insert(0, p0); initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); @@ -245,7 +247,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - Pose2Values actual = *optimizer.values(); + pose2SLAM::Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual)); @@ -279,7 +281,7 @@ TEST_UNSAFE(Pose2Graph, optimizeCircle) { // // myOptimizer.update(x); // -// Pose2Values expected; +// pose2SLAM::Values expected; // expected.insert(0, Pose2(0.,0.,0.)); // expected.insert(1, Pose2(1.,0.,0.)); // expected.insert(2, Pose2(2.,0.,0.)); @@ -307,8 +309,8 @@ TEST(Pose2Graph, optimize2) { } ///* ************************************************************************* */ -// SL-NEEDED? TEST(Pose2Graph, findMinimumSpanningTree) { -// Pose2Graph G, T, C; +// SL-NEEDED? TEST(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); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); @@ -321,8 +323,8 @@ TEST(Pose2Graph, optimize2) { //} // ///* ************************************************************************* */ -// SL-NEEDED? TEST(Pose2Graph, split) { -// Pose2Graph G, T, C; +// SL-NEEDED? TEST(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); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); @@ -340,38 +342,38 @@ TEST(Pose2Graph, optimize2) { using namespace pose2SLAM; /* ************************************************************************* */ -TEST( Pose2Values, pose2Circle ) +TEST(Pose2Values, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m - Pose2Values expected; + pose2SLAM::Values expected; 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 )); - Pose2Values actual = pose2SLAM::circle(4,1.0); + pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( Pose2Values, expmap ) +TEST(Pose2SLAM, expmap ) { // expected is circle shifted to right - Pose2Values expected; + pose2SLAM::Values expected; 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 !!! - Pose2Values circle(pose2SLAM::circle(4,1.0)); + pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); Ordering ordering(*circle.orderingArbitrary()); VectorValues delta(circle.dims(ordering)); - delta[ordering[Key(0)]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[Key(1)]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[Key(2)]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[Key(3)]] = Vector_(3, 0.1,0.0,0.0); - Pose2Values actual = circle.retract(delta, ordering); + delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); + delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); + delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); + delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); + pose2SLAM::Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -384,11 +386,11 @@ TEST( Pose2Prior, error ) { // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) - Pose2Values x0; + pose2SLAM::Values x0; x0.insert(1, p1); // Create factor - Pose2Prior factor(1, p1, sigmas); + pose2SLAM::Prior factor(1, p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -406,7 +408,7 @@ TEST( Pose2Prior, error ) VectorValues addition(VectorValues::Zero(x0.dims(ordering))); addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; - Pose2Values x1 = x0.retract(plus, ordering); + 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))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); @@ -415,7 +417,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static Pose2Prior priorFactor(1,priorVal, sigmas); +static pose2SLAM::Prior priorFactor(1,priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector @@ -428,7 +430,7 @@ LieVector hprior(const Pose2& p1) { TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth - Pose2Values x0; + pose2SLAM::Values x0; x0.insert(1,priorVal); // Actual linearization @@ -448,7 +450,7 @@ TEST( Pose2Factor, error ) // Choose a linearization point Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) - Pose2Values x0; + pose2SLAM::Values x0; x0.insert(1, p1); x0.insert(2, p2); @@ -472,7 +474,7 @@ TEST( Pose2Factor, error ) // Check error after increasing p2 VectorValues plus = delta; plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); - Pose2Values x1 = x0.retract(plus, ordering); + 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))); CHECK(assert_equal(error_at_plus,linear->error_vector(plus))); @@ -489,7 +491,7 @@ TEST( 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) - Pose2Values x0; + pose2SLAM::Values x0; x0.insert(1,p1); x0.insert(2,p2); @@ -519,7 +521,7 @@ TEST( Pose2Factor, linearize ) // Choose a linearization point at ground truth Pose2 p1(1,2,M_PI_2); Pose2 p2(-1,4,M_PI); - Pose2Values x0; + pose2SLAM::Values x0; x0.insert(1,p1); x0.insert(2,p2); diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index c7418a673..da9a8aede 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -195,8 +195,8 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0)); - fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); + fg.addPrior(pose2SLAM::PoseKey(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))); pose2SLAM::Values init; init.insert(0, Pose2()); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index e9698a753..db9874c89 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -78,15 +78,15 @@ TEST( Graph, predecessorMap2Graph ) /* ************************************************************************* */ TEST( Graph, composePoses ) { - Pose2Graph graph; + pose2SLAM::Graph graph; Matrix cov = eye(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.addConstraint(1,2, p12, cov); - graph.addConstraint(2,3, p23, cov); - graph.addConstraint(4,3, p43, cov); + graph.addOdometry(1,2, p12, cov); + graph.addOdometry(2,3, p23, cov); + graph.addOdometry(4,3, p43, cov); - PredecessorMap tree; + PredecessorMap tree; tree.insert(1,2); tree.insert(2,2); tree.insert(3,2); @@ -94,10 +94,10 @@ TEST( Graph, composePoses ) Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses (graph, tree, rootPose); - Pose2Values expected; + pose2SLAM::Values expected; expected.insert(1, p1); expected.insert(2, p2); expected.insert(3, p3); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index ccc1748dc..5f0cdb8a1 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -253,24 +253,24 @@ TEST( NonlinearOptimizer, optimization_method ) /* ************************************************************************* */ TEST( NonlinearOptimizer, Factorization ) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; - boost::shared_ptr config(new Pose2Values); + boost::shared_ptr config(new pose2SLAM::Values); config->insert(1, Pose2(0.,0.,0.)); config->insert(2, Pose2(1.5,0.,0.)); - boost::shared_ptr graph(new Pose2Graph); + boost::shared_ptr graph(new pose2SLAM::Graph); graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph->addConstraint(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph->addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); boost::shared_ptr ordering(new Ordering); - ordering->push_back(Pose2Values::Key(1)); - ordering->push_back(Pose2Values::Key(2)); + ordering->push_back(pose2SLAM::PoseKey(1)); + ordering->push_back(pose2SLAM::PoseKey(2)); Optimizer optimizer(graph, config, ordering); Optimizer optimized = optimizer.iterateLM(); - Pose2Values expected; + pose2SLAM::Values expected; expected.insert(1, Pose2(0.,0.,0.)); expected.insert(2, Pose2(1.,0.,0.)); CHECK(assert_equal(expected, *optimized.values(), 1e-5)); diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index c4d23ad73..8fe127989 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -44,13 +44,13 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005)); else - data.first->addHardConstraint(0, Pose2()); + data.first->addPoseConstraint(0, Pose2()); tic_(1, "order"); Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second)); diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index 063ecd651..d9b71576f 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -44,13 +44,13 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) data.first->addPrior(0, Pose2(), sharedSigma(Pose2::Dim(), 0.0005)); else - data.first->addHardConstraint(0, Pose2()); + data.first->addPoseConstraint(0, Pose2()); tic_(1, "order"); Ordering::shared_ptr ordering(data.first->orderingCOLAMD(*data.second));