diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 5e38acbd2..b5f7b18ef 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -21,39 +21,48 @@ // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { + /* ************************************************************************* */ Graph::Graph(const NonlinearFactorGraph& graph) : NonlinearFactorGraph(graph) {} + /* ************************************************************************* */ void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } + /* ************************************************************************* */ void Graph::addPoseConstraint(Index i, const Pose2& p) { sharedFactor factor(new Constraint(PoseKey(i), p)); push_back(factor); } + /* ************************************************************************* */ void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model)); push_back(factor); } + /* ************************************************************************* */ void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) { sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model)); push_back(factor); } + /* ************************************************************************* */ void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) { sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model)); push_back(factor); } + /* ************************************************************************* */ void Graph::addBearingRange(Index i, Index j, const Rot2& z1, double z2, const SharedNoiseModel& model) { sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model)); push_back(factor); } + /* ************************************************************************* */ + } // planarSLAM diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 06994d8bd..07e6896e5 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -25,7 +25,6 @@ #include #include #include -#include #include // Use planarSLAM namespace for specific SLAM instance diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index a500ed22e..e8cbcd7fe 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -16,8 +16,6 @@ **/ #include -#include -#include // Use pose2SLAM namespace for specific SLAM instance @@ -39,17 +37,19 @@ namespace pose2SLAM { push_back(factor); } + /* ************************************************************************* */ void Graph::addPoseConstraint(Index i, const Pose2& p) { sharedFactor factor(new HardConstraint(PoseKey(i), p)); push_back(factor); } + /* ************************************************************************* */ void Graph::addOdometry(Index i, Index j, const Pose2& z, const SharedNoiseModel& model) { sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model)); push_back(factor); } -/* ************************************************************************* */ + /* ************************************************************************* */ } // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 42bd8758c..6dc682f9b 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -25,8 +25,6 @@ #include #include #include -#include -#include // Use pose2SLAM namespace for specific SLAM instance namespace pose2SLAM { @@ -96,7 +94,7 @@ namespace pose2SLAM { const SharedNoiseModel& model); /// Optimize - Values optimize(const Values& initialEstimate) { + Values optimize(const Values& initialEstimate) const { return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); } }; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 2b9a3db1d..0ab962f1e 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -17,51 +17,48 @@ #include #include -#include // Use pose3SLAM namespace for specific SLAM instance -namespace gtsam { +namespace pose3SLAM { - namespace pose3SLAM { + /* ************************************************************************* */ + Values circle(size_t n, double radius) { + Values x; + double theta = 0, dtheta = 2 * M_PI / n; + // We use aerospace/navlab convention, X forward, Y right, Z down + // First pose will be at (R,0,0) + // ^y ^ X + // | | + // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) + // Vehicle at p0 is looking towards y axis (X-axis points towards world y) + Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + for (size_t i = 0; i < n; i++, theta += dtheta) { + Point3 gti(radius*cos(theta), radius*sin(theta), 0); + Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! + Pose3 gTi(gR0 * _0Ri, gti); + x.insert(PoseKey(i), gTi); + } + return x; + } - /* ************************************************************************* */ - Values circle(size_t n, double radius) { - Values x; - double theta = 0, dtheta = 2 * M_PI / n; - // We use aerospace/navlab convention, X forward, Y right, Z down - // First pose will be at (R,0,0) - // ^y ^ X - // | | - // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) - // Vehicle at p0 is looking towards y axis (X-axis points towards world y) - Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) { - Point3 gti(radius*cos(theta), radius*sin(theta), 0); - Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! - Pose3 gTi(gR0 * _0Ri, gti); - x.insert(PoseKey(i), gTi); - } - return x; - } + /* ************************************************************************* */ + void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(PoseKey(i), p, model)); + push_back(factor); + } - /* ************************************************************************* */ - void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(PoseKey(i), p, model)); - push_back(factor); - } + /* ************************************************************************* */ + void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) { + sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model)); + push_back(factor); + } - void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model)); - push_back(factor); - } + /* ************************************************************************* */ + void Graph::addHardConstraint(Index i, const Pose3& p) { + sharedFactor factor(new HardConstraint(PoseKey(i), p)); + push_back(factor); + } - void Graph::addHardConstraint(Index i, const Pose3& p) { - sharedFactor factor(new HardConstraint(PoseKey(i), p)); - push_back(factor); - } + /* ************************************************************************* */ - /* ************************************************************************* */ - - } // pose3SLAM - -} // gtsam +} // pose3SLAM diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index c394d800b..6b4efbe6c 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -23,52 +23,57 @@ #include #include #include -#include +#include +/// Use pose3SLAM namespace for specific SLAM instance +namespace pose3SLAM { + + using namespace gtsam; + + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /** + * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) + * @param n number of poses + * @param R radius of circle + * @return circle of n 3D poses + */ + Values circle(size_t n, double R); + + /// 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; + + /// Graph + struct Graph: public NonlinearFactorGraph { + + /// Adds a factor between keys of the same type + void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model); + + /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph + void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model); + + /// Creates a hard constraint for key i with the given Pose3 p. + void addHardConstraint(Index i, const Pose3& p); + + /// Optimize + Values optimize(const Values& initialEstimate) { + return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); + } + + }; + +} // pose3SLAM + +/** + * Backwards compatibility + */ namespace gtsam { - - /// Use pose3SLAM namespace for specific SLAM instance - namespace pose3SLAM { - - /// Convenience function for constructing a pose key - inline Symbol PoseKey(Index j) { return Symbol('x', j); } - - /** - * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) - * @param n number of poses - * @param R radius of circle - * @return circle of n 3D poses - */ - Values circle(size_t n, double R); - - /// 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; - - /// Graph - struct Graph: public NonlinearFactorGraph { - - /// Adds a factor between keys of the same type - void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model); - - /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph - void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model); - - /// Creates a hard constraint for key i with the given Pose3 p. - void addHardConstraint(Index i, const Pose3& p); - }; - - } // pose3SLAM - - /** - * Backwards compatibility - */ 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 gtsam - +} diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 688dcc7b9..d4ab3afdb 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -53,7 +53,7 @@ TEST(Pose3Graph, optimizeCircle) { Pose3 gT0 = hexagon.at(PoseKey(0)), gT1 = hexagon.at(PoseKey(1)); // create a Pose graph with one equality constraint and one measurement - Pose3Graph fg; + pose3SLAM::Graph fg; fg.addHardConstraint(0, gT0); Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1 double theta = M_PI/3.0; @@ -128,7 +128,7 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6); + pose3SLAM::Constraint factor(PoseKey(1), PoseKey(2), z, I6); // Create config Values x; diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index f6512e843..bc2ee199b 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -18,6 +18,44 @@ #include #include -namespace gtsam { +namespace visualSLAM { + + /* ************************************************************************* */ + void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, + Index poseKey, Index pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addPoseConstraint(Index poseKey, const Pose3& p) { + boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addPointConstraint(Index pointKey, const Point3& p) { + boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) { + boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) { + boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); + push_back(factor); + } + + /* ************************************************************************* */ + void Graph::addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model) { + push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); + } + + /* ************************************************************************* */ } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index d9be45eb3..38d3312a7 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include @@ -85,30 +85,21 @@ namespace visualSLAM { * @param K shared pointer to calibration object */ void addMeasurement(const Point2& measured, const SharedNoiseModel& model, - Index poseKey, Index pointKey, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); - push_back(factor); - } + Index poseKey, Index pointKey, const shared_ptrK& K); /** * Add a constraint on a pose (for now, *must* be satisfied in any Values) * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); - push_back(factor); - } + void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()); /** * Add a constraint on a point (for now, *must* be satisfied in any Values) * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(Index pointKey, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); - push_back(factor); - } + void addPointConstraint(Index pointKey, const Point3& p = Point3()); /** * Add a prior on a pose @@ -116,10 +107,7 @@ namespace visualSLAM { * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); - push_back(factor); - } + void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)); /** * Add a prior on a landmark @@ -127,10 +115,7 @@ namespace visualSLAM { * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); - push_back(factor); - } + void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)); /** * Add a range prior to a landmark @@ -139,8 +124,17 @@ namespace visualSLAM { * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); + void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + /** + * 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) { + return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); } }; // Graph