From 2d0ce1c3cae0c8c33207554e0b13144239fdc2d1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 24 Jun 2012 02:48:12 +0000 Subject: [PATCH] Streamlining of SLAM namespaces: planarSLAM Values and Graph now derive from Pose3SLAM. visualSLAM Values and Graph now derive from pose3SLAM. Several methods have been renamed to make them consistent through these 4 namespaces: addPrior -> addPosePrior addHardConstraint -> addPoseConstraint addOdometry/addConstraint -> addRelativePose All gtsam and matlab examples/tests run. PS: please don't use the deprecated typedefs in these namespaces. --- examples/LocalizationExample.cpp | 4 +- examples/OdometryExample.cpp | 6 +- examples/PlanarSLAMExample.cpp | 6 +- examples/Pose2SLAMExample.cpp | 12 +- examples/Pose2SLAMExample_advanced.cpp | 6 +- examples/Pose2SLAMExample_graph.cpp | 2 +- examples/Pose2SLAMwSPCG.cpp | 12 +- examples/VisualISAMExample.cpp | 4 +- gtsam.h | 103 ++++++----- gtsam/slam/dataset.cpp | 6 +- gtsam/slam/planarSLAM.cpp | 38 ++--- gtsam/slam/planarSLAM.h | 73 +++----- gtsam/slam/pose2SLAM.cpp | 61 +++---- gtsam/slam/pose2SLAM.h | 47 +++-- gtsam/slam/pose3SLAM.cpp | 57 +++---- gtsam/slam/pose3SLAM.h | 87 +++++++--- gtsam/slam/tests/testAntiFactor.cpp | 6 +- gtsam/slam/tests/testPlanarSLAM.cpp | 18 +- gtsam/slam/tests/testPose2SLAM.cpp | 55 +++--- gtsam/slam/tests/testPose3SLAM.cpp | 16 +- gtsam/slam/tests/testProjectionFactor.cpp | 14 +- gtsam/slam/tests/testStereoFactor.cpp | 6 +- gtsam/slam/visualSLAM.cpp | 78 ++------- gtsam/slam/visualSLAM.h | 161 +++++------------- .../{examples => }/VisualISAMGenerateData.m | 0 matlab/VisualISAMInitialize.m | 2 +- matlab/VisualISAMStep.m | 2 +- matlab/examples/LocalizationExample.m | 15 +- matlab/examples/OdometryExample.m | 11 +- matlab/examples/PlanarSLAMExample.m | 8 +- matlab/examples/PlanarSLAMExample_sampling.m | 6 +- matlab/examples/Pose2SLAMExample.m | 17 +- matlab/examples/Pose2SLAMExample_advanced.m | 8 +- matlab/examples/Pose2SLAMExample_circle.m | 12 +- matlab/examples/Pose2SLAMExample_graph.m | 2 +- matlab/examples/Pose2SLAMwSPCG.m | 2 +- matlab/examples/Pose3SLAMExample.m | 19 ++- matlab/examples/StereoVOExample.m | 10 +- matlab/load3D.m | 2 +- matlab/plot3DTrajectory.m | 3 +- matlab/tests/testLocalizationExample.m | 8 +- matlab/tests/testOdometryExample.m | 8 +- matlab/tests/testPlanarSLAMExample.m | 8 +- matlab/tests/testPose2SLAMExample.m | 16 +- matlab/tests/testPose3SLAMExample.m | 16 +- tests/testGaussianISAM2.cpp | 72 ++++---- tests/testGaussianJunctionTreeB.cpp | 14 +- tests/testGradientDescentOptimizer.cpp | 12 +- tests/testGraph.cpp | 6 +- tests/testInferenceB.cpp | 6 +- tests/testNonlinearISAM.cpp | 2 +- tests/testNonlinearOptimizer.cpp | 2 +- tests/testOccupancyGrid.cpp | 4 +- tests/timeMultifrontalOnDataset.cpp | 2 +- tests/timeSequentialOnDataset.cpp | 2 +- 55 files changed, 523 insertions(+), 652 deletions(-) rename matlab/{examples => }/VisualISAMGenerateData.m (100%) diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 39f8de8a6..21fb36860 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -65,8 +65,8 @@ int main(int argc, char** argv) { // 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 = Diagonal::Sigmas(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.addRelativePose(1, 2, odometry, odometryNoise); + graph.addRelativePose(2, 3, odometry, odometryNoise); // add unary measurement factors, like GPS, on all three poses SharedDiagonal noiseModel = Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 87844dfbc..36ad5f228 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -42,13 +42,13 @@ int main(int argc, char** argv) { // add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin SharedDiagonal priorNoise = Diagonal::Sigmas(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.addPosePrior(1, 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 = Diagonal::Sigmas(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.addRelativePose(1, 2, odometry, odometryNoise); + graph.addRelativePose(2, 3, odometry, odometryNoise); // print graph.print("\nFactor graph:\n"); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 2d739416a..f05ffa9b2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -46,13 +46,13 @@ int main(int argc, char** argv) { // add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta - graph.addPrior(i1, priorMean, priorNoise); // add directly to graph + graph.addPosePrior(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 = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta - graph.addOdometry(i1, i2, odometry, odometryNoise); - graph.addOdometry(i2, i3, odometry, odometryNoise); + graph.addRelativePose(i1, i2, odometry, odometryNoise); + graph.addRelativePose(i2, i3, odometry, odometryNoise); // create a noise model for the landmark measurements SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 61bdb1bda..0777301ea 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -32,18 +32,18 @@ int main(int argc, char** argv) { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPrior(1, priorMean, priorNoise); + graph.addPosePrior(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); - graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model); + graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), model); // print graph.print("\nFactor graph:\n"); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index f727e27e2..b5fcddfcf 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -38,13 +38,13 @@ int main(int argc, char** argv) { /* 2.a add prior */ Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = Diagonal::Sigmas(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.addPosePrior(1, priorMean, priorNoise); // add directly to graph /* 2.b add odometry */ SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(1, 2, odometry, odometryNoise); - graph.addOdometry(2, 3, odometry, odometryNoise); + graph.addRelativePose(1, 2, odometry, odometryNoise); + graph.addRelativePose(2, 3, odometry, odometryNoise); graph.print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 4dcdbea08..d27e184d8 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -39,7 +39,7 @@ int main(int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01)); - graph->addPrior(0, priorMean, priorNoise); + graph->addPosePrior(0, priorMean, priorNoise); // Single Step Optimization using Levenberg-Marquardt pose2SLAM::Values result = graph->optimize(*initial); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 76f51e366..e2f3801f7 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -37,18 +37,18 @@ int main(void) { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPrior(1, priorMean, priorNoise); + graph.addPosePrior(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // print graph.print("\nFactor graph:\n"); diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index c2f6f5d5b..25e993bca 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -54,7 +54,7 @@ int main(int argc, char* argv[]) { newFactors.addPosePrior(X(0), data.poses[0], data.noiseX); // Second pose with odometry measurement - newFactors.addOdometry(X(0), X(1), data.odometry, data.noiseX); + newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX); // Visual measurements at both poses for (size_t i=0; i<2; ++i) { @@ -85,7 +85,7 @@ int main(int argc, char* argv[]) { visualSLAM::Graph newFactors; // Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth. Pose3 odoMeasurement = data.odometry; - newFactors.addOdometry(X(i-1), X(i), data.odometry, data.noiseX); + newFactors.addRelativePose(X(i-1), X(i), data.odometry, data.noiseX); // Factors for visual measurements for (size_t j=0; j load2D( poses->insert(id2, poses->at(id1) * l1Xl2); pose2SLAM::Graph::sharedFactor factor( - new pose2SLAM::Odometry(id1, id2, l1Xl2, *model)); + new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); @@ -189,8 +189,8 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config, Matrix RR = trans(R) * R; //prod(trans(R),R); BOOST_FOREACH(boost::shared_ptr factor_, graph) { - boost::shared_ptr factor = - boost::dynamic_pointer_cast(factor_); + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); if (!factor) continue; diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index e385cb66f..20c149b66 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -22,49 +22,39 @@ namespace planarSLAM { /* ************************************************************************* */ - Graph::Graph(const NonlinearFactorGraph& graph) : - NonlinearFactorGraph(graph) {} - - /* ************************************************************************* */ - void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); - push_back(factor); + Matrix Values::points() const { + size_t j=0; + ConstFiltered points = filter(); + Matrix result(points.size(),2); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) + result.row(j++) = keyValue.value.vector(); + return result; } /* ************************************************************************* */ - void Graph::addPoseConstraint(Key i, const Pose2& p) { - sharedFactor factor(new Constraint(i, p)); - push_back(factor); + void Graph::addPointConstraint(Key pointKey, const Point2& p) { + push_back(boost::make_shared >(pointKey, p)); } /* ************************************************************************* */ - 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::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) { + push_back(boost::make_shared >(pointKey, p, 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); + push_back(boost::make_shared >(i, 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); + push_back(boost::make_shared >(i, j, z, model)); } /* ************************************************************************* */ void Graph::addBearingRange(Key i, Key j, const Rot2& z1, double z2, const SharedNoiseModel& model) { - sharedFactor factor(new BearingRange(i, j, z1, z2, model)); - push_back(factor); - } - - /* ************************************************************************* */ - Values Graph::optimize(const Values& initialEstimate) const { - return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); + push_back(boost::make_shared >(i, j, z1, z2, model)); } /* ************************************************************************* */ diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index aecd22b75..aeb38794e 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include #include @@ -33,75 +34,50 @@ namespace planarSLAM { using namespace gtsam; /* - * List of typedefs for factors - */ - - /// A hard constraint for poses to enforce particular values - typedef NonlinearEquality Constraint; - /// A prior factor to bias the value of a pose - typedef PriorFactor Prior; - /// A factor between two poses set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a pose and a point to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a pose and a point to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a pose and a point to express difference in rotation and location - typedef BearingRangeFactor BearingRange; - - /* - * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper + * Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper * @addtogroup SLAM */ - struct Values: public gtsam::Values { + struct Values: public pose2SLAM::Values { /// Default constructor Values() {} /// Copy constructor Values(const gtsam::Values& values) : - gtsam::Values(values) { + pose2SLAM::Values(values) { } - /// get a pose - Pose2 pose(Key i) const { return at(i); } - /// get a point Point2 point(Key j) const { return at(j); } - /// insert a pose - void insertPose(Key i, const Pose2& pose) { insert(i, pose); } - /// insert a point void insertPoint(Key j, const Point2& point) { insert(j, point); } - /// update a pose - void updatePose(Key i, const Pose2& pose) { update(i, pose); } - /// update a point void updatePoint(Key j, const Point2& point) { update(j, point); } + + /// get all [x,y] coordinates in a 2*n matrix + Matrix points() const; }; /** * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper * @addtogroup SLAM */ - struct Graph: public NonlinearFactorGraph { + struct Graph: public pose2SLAM::Graph { /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + Graph(const NonlinearFactorGraph& graph): + pose2SLAM::Graph(graph) {} - /// 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 on a point + void addPointConstraint(Key j, const Point2& p); - /// Creates a hard constraint on the ith pose - void addPoseConstraint(Key i, const Pose2& pose); - - /// Creates an odometry factor between poses with keys i1 and i2 - void addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model); + /// Adds a prior with mean p and given noise model on point j + void addPointPrior(Key j, const Point2& p, 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); @@ -111,16 +87,21 @@ namespace planarSLAM { /// 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; - - /// Return a Marginals object - Marginals marginals(const Values& solution) const { - return Marginals(*this,solution); - } }; } // planarSLAM +/** + * Backwards compatibility and wrap use only, avoid using + */ +namespace planarSLAM { + typedef gtsam::NonlinearEquality Constraint; ///< \deprecated typedef for backwards compatibility + typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility + typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility + typedef gtsam::BearingFactor Bearing; ///< \deprecated typedef for backwards compatibility + typedef gtsam::RangeFactor Range; ///< \deprecated typedef for backwards compatibility + typedef gtsam::BearingRangeFactor BearingRange; ///< \deprecated typedef for backwards compatibility +} + + diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index c0a63e53d..9607448c7 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -17,7 +17,6 @@ #include #include -#include // Use pose2SLAM namespace for specific SLAM instance @@ -33,67 +32,53 @@ namespace pose2SLAM { } /* ************************************************************************* */ - Vector Values::xs() const { + Matrix Values::poses() const { size_t j=0; - Vector result(size()); ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.x(); + Matrix result(poses.size(),3); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) { + const Pose2& r = keyValue.value; + result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta()); + } return result; } - /* ************************************************************************* */ - Vector Values::ys() const { - size_t j=0; - Vector result(size()); - ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.y(); - return result; - } - - /* ************************************************************************* */ - Vector Values::thetas() const { - size_t j=0; - Vector result(size()); - ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.theta (); - return result; - } - - /* ************************************************************************* */ - void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); - push_back(factor); - } - /* ************************************************************************* */ void Graph::addPoseConstraint(Key i, const Pose2& p) { - sharedFactor factor(new HardConstraint(i, p)); + sharedFactor factor(new NonlinearEquality(i, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addOdometry(Key i1, Key i2, const Pose2& z, + void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) { + sharedFactor factor(new PriorFactor(i, p, model)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(i1, i2, z, model)); + sharedFactor factor(new BetweenFactor(i1, i2, z, model)); push_back(factor); } /* ************************************************************************* */ - Values Graph::optimize(const Values& initialEstimate) const { - return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); + Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const { + LevenbergMarquardtParams params; + params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; + LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params); + return optimizer.optimize(); } - Values Graph::optimizeSPCG(const Values& initialEstimate) const { + /* ************************************************************************* */ + Values Graph::optimizeSPCG(const Values& initialEstimate, size_t verbosity) const { LevenbergMarquardtParams params; + params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; params.linearSolverType = SuccessiveLinearizationParams::CG; params.iterativeParams = boost::make_shared(); return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize(); } - /* ************************************************************************* */ } // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 062893a9c..6349af5d5 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -22,13 +22,14 @@ #include #include #include +#include #include #include // Use pose2SLAM namespace for specific SLAM instance namespace pose2SLAM { - using namespace gtsam; + using namespace gtsam; /* * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper @@ -64,22 +65,14 @@ namespace pose2SLAM { /// get a pose Pose2 pose(Key i) const { return at(i); } - Vector xs() const; ///< get all x coordinates in a matrix - Vector ys() const; ///< get all y coordinates in a matrix - Vector thetas() const; ///< get all orientations in a matrix + /// get all [x,y,theta] coordinates in a 3*m matrix + Matrix poses() const; }; /** * List of typedefs for factors */ - /// 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; - /** * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper * @addtogroup SLAM @@ -92,25 +85,23 @@ namespace pose2SLAM { Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); - - /// Adds a Pose2 prior with mean p and given noise model on pose i - void addPrior(Key i, const Pose2& p, const SharedNoiseModel& model); + Graph(const NonlinearFactorGraph& graph): + NonlinearFactorGraph(graph) {} /// Creates a hard constraint for key i with the given Pose2 p. void addPoseConstraint(Key i, const Pose2& p); - /// Creates an odometry factor between poses with keys i1 and i2 - void addOdometry(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); + /// Adds a Pose2 prior with mean p and given noise model on pose i + void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model); - /// AddConstraint adds a soft constraint between factor between keys i and j - void addConstraint(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) { - addOdometry(i1,i2,z,model); // same for now - } + /// Creates an relative pose factor between poses with keys i1 and i2 + void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model); /// Optimize - Values optimize(const Values& initialEstimate) const; - Values optimizeSPCG(const Values& initialEstimate) const; + Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; + + /// Optimize using subgraph preconditioning + Values optimizeSPCG(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; /// Return a Marginals object Marginals marginals(const Values& solution) const { @@ -121,5 +112,11 @@ namespace pose2SLAM { } // pose2SLAM - - +/** + * Backwards compatibility and wrap use only, avoid using + */ +namespace pose2SLAM { + typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility + typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility + typedef gtsam::BetweenFactor Odometry; ///< \deprecated typedef for backwards compatibility +} diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index c7f8a29c5..2a5f435fc 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -42,51 +42,38 @@ namespace pose3SLAM { } /* ************************************************************************* */ - Vector Values::xs() const { - size_t j=0; - Vector result(size()); - ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.x(); - return result; - } + Matrix Values::translations() const { + size_t j=0; + ConstFiltered poses = filter(); + Matrix result(poses.size(),3); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) + result.row(j++) = keyValue.value.translation().vector(); + return result; + } /* ************************************************************************* */ - Vector Values::ys() const { - size_t j=0; - Vector result(size()); - ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.y(); - return result; - } - - /* ************************************************************************* */ - Vector Values::zs() const { - size_t j=0; - Vector result(size()); - ConstFiltered poses = filter(); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.z(); - return result; - } - - /* ************************************************************************* */ - void Graph::addPrior(Key i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + void Graph::addPoseConstraint(Key i, const Pose3& p) { + sharedFactor factor(new NonlinearEquality(i, p)); push_back(factor); } /* ************************************************************************* */ - void Graph::addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(i1, i2, z, model)); + void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) { + sharedFactor factor(new PriorFactor(i, p, model)); push_back(factor); } /* ************************************************************************* */ - void Graph::addHardConstraint(Key i, const Pose3& p) { - sharedFactor factor(new HardConstraint(i, p)); - push_back(factor); + void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) { + push_back(boost::make_shared >(i1, i2, z, model)); + } + + /* ************************************************************************* */ + Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const { + LevenbergMarquardtParams params; + params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; + LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params); + return optimizer.optimize(); } /* ************************************************************************* */ diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 41d301e08..8e60265e8 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -19,16 +19,16 @@ #include #include +#include #include #include #include -#include #include /// Use pose3SLAM namespace for specific SLAM instance namespace pose3SLAM { - using namespace gtsam; + using namespace gtsam; /* * Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper @@ -63,17 +63,11 @@ namespace pose3SLAM { /// get a pose Pose3 pose(Key i) const { return at(i); } - Vector xs() const; ///< get all x coordinates in a matrix - Vector ys() const; ///< get all y coordinates in a matrix - Vector zs() const; ///< get all z coordinates in a matrix - }; + /// check if value with specified key exists + bool exists(Key i) const { return gtsam::Values::exists(i); } - /// A prior factor on Key with Pose3 data type. - typedef PriorFactor Prior; - /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; - /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + Matrix translations() const; ///< get all pose translations coordinates in a matrix + }; /** * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper @@ -81,33 +75,70 @@ namespace pose3SLAM { */ struct Graph: public NonlinearFactorGraph { - /// Adds a factor between keys of the same type - void addPrior(Key i, const Pose3& p, const SharedNoiseModel& model); + /** + * Add a prior on a pose + * @param key variable key of the camera pose + * @param p around which soft prior is defined + * @param model uncertainty model of this prior + */ + void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); - /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph - void addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model); + /** + * 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(Key poseKey, const Pose3& p = Pose3()); - /// Creates a hard constraint for key i with the given Pose3 p. - void addHardConstraint(Key i, const Pose3& p); + /** + * Add a relative pose measurement between two poses + * @param x1 variable key of the first camera pose + * @param x2 variable key of the second camera pose + * @param relative pose measurement from x1 to x2 (x1.between(x2)) + * @param model uncertainty model of this measurement + */ + void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model); - /// Optimize - Values optimize(const Values& initialEstimate) { - return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); - } + /** + * Optimize the graph + * @param initialEstimate initial estimate of all variables in the graph + * @param pointKey variable key of the landmark + * @param range approximate range to landmark + * @param model uncertainty model of this prior + */ + Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const; + + /** + * Setup and return a LevenbargMarquardtOptimizer + * @param initialEstimate initial estimate of all variables in the graph + * @param parameters optimizer's parameters + * @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process + */ + LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, + const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const { + return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters); + } /// Return a Marginals object Marginals marginals(const Values& solution) const { - return Marginals(*this,solution); + return Marginals(*this,solution); } + }; } // pose3SLAM /** - * Backwards compatibility + * Backwards compatibility and wrap use only, avoid using */ -namespace gtsam { - typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility - typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility - typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility +namespace pose3SLAM { + typedef gtsam::PriorFactor Prior; ///< \deprecated typedef for backwards compatibility + typedef gtsam::BetweenFactor Constraint; ///< \deprecated typedef for backwards compatibility + typedef gtsam::NonlinearEquality HardConstraint; ///< \deprecated typedef for backwards compatibility } +namespace gtsam { + typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility + typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility + typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility +} + diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index b73118352..d99313767 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -96,8 +96,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); boost::shared_ptr graph(new pose3SLAM::Graph()); - graph->addPrior(1, pose1, sigma); - graph->addConstraint(1, 2, pose1.between(pose2), sigma); + graph->addPosePrior(1, pose1, sigma); + graph->addRelativePose(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth boost::shared_ptr values(new Values()); @@ -115,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(1, 2, z, sigma)); + BetweenFactor::shared_ptr f1(new BetweenFactor(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 81a15290c..cbe33e1da 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -38,7 +38,7 @@ SharedNoiseModel /* ************************************************************************* */ TEST( planarSLAM, PriorFactor_equals ) { - planarSLAM::Prior factor1(i2, x1, I3), factor2(i2, x2, I3); + PriorFactor 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)); @@ -49,7 +49,7 @@ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - planarSLAM::Bearing factor(i2, j3, z, sigma); + BearingFactor factor(i2, j3, z, sigma); // create config planarSLAM::Values c; @@ -64,7 +64,7 @@ TEST( planarSLAM, BearingFactor ) /* ************************************************************************* */ TEST( planarSLAM, BearingFactor_equals ) { - planarSLAM::Bearing + BearingFactor factor1(i2, j3, Rot2::fromAngle(0.1), sigma), factor2(i2, j3, Rot2::fromAngle(2.3), sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); @@ -77,7 +77,7 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - planarSLAM::Range factor(i2, j3, z, sigma); + RangeFactor factor(i2, j3, z, sigma); // create config planarSLAM::Values c; @@ -92,7 +92,7 @@ TEST( planarSLAM, RangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, RangeFactor_equals ) { - planarSLAM::Range factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma); + RangeFactor 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)); @@ -104,7 +104,7 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(i2, j3, r, b, sigma2); + BearingRangeFactor factor(i2, j3, r, b, sigma2); // create config planarSLAM::Values c; @@ -119,7 +119,7 @@ TEST( planarSLAM, BearingRangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, BearingRangeFactor_equals ) { - planarSLAM::BearingRange + BearingRangeFactor 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)); @@ -137,7 +137,7 @@ TEST( planarSLAM, BearingRangeFactor_poses ) /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { - planarSLAM::Constraint factor1(i2, x2), factor2(i2, x3); + NonlinearEquality 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)); @@ -159,7 +159,7 @@ TEST( planarSLAM, constructor ) G.addPoseConstraint(i2, x2); // make it feasible :-) // Add odometry - G.addOdometry(i2, i3, Pose2(0, 0, M_PI_4), I3); + G.addRelativePose(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 diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 923a67031..766d06ded 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -32,7 +32,7 @@ using namespace boost::assign; #include using namespace std; -typedef pose2SLAM::Odometry Pose2Factor; +typedef BetweenFactor Pose2Factor; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; @@ -49,9 +49,8 @@ TEST_UNSAFE( Pose2SLAM, XYT ) pose2SLAM::Values values; values.insertPose(1,Pose2(1,2,3)); values.insertPose(2,Pose2(4,5,6)); - EXPECT(assert_equal(Vector_(2,1.0,4.0),values.xs())); - EXPECT(assert_equal(Vector_(2,2.0,5.0),values.ys())); - EXPECT(assert_equal(Vector_(2,3.0,6.0-2*M_PI),values.thetas())); + Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI); + EXPECT(assert_equal(expected,values.poses())); } /* ************************************************************************* */ @@ -106,7 +105,7 @@ TEST_UNSAFE( Pose2SLAM, constructor ) // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); pose2SLAM::Graph graph; - graph.addOdometry(1,2,measured, covariance); + graph.addRelativePose(1,2,measured, covariance); // get the size of the graph size_t actual = graph.size(); // verify @@ -121,7 +120,7 @@ TEST_UNSAFE( Pose2SLAM, linearization ) Pose2 measured(2,2,M_PI_2); Pose2Factor constraint(1, 2, measured, covariance); pose2SLAM::Graph graph; - graph.addOdometry(1,2,measured, covariance); + graph.addRelativePose(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) @@ -159,7 +158,7 @@ TEST_UNSAFE(Pose2SLAM, optimize) { // create a Pose graph with one equality constraint and one measurement pose2SLAM::Graph fg; fg.addPoseConstraint(0, Pose2(0,0,0)); - fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); + fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config Values initial; @@ -196,11 +195,11 @@ TEST_UNSAFE(Pose2SLAM, optimizeSPCG) { // create a Pose graph with one equality constraint and one measurement pose2SLAM::Graph fg; - fg.addPrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0))); - fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); + fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0))); + fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance); // [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this. - fg.addConstraint(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1))); + fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1))); // Create initial config Values initial; @@ -208,7 +207,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeSPCG) { initial.insert(1, Pose2(0,0,0)); // Optimize using SPCG - Values actual = fg.optimizeSPCG(initial); + Values actual = fg.optimizeSPCG(initial,0); // Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works! Values actual2 = GaussNewtonOptimizer(fg, initial).optimize(); @@ -233,9 +232,9 @@ TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) { pose2SLAM::Graph fg; fg.addPoseConstraint(0, p0); Pose2 delta = p0.between(p1); - fg.addOdometry(0, 1, delta, covariance); - fg.addOdometry(1, 2, delta, covariance); - fg.addOdometry(2, 0, delta, covariance); + fg.addRelativePose(0, 1, delta, covariance); + fg.addRelativePose(1, 2, delta, covariance); + fg.addRelativePose(2, 0, delta, covariance); // Create initial config pose2SLAM::Values initial; @@ -269,12 +268,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(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.addRelativePose(0,1, delta, covariance); + fg.addRelativePose(1,2, delta, covariance); + fg.addRelativePose(2,3, delta, covariance); + fg.addRelativePose(3,4, delta, covariance); + fg.addRelativePose(4,5, delta, covariance); + fg.addRelativePose(5,0, delta, covariance); // Create initial config pose2SLAM::Values initial; @@ -357,9 +356,9 @@ TEST_UNSAFE(Pose2SLAM, optimize2) { ///* ************************************************************************* */ // 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); -// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); // // PredecessorMap tree = // G.findMinimumSpanningTree(); @@ -371,9 +370,9 @@ TEST_UNSAFE(Pose2SLAM, optimize2) { ///* ************************************************************************* */ // 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); -// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3); +// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3); // // PredecessorMap tree; // tree.insert(1,2); @@ -436,7 +435,7 @@ TEST_UNSAFE( Pose2Prior, error ) x0.insert(1, p1); // Create factor - pose2SLAM::Prior factor(1, p1, sigmas); + PriorFactor factor(1, p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -463,7 +462,7 @@ TEST_UNSAFE( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static pose2SLAM::Prior priorFactor(1, priorVal, sigmas); +static PriorFactor priorFactor(1, priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 46a1c35c5..23367bd8a 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -48,16 +48,16 @@ TEST(Pose3Graph, optimizeCircle) { // create a Pose graph with one equality constraint and one measurement pose3SLAM::Graph fg; - fg.addHardConstraint(0, gT0); + fg.addPoseConstraint(0, gT0); Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 double theta = M_PI/3.0; CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); - fg.addConstraint(0,1, _0T1, covariance); - fg.addConstraint(1,2, _0T1, covariance); - fg.addConstraint(2,3, _0T1, covariance); - fg.addConstraint(3,4, _0T1, covariance); - fg.addConstraint(4,5, _0T1, covariance); - fg.addConstraint(5,0, _0T1, covariance); + fg.addRelativePose(0,1, _0T1, covariance); + fg.addRelativePose(1,2, _0T1, covariance); + fg.addRelativePose(2,3, _0T1, covariance); + fg.addRelativePose(3,4, _0T1, covariance); + fg.addRelativePose(4,5, _0T1, covariance); + fg.addRelativePose(5,0, _0T1, covariance); // Create initial config Values initial; @@ -121,7 +121,7 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - pose3SLAM::Constraint factor(1, 2, z, I6); + BetweenFactor factor(1, 2, z, I6); // Create config Values x; diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index f344f98c9..ac9a6891d 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -41,6 +41,8 @@ static shared_ptrK sK(new Cal3_S2(K)); using symbol_shorthand::X; using symbol_shorthand::L; +typedef GenericProjectionFactor MyProjectionFactor; + /* ************************************************************************* */ TEST( ProjectionFactor, nonStandard ) { @@ -53,8 +55,8 @@ TEST( ProjectionFactor, error ) // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); int i=1, j=1; - boost::shared_ptr - factor(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK)); + boost::shared_ptr + factor(new MyProjectionFactor(z, sigma, X(i), L(j), sK)); // For the following values structure, the factor predicts 320,240 Values config; @@ -102,11 +104,11 @@ TEST( ProjectionFactor, equals ) // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); int i=1, j=1; - boost::shared_ptr - factor1(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK)); + boost::shared_ptr + factor1(new MyProjectionFactor(z, sigma, X(i), L(j), sK)); - boost::shared_ptr - factor2(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK)); + boost::shared_ptr + factor2(new MyProjectionFactor(z, sigma, X(i), L(j), sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 35ef081bc..986538448 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -46,6 +46,8 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); using symbol_shorthand::X; using symbol_shorthand::L; +typedef GenericStereoFactor MyStereoFactor; + /* ************************************************************************* */ TEST( StereoFactor, singlePoint) { @@ -53,11 +55,11 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); NonlinearFactorGraph graph; - graph.add(visualSLAM::PoseConstraint(X(1),camera1)); + graph.add(NonlinearEquality(X(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.add(visualSLAM::StereoFactor(z14,sigma, X(1), L(1), K)); + graph.add(MyStereoFactor(z14,sigma, X(1), L(1), K)); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index f8aef8bb9..793051370 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -38,36 +38,6 @@ namespace visualSLAM { return points.size(); } - /* ************************************************************************* */ - Vector Values::xs() const { - size_t j=0; - ConstFiltered poses = filter(); - Vector result(poses.size()); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.x(); - return result; - } - - /* ************************************************************************* */ - Vector Values::ys() const { - size_t j=0; - ConstFiltered poses = filter(); - Vector result(poses.size()); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.y(); - return result; - } - - /* ************************************************************************* */ - Vector Values::zs() const { - size_t j=0; - ConstFiltered poses = filter(); - Vector result(poses.size()); - BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, poses) - result(j++) = keyValue.value.z(); - return result; - } - /* ************************************************************************* */ Matrix Values::points() const { size_t j=0; @@ -78,54 +48,32 @@ namespace visualSLAM { return result; } + /* ************************************************************************* */ + void Graph::addPointConstraint(Key pointKey, const Point3& p) { + push_back(make_shared >(pointKey, p)); + } + + /* ************************************************************************* */ + void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { + push_back(make_shared >(pointKey, p, model)); + } + /* ************************************************************************* */ void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrK K) { - push_back(make_shared(measured, model, poseKey, pointKey, K)); + push_back(make_shared >(measured, model, poseKey, pointKey, K)); } /* ************************************************************************* */ void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrKStereo K) { - push_back(make_shared(measured, model, poseKey, pointKey, K)); - } - - /* ************************************************************************* */ - void Graph::addPoseConstraint(Key poseKey, const Pose3& p) { - push_back(make_shared(poseKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPointConstraint(Key pointKey, const Point3& p) { - push_back(make_shared(pointKey, p)); - } - - /* ************************************************************************* */ - void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) { - push_back(make_shared(poseKey, p, model)); - } - - /* ************************************************************************* */ - void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { - push_back(make_shared(pointKey, p, model)); + push_back(make_shared >(measured, model, poseKey, pointKey, K)); } /* ************************************************************************* */ void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { - push_back(make_shared(poseKey, pointKey, range, model)); + push_back(make_shared >(poseKey, pointKey, range, model)); } /* ************************************************************************* */ - void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) { - push_back(make_shared >(x1, x2, odometry, model)); - } - - /* ************************************************************************* */ - Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const { - LevenbergMarquardtParams params; - params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity; - LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params); - return optimizer.optimize(); - } - /* ************************************************************************* */ } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 3fb213ee7..dce603a03 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -19,39 +19,20 @@ #pragma once -#include +#include #include #include #include - -#include #include #include -#include -#include -#include - #include namespace visualSLAM { using namespace gtsam; - /** - * Typedefs that make up the visualSLAM namespace. - */ - typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose - typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point - typedef PriorFactor PosePrior; ///< put a soft prior on a Pose - typedef PriorFactor PointPrior; ///< put a soft prior on a point - typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point - - /// monocular and stereo camera typedefs for general use - typedef GenericProjectionFactor ProjectionFactor; - typedef GenericStereoFactor StereoFactor; - /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper - struct Values: public gtsam::Values { + struct Values: public pose3SLAM::Values { typedef boost::shared_ptr shared_ptr; typedef gtsam::Values::ConstFiltered PoseFiltered; @@ -62,28 +43,22 @@ namespace visualSLAM { /// Copy constructor Values(const gtsam::Values& values) : - gtsam::Values(values) { + pose3SLAM::Values(values) { } /// Constructor from filtered values view of poses Values(const PoseFiltered& view) : - gtsam::Values(view) { + pose3SLAM::Values(view) { } /// Constructor from filtered values view of points Values(const PointFiltered& view) : - gtsam::Values(view) { + pose3SLAM::Values(view) { } - /// insert a pose - void insertPose(Key i, const Pose3& pose) { insert(i, pose); } - /// insert a point void insertPoint(Key j, const Point3& point) { insert(j, point); } - /// update a pose - void updatePose(Key i, const Pose3& pose) { update(i, pose); } - /// update a point void updatePoint(Key j, const Point3& point) { update(j, point); } @@ -93,9 +68,6 @@ namespace visualSLAM { /// get number of points size_t nrPoints() const; - /// get a pose - Pose3 pose(Key i) const { return at(i); } - /// get a point Point3 point(Key j) const { return at(j); } @@ -105,13 +77,6 @@ namespace visualSLAM { /// get a const view containing only points PointFiltered allPoints() const { return this->filter(); } - /// check if value with specified key exists - bool exists(Key i) const { return gtsam::Values::exists(i); } - - Vector xs() const; ///< get all pose x coordinates in a matrix - Vector ys() const; ///< get all pose y coordinates in a matrix - Vector zs() const; ///< get all pose z coordinates in a matrix - Matrix points() const; ///< get all point coordinates in a matrix }; @@ -119,7 +84,7 @@ namespace visualSLAM { /** * Non-linear factor graph for vanilla visual SLAM */ - class Graph: public NonlinearFactorGraph { + class Graph: public pose3SLAM::Graph { public: /// shared pointer to this type of graph @@ -139,6 +104,32 @@ namespace visualSLAM { return NonlinearFactorGraph::equals(p, tol); } + /** + * 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(Key pointKey, const Point3& p = Point3()); + + /** + * Add a prior on a landmark + * @param key variable key of the landmark + * @param p to which point to constrain it to + * @param model uncertainty model of this prior + */ + void addPointPrior(Key pointKey, const Point3& p = Point3(), + const SharedNoiseModel& model = noiseModel::Unit::Create(3)); + + /** + * Add a range prior to a landmark + * @param poseKey variable key of the camera pose + * @param pointKey variable key of the landmark + * @param range approximate range to landmark + * @param model uncertainty model of this prior + */ + void addRangeFactor(Key poseKey, Key pointKey, double range, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + /** * Add a projection factor measurement (monocular) * @param measured the measurement @@ -161,78 +152,6 @@ namespace visualSLAM { void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrKStereo 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(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(Key pointKey, const Point3& p = Point3()); - - /** - * Add a prior on a pose - * @param key variable key of the camera pose - * @param p around which soft prior is defined - * @param model uncertainty model of this prior - */ - void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); - - /** - * Add a prior on a landmark - * @param key variable key of the landmark - * @param p to which point to constrain it to - * @param model uncertainty model of this prior - */ - void addPointPrior(Key pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)); - - /** - * Add a range prior to a landmark - * @param poseKey variable key of the camera pose - * @param pointKey variable key of the landmark - * @param range approximate range to landmark - * @param model uncertainty model of this prior - */ - void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); - - /** - * Add an odometry between two poses - * @param x1 variable key of the first camera pose - * @param x2 variable key of the second camera pose - * @param odometry measurement from x1 to x2 (x1.between(x2)) - * @param model uncertainty model of this measurement - */ - void addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model); - - /** - * Optimize the graph - * @param initialEstimate initial estimate of all variables in the graph - * @param pointKey variable key of the landmark - * @param range approximate range to landmark - * @param model uncertainty model of this prior - */ - Values optimize(const Values& initialEstimate, size_t verbosity) const; - - /** - * Setup and return a LevenbargMarquardtOptimizer - * @param initialEstimate initial estimate of all variables in the graph - * @param parameters optimizer's parameters - * @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process - */ - LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const { - return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters); - } - - /// Return a Marginals object - Marginals marginals(const Values& solution) const { - return Marginals(*this,solution); - } - }; // Graph /** @@ -240,4 +159,18 @@ namespace visualSLAM { */ typedef gtsam::NonlinearISAM ISAM; -} // namespaces +} // visualSLAM + +/** + * Backwards compatibility and wrap use only, avoid using + */ +namespace visualSLAM { + typedef gtsam::NonlinearEquality PoseConstraint; ///< \deprecated typedef for backwards compatibility + typedef gtsam::NonlinearEquality PointConstraint; ///< \deprecated typedef for backwards compatibility + typedef gtsam::PriorFactor PosePrior; ///< \deprecated typedef for backwards compatibility + typedef gtsam::PriorFactor PointPrior; ///< \deprecated typedef for backwards compatibility + typedef gtsam::RangeFactor RangeFactor; ///< \deprecated typedef for backwards compatibility + typedef gtsam::GenericProjectionFactor ProjectionFactor; ///< \deprecated typedef for backwards compatibility + typedef gtsam::GenericStereoFactor StereoFactor; ///< \deprecated typedef for backwards compatibility +} + diff --git a/matlab/examples/VisualISAMGenerateData.m b/matlab/VisualISAMGenerateData.m similarity index 100% rename from matlab/examples/VisualISAMGenerateData.m rename to matlab/VisualISAMGenerateData.m diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m index 01fdfe79c..139d40e65 100644 --- a/matlab/VisualISAMInitialize.m +++ b/matlab/VisualISAMInitialize.m @@ -45,7 +45,7 @@ for i=1:2 end %% Add odometry between frames 1 and 2 -newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry); +newFactors.addRelativePose(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry); %% Update ISAM if options.batchInitialization % Do a full optimize for first two poses diff --git a/matlab/VisualISAMStep.m b/matlab/VisualISAMStep.m index 0f87a66f0..5033a8b39 100644 --- a/matlab/VisualISAMStep.m +++ b/matlab/VisualISAMStep.m @@ -10,7 +10,7 @@ initialEstimates = visualSLAMValues; %% Add odometry i = result.nrPoses+1; odometry = data.odometry{i-1}; -newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); +newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); %% Add visual measurement factors and initializations as necessary for k=1:length(data.Z{i}) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index a523c312d..d3e563dd4 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -21,15 +21,15 @@ graph = pose2SLAMGraph; %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = gtsamnoiseModelDiagonal_Sigmas([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.addRelativePose(1, 2, odometry, odometryNoise); +graph.addRelativePose(2, 3, odometry, odometryNoise); %% Add three "GPS" measurements % We use Pose2 Priors here with high variance on theta noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]); -graph.addPrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); -graph.addPrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); -graph.addPrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); +graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); +graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); +graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); %% print graph.print(sprintf('\nFactor graph:\n')); @@ -42,12 +42,13 @@ initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.print(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,1); result.print(sprintf('\nFinal result:\n ')); %% Plot Covariance Ellipses cla; -plot(result.xs(),result.ys(),'k*-'); hold on +X=result.poses(); +plot(X(:,1),X(:,2),'k*-'); hold on marginals = graph.marginals(result); P={}; for i=1:result.size() diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index e712e4e21..f203c858e 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -21,13 +21,13 @@ graph = pose2SLAMGraph; %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([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.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = gtsamnoiseModelDiagonal_Sigmas([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.addRelativePose(1, 2, odometry, odometryNoise); +graph.addRelativePose(2, 3, odometry, odometryNoise); %% print graph.print(sprintf('\nFactor graph:\n')); @@ -40,12 +40,13 @@ initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.print(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,1); result.print(sprintf('\nFinal result:\n ')); %% Plot Covariance Ellipses cla; -plot(result.xs(),result.ys(),'k*-'); hold on +X=result.poses(); +plot(X(:,1),X(:,2),'k*-'); hold on marginals = graph.marginals(result); P={}; for i=1:result.size() diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index 27dc97a08..499845eda 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/examples/PlanarSLAMExample.m @@ -29,13 +29,13 @@ graph = planarSLAMGraph; %% Add prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(i1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph %% Add odometry odometry = gtsamPose2(2.0, 0.0, 0.0); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addOdometry(i1, i2, odometry, odometryNoise); -graph.addOdometry(i2, i3, odometry, odometryNoise); +graph.addRelativePose(i1, i2, odometry, odometryNoise); +graph.addRelativePose(i2, i3, odometry, odometryNoise); %% Add bearing/range measurement factors degrees = pi/180; @@ -58,7 +58,7 @@ initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,1); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index 99429900f..861e56db9 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/examples/PlanarSLAMExample_sampling.m @@ -16,11 +16,11 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); graph = planarSLAMGraph; priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(i1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph odometry = gtsamPose2(2.0, 0.0, 0.0); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addOdometry(i1, i2, odometry, odometryNoise); -graph.addOdometry(i2, i3, odometry, odometryNoise); +graph.addRelativePose(i1, i2, odometry, odometryNoise); +graph.addRelativePose(i2, i3, odometry, odometryNoise); %% Except, for measurements we offer a choice j1 = symbol('l',1); j2 = symbol('l',2); diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index 29133031e..776260e3f 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/examples/Pose2SLAMExample.m @@ -25,19 +25,19 @@ graph = pose2SLAMGraph; % gaussian for prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); %% Add pose constraint model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); % print graph.print(sprintf('\nFactor graph:\n')); @@ -52,12 +52,13 @@ initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,1); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses cla; -plot(result.xs(),result.ys(),'k*-'); hold on +X=result.poses(); +plot(X(:,1),X(:,2),'k*-'); hold on plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); marginals = graph.marginals(result); diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index fbffe9911..843ae7e1c 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/examples/Pose2SLAMExample_advanced.m @@ -27,14 +27,14 @@ graph = pose2SLAMGraph; % gaussian for prior priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -graph.addPrior(1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) -graph.addOdometry(1, 2, odometry, odometryNoise); -graph.addOdometry(2, 3, odometry, odometryNoise); +graph.addRelativePose(1, 2, odometry, odometryNoise); +graph.addRelativePose(2, 3, odometry, odometryNoise); %% Add measurements % general noisemodel for measurements @@ -60,7 +60,7 @@ initialEstimate.print('initial estimate'); %result.print('final result'); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,1); result.print('final result'); %% Get the corresponding dense matrix diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index 828b6f5f6..e05a77186 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -20,12 +20,12 @@ fg = pose2SLAMGraph; fg.addPoseConstraint(0, p0); delta = p0.between(p1); covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]); -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.addRelativePose(0,1, delta, covariance); +fg.addRelativePose(1,2, delta, covariance); +fg.addRelativePose(2,3, delta, covariance); +fg.addRelativePose(3,4, delta, covariance); +fg.addRelativePose(4,5, delta, covariance); +fg.addRelativePose(5,0, delta, covariance); %% Create initial config initial = pose2SLAMValues; diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index 06a16ac55..a2645a593 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/examples/Pose2SLAMExample_graph.m @@ -18,7 +18,7 @@ initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.01; 0.01; 0.01]); -graph.addPrior(0, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph %% Plot Initial Estimate figure(1);clf diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index 3f8f8adf1..3d0ce8da7 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/matlab/examples/Pose2SLAMwSPCG.m @@ -35,7 +35,7 @@ graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); %% Add pose constraint model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); % print graph.print(sprintf('\nFactor graph:\n')); diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index 4cc41a4de..c335203eb 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -17,15 +17,15 @@ p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement fg = pose3SLAMGraph; -fg.addHardConstraint(0, p0); +fg.addPoseConstraint(0, p0); delta = p0.between(p1); covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -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.addRelativePose(0,1, delta, covariance); +fg.addRelativePose(1,2, delta, covariance); +fg.addRelativePose(2,3, delta, covariance); +fg.addRelativePose(3,4, delta, covariance); +fg.addRelativePose(4,5, delta, covariance); +fg.addRelativePose(5,0, delta, covariance); %% Create initial config initial = pose3SLAMValues; @@ -39,10 +39,11 @@ initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla -plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); +T=initial.translations(); +plot3(T(:,1),T(:,2),T(:,3),'g-*'); %% optimize -result = fg.optimize(initial); +result = fg.optimize(initial,1); %% Show Result hold on; plot3DTrajectory(result,'b-*', true, 0.3); diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m index 980a3c5a1..99adaa82b 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/examples/StereoVOExample.m @@ -67,13 +67,15 @@ axis equal view(-38,12) % initial trajectory in red (rotated so Z is up) -plot3(initialEstimate.zs(),-initialEstimate.xs(),-initialEstimate.ys(), '-*r','LineWidth',2); +T=initialEstimate.translations; +plot3(T(:,3),-T(:,1),-T(:,2), '-*r','LineWidth',2); % final trajectory in green (rotated so Z is up) -plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2); +T=result.translations; +plot3(T(:,3),-T(:,1),-T(:,2), '-*g','LineWidth',2); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); % optimized 3D points (rotated so Z is up) -points = result.points(); -plot3(points(:,3),-points(:,1),-points(:,2),'*'); +P = result.points(); +plot3(P(:,3),-P(:,1),-P(:,2),'*'); diff --git a/matlab/load3D.m b/matlab/load3D.m index 82ae098b9..6ba861a92 100644 --- a/matlab/load3D.m +++ b/matlab/load3D.m @@ -41,7 +41,7 @@ for i=1:n t = gtsamPoint3(e{4}, e{5}, e{6}); R = gtsamRot3_ypr(e{9}, e{8}, e{7}); dpose = gtsamPose3(R,t); - graph.addConstraint(i1, i2, dpose, model); + graph.addRelativePose(i1, i2, dpose, model); if successive if i2>i1 initial.insertPose(i2,initial.pose(i1).compose(dpose)); diff --git a/matlab/plot3DTrajectory.m b/matlab/plot3DTrajectory.m index 524b41d33..83d3d76e2 100644 --- a/matlab/plot3DTrajectory.m +++ b/matlab/plot3DTrajectory.m @@ -3,7 +3,8 @@ function plot3DTrajectory(values,style,frames,scale) if nargin<3,frames=false;end if nargin<4,scale=0;end -plot3(values.xs(),values.ys(),values.zs(),style); hold on +T=values.translations() +plot3(T(:,1),T(:,2),T(:,3),style); hold on if frames N=values.size; for i=0:N-1 diff --git a/matlab/tests/testLocalizationExample.m b/matlab/tests/testLocalizationExample.m index 07b47257b..9e28ea64b 100644 --- a/matlab/tests/testLocalizationExample.m +++ b/matlab/tests/testLocalizationExample.m @@ -16,8 +16,8 @@ graph = pose2SLAMGraph; %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = gtsamnoiseModelDiagonal_Sigmas([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.addRelativePose(1, 2, odometry, odometryNoise); +graph.addRelativePose(2, 3, odometry, odometryNoise); %% Add three "GPS" measurements % We use Pose2 Priors here with high variance on theta @@ -27,7 +27,7 @@ groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0)); groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0)); noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]); for i=1:3 - graph.addPrior(i, groundTruth.pose(i), noiseModel); + graph.addPosePrior(i, groundTruth.pose(i), noiseModel); end %% Initialize to noisy points @@ -37,7 +37,7 @@ initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,0); %% Plot Covariance Ellipses marginals = graph.marginals(result); diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m index e83d0ec0a..0056a43e8 100644 --- a/matlab/tests/testOdometryExample.m +++ b/matlab/tests/testOdometryExample.m @@ -16,13 +16,13 @@ graph = pose2SLAMGraph; %% Add a Gaussian prior on pose x_1 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([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.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add two odometry factors odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = gtsamnoiseModelDiagonal_Sigmas([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.addRelativePose(1, 2, odometry, odometryNoise); +graph.addRelativePose(2, 3, odometry, odometryNoise); %% Initialize to noisy points initialEstimate = pose2SLAMValues; @@ -31,7 +31,7 @@ initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,0); marginals = graph.marginals(result); marginals.marginalCovariance(i); diff --git a/matlab/tests/testPlanarSLAMExample.m b/matlab/tests/testPlanarSLAMExample.m index 6446da790..9747366c1 100644 --- a/matlab/tests/testPlanarSLAMExample.m +++ b/matlab/tests/testPlanarSLAMExample.m @@ -29,13 +29,13 @@ graph = planarSLAMGraph; %% Add prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(i1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph %% Add odometry odometry = gtsamPose2(2.0, 0.0, 0.0); odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addOdometry(i1, i2, odometry, odometryNoise); -graph.addOdometry(i2, i3, odometry, odometryNoise); +graph.addRelativePose(i1, i2, odometry, odometryNoise); +graph.addRelativePose(i2, i3, odometry, odometryNoise); %% Add bearing/range measurement factors degrees = pi/180; @@ -53,7 +53,7 @@ initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1)); initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); +result = graph.optimize(initialEstimate,0); marginals = graph.marginals(result); %% Check first pose and point equality diff --git a/matlab/tests/testPose2SLAMExample.m b/matlab/tests/testPose2SLAMExample.m index 707089fea..4fd97e916 100644 --- a/matlab/tests/testPose2SLAMExample.m +++ b/matlab/tests/testPose2SLAMExample.m @@ -25,19 +25,19 @@ graph = pose2SLAMGraph; % gaussian for prior priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(1, priorMean, priorNoise); % add directly to graph +graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); %% Add pose constraint model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); -graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); %% Initialize to noisy points initialEstimate = pose2SLAMValues; @@ -48,8 +48,8 @@ initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi )); initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate); -resultSPCG = graph.optimizeSPCG(initialEstimate); +result = graph.optimize(initialEstimate,0); +resultSPCG = graph.optimizeSPCG(initialEstimate,0); %% Plot Covariance Ellipses marginals = graph.marginals(result); diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m index 9d2cf4271..e63169d2b 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/matlab/tests/testPose3SLAMExample.m @@ -17,15 +17,15 @@ p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement fg = pose3SLAMGraph; -fg.addHardConstraint(0, p0); +fg.addPoseConstraint(0, p0); delta = p0.between(p1); covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -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.addRelativePose(0,1, delta, covariance); +fg.addRelativePose(1,2, delta, covariance); +fg.addRelativePose(2,3, delta, covariance); +fg.addRelativePose(3,4, delta, covariance); +fg.addRelativePose(4,5, delta, covariance); +fg.addRelativePose(5,0, delta, covariance); %% Create initial config initial = pose3SLAMValues; @@ -38,7 +38,7 @@ initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); %% optimize -result = fg.optimize(initial); +result = fg.optimize(initial,0); pose_1 = result.pose(1); CHECK('pose_1.equals(gtsamPose3,1e-4)',pose_1.equals(p1,1e-4)); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b5c1d4b89..80bd651c8 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -53,7 +53,7 @@ ISAM2 createSlamlikeISAM2( // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -66,7 +66,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -79,7 +79,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -99,7 +99,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -112,7 +112,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); @@ -257,7 +257,7 @@ TEST_UNSAFE(ISAM2, AddVariables) { // // Ordering ordering; ordering += (0), (0), (1); // planarSLAM::Graph graph; -// graph.addPrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); +// graph.addPosePrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); // graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); // // FastSet expected; @@ -364,7 +364,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -379,7 +379,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -392,7 +392,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -412,7 +412,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -425,7 +425,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); @@ -484,7 +484,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -499,7 +499,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -512,7 +512,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -532,7 +532,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -545,7 +545,7 @@ TEST(ISAM2, slamlike_solution_dogleg) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); @@ -604,7 +604,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -619,7 +619,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -632,7 +632,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -652,7 +652,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -665,7 +665,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); @@ -724,7 +724,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -739,7 +739,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -752,7 +752,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -772,7 +772,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -785,7 +785,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); @@ -941,7 +941,7 @@ TEST(ISAM2, removeFactors) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -956,7 +956,7 @@ TEST(ISAM2, removeFactors) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -969,7 +969,7 @@ TEST(ISAM2, removeFactors) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -989,7 +989,7 @@ TEST(ISAM2, removeFactors) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -1002,7 +1002,7 @@ TEST(ISAM2, removeFactors) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors[0]); @@ -1130,7 +1130,7 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { planarSLAM::Graph newfactors; - newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -1145,7 +1145,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -1161,7 +1161,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); @@ -1181,7 +1181,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -1194,7 +1194,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { planarSLAM::Graph newfactors; - newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 769e95d26..0f412a0b5 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -136,19 +136,19 @@ TEST(GaussianJunctionTree, slamlike) { size_t i = 0; newfactors = planarSLAM::Graph(); - newfactors.addPrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise); + newfactors.addPosePrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise); init.insert(X(0), Pose2(0.01, 0.01, 0.01)); fullgraph.push_back(newfactors); for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); - newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); - newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); @@ -159,13 +159,13 @@ TEST(GaussianJunctionTree, slamlike) { for( ; i<5; ++i) { newfactors = planarSLAM::Graph(); - newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullgraph.push_back(newfactors); } newfactors = planarSLAM::Graph(); - newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise); newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); @@ -194,8 +194,8 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)); - fg.addOdometry(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))); + fg.addPosePrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)); + fg.addRelativePose(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 34a39fdca..fefa2a05f 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -27,18 +27,18 @@ boost::tuple generateProblem() { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); - graph.addPrior(1, priorMean, priorNoise); + graph.addPosePrior(1, priorMean, priorNoise); // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); - graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estinmate to the solution pose2SLAM::Values initialEstimate; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 0a3f3a562..5f99d60c6 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -79,9 +79,9 @@ TEST( Graph, composePoses ) SharedNoiseModel cov = noiseModel::Unit::Create(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); - graph.addOdometry(2,3, p23, cov); - graph.addOdometry(4,3, p43, cov); + graph.addRelativePose(1,2, p12, cov); + graph.addRelativePose(2,3, p23, cov); + graph.addRelativePose(4,3, p43, cov); PredecessorMap tree; tree.insert(1,2); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 5da4ca955..3e68780c4 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -58,9 +58,9 @@ TEST( inference, marginals2) SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1)); SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1)); - fg.addPrior(X(0), Pose2(), poseModel); - fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel); + fg.addPosePrior(X(0), Pose2(), poseModel); + fg.addRelativePose(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel); + fg.addRelativePose(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel); fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel); fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel); fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index f063f9c7d..980ea8033 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -41,7 +41,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { Graph new_factors; - new_factors.addOdometry(i-1, i, z, model); + new_factors.addRelativePose(i-1, i, z, model); planarSLAM::Values new_init; // perform a check on changing orderings diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f7112bb03..a61b709d0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -176,7 +176,7 @@ TEST( NonlinearOptimizer, Factorization ) pose2SLAM::Graph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph.addOdometry(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.addRelativePose(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; ordering.push_back(X(1)); diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 2a4405877..86678cc12 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -119,7 +119,7 @@ public: } ///add a prior - void addPrior(Index cell, double prior){ + void addPosePrior(Index cell, double prior){ size_t numStates = 2; DiscreteKey key(cell, numStates); @@ -295,7 +295,7 @@ TEST_UNSAFE( OccupancyGrid, Test1) { Pose2 pose(0,0,0); double range = 1; - occupancyGrid.addPrior(0, 0.7); + occupancyGrid.addPosePrior(0, 0.7); EXPECT_LONGS_EQUAL(1, occupancyGrid.size()); occupancyGrid.addLaser(pose, range); diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index 05d2c9398..5bc34b386 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -48,7 +48,7 @@ int main(int argc, char *argv[]) { // Add a prior on the first pose if (soft_prior) - data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005)); + data.first->addPosePrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005)); else data.first->addPoseConstraint(0, Pose2()); diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index 4a53737e0..03f33d334 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -48,7 +48,7 @@ int main(int argc, char *argv[]) { // Add a prior on the first pose if (soft_prior) - data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005)); + data.first->addPosePrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005)); else data.first->addPoseConstraint(0, Pose2());