Added 'optimize' methods to all the namespace graphs, and general clean-up/consistency between namespaces

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
 

gtsam/slam/planarSLAM.h

@@ -25,7 +25,6 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/geometry/Pose2.h>
 
 // Use planarSLAM namespace for specific SLAM instance

gtsam/slam/pose2SLAM.cpp

@@ -16,8 +16,6 @@
  **/
 
 #include <gtsam/slam/pose2SLAM.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
 
@@ -39,11 +37,13 @@ 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));

gtsam/slam/pose2SLAM.h

@@ -25,8 +25,6 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
-#include <gtsam/linear/GaussianMultifrontalSolver.h>
 
 // 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();
     }
   };

gtsam/slam/pose3SLAM.cpp

@@ -17,11 +17,8 @@
 
 #include <gtsam/slam/pose3SLAM.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
 
 // Use pose3SLAM namespace for specific SLAM instance
-namespace gtsam {
-
 namespace pose3SLAM {
 
   /* ************************************************************************* */
@@ -50,11 +47,13 @@ namespace gtsam {
     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);
@@ -63,5 +62,3 @@ namespace gtsam {
   /* ************************************************************************* */
 
 } // pose3SLAM
-
-} // gtsam

gtsam/slam/pose3SLAM.h

@@ -23,13 +23,13 @@
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-
-namespace gtsam {
 
 /// 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); }
 
@@ -59,6 +59,12 @@ namespace gtsam {
 
     /// 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
@@ -66,9 +72,8 @@ namespace gtsam {
 /**
  * Backwards compatibility
  */
+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 gtsam
-

gtsam/slam/tests/testPose3SLAM.cpp

@@ -53,7 +53,7 @@ TEST(Pose3Graph, optimizeCircle) {
   Pose3 gT0 = hexagon.at<Pose3>(PoseKey(0)), gT1 = hexagon.at<Pose3>(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;

gtsam/slam/visualSLAM.cpp

@@ -18,6 +18,44 @@
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
-namespace gtsam {
+namespace visualSLAM {
+
+  /* ************************************************************************* */
+  void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
+      Index poseKey, Index pointKey, const shared_ptrK& K) {
+    boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
+    push_back(factor);
+  }
+
+  /* ************************************************************************* */
+  void Graph::addPoseConstraint(Index poseKey, const Pose3& p) {
+    boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
+    push_back(factor);
+  }
+
+  /* ************************************************************************* */
+  void Graph::addPointConstraint(Index pointKey, const Point3& p) {
+    boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
+    push_back(factor);
+  }
+
+  /* ************************************************************************* */
+  void Graph::addPosePrior(Index poseKey, const Pose3& p, const SharedNoiseModel& model) {
+    boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
+    push_back(factor);
+  }
+
+  /* ************************************************************************* */
+  void Graph::addPointPrior(Index pointKey, const Point3& p, const SharedNoiseModel& model) {
+    boost::shared_ptr<PointPrior> 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<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model)));
+  }
+
+  /* ************************************************************************* */
 
 }

gtsam/slam/visualSLAM.h

@@ -26,7 +26,7 @@
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
 
@@ -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) {
+        Index poseKey, Index pointKey, const shared_ptrK& K);
-      boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K));
-      push_back(factor);
-    }
 
     /**
      *  Add a constraint on a pose (for now, *must* be satisfied in any Values)
      *  @param key variable key of the camera pose
      *  @param p to which pose to constrain it to
      */
-    void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) {
+    void addPoseConstraint(Index poseKey, const Pose3& p = Pose3());
-      boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(PoseKey(poseKey), p));
-      push_back(factor);
-    }
 
     /**
      *  Add a constraint on a point (for now, *must* be satisfied in any Values)
      *  @param key variable key of the landmark
      *  @param p point around which soft prior is defined
      */
-    void addPointConstraint(Index pointKey, const Point3& p = Point3()) {
+    void addPointConstraint(Index pointKey, const Point3& p = Point3());
-      boost::shared_ptr<PointConstraint> factor(new PointConstraint(PointKey(pointKey), p));
-      push_back(factor);
-    }
 
     /**
      *  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)) {
+    void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
-      boost::shared_ptr<PosePrior> factor(new PosePrior(PoseKey(poseKey), p, model));
-      push_back(factor);
-    }
 
     /**
      *  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)) {
+    void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
-      boost::shared_ptr<PointPrior> factor(new PointPrior(PointKey(pointKey), p, model));
-      push_back(factor);
-    }
 
     /**
      *  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)) {
+    void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
-      push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, 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) {
+      return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
     }
 
   }; // Graph