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,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

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,51 +17,48 @@
 
 #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 {
 
-	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) {
+  void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) {
-			Values x;
+    sharedFactor factor(new Prior(PoseKey(i), p, model));
-			double theta = 0, dtheta = 2 * M_PI / n;
+    push_back(factor);
-			// 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) {
+  void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
-			sharedFactor factor(new Prior(PoseKey(i), p, model));
+    sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
-			push_back(factor);
+    push_back(factor);
-		}
+  }
 
-		void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) {
+  /* ************************************************************************* */
-			sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model));
+  void Graph::addHardConstraint(Index i, const Pose3& p) {
-			push_back(factor);
+    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
-
-	} // pose3SLAM
-
-} // gtsam

gtsam/slam/pose3SLAM.h

@@ -23,52 +23,57 @@
 #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>
 
+/// 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<Pose3> Prior;
+  /// A factor to put constraints between two factors.
+  typedef BetweenFactor<Pose3> Constraint;
+  /// A hard constraint would enforce that the given key would have the input value in the results.
+  typedef NonlinearEquality<Pose3> 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<Pose3> Prior;
-		/// A factor to put constraints between two factors.
-		typedef BetweenFactor<Pose3> Constraint;
-		/// A hard constraint would enforce that the given key would have the input value in the results.
-		typedef NonlinearEquality<Pose3> 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
-

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