finished wrapping visualSLAM namespace

examples/vSLAMexample/vISAMexample.cpp

@@ -78,7 +78,7 @@ void readAllDataISAM() {
 /**
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
-void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
+void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
     int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
 
   // Create a graph of newFactors with new measurements
@@ -99,7 +99,7 @@ void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_p
   }
 
   // Create initial values for all nodes in the newFactors
-  initialValues = shared_ptr<Values> (new Values());
+  initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
   initialValues->insert(X(pose_id), pose);
   for (size_t i = 0; i < measurements.size(); i++) {
     initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);

gtsam.h

@@ -81,6 +81,18 @@ class Point2 {
 	gtsam::Point2 retract(Vector v);
 };
 
+class StereoPoint2 {
+  StereoPoint2();
+  StereoPoint2(double uL, double uR, double v);
+  static gtsam::StereoPoint2 Expmap(Vector v);
+  static Vector Logmap(const gtsam::StereoPoint2& p);
+  void print(string s) const;
+  Vector localCoordinates(const gtsam::StereoPoint2& p);
+  gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2);
+  gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2);
+  gtsam::StereoPoint2 retract(Vector v);
+};
+
 class Point3 {
 	Point3();
 	Point3(double x, double y, double z);
@@ -207,6 +219,20 @@ class Pose3 {
 	gtsam::Rot3 rotation() const;
 };
 
+class Cal3_S2 {
+  Cal3_S2();
+  Cal3_S2(double fx, double fy, double s, double u0, double v0);
+
+  void print(string s) const;
+};
+
+class Cal3_S2Stereo {
+  Cal3_S2Stereo();
+  Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
+
+  void print(string s) const;
+};
+
 class CalibratedCamera {
 
 	CalibratedCamera();
@@ -635,3 +661,51 @@ class Graph {
 // TODO: add factors, etc.
 
 }///\namespace simulated2DOriented
+
+//*************************************************************************
+// VisualSLAM
+//*************************************************************************
+
+#include <gtsam/slam/visualSLAM.h>
+namespace visualSLAM {
+
+class Values {
+  Values();
+  void insertPose(size_t key, const gtsam::Pose3& pose);
+  void insertPoint(size_t key, const gtsam::Point3& pose);
+  size_t size() const;
+  void print(string s) const;
+  gtsam::Pose3 pose(size_t i);
+  gtsam::Point3 point(size_t j);
+};
+
+class Graph {
+  Graph();
+
+  void print(string s) const;
+
+  double error(const pose2SLAM::Values& values) const;
+  gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
+  gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
+      const gtsam::Ordering& ordering) const;
+
+  // Measurements
+  void addMeasurement(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
+      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
+  void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::SharedNoiseModel& model,
+      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
+
+  // Constraints
+  void addPoseConstraint(size_t poseKey, const gtsam::Pose3& p);
+  void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
+
+  // Priors
+  void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
+  void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model);
+  void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model);
+
+  visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate) const;
+  gtsam::Marginals marginals(const visualSLAM::Values& solution) const;
+};
+
+}///\namespace visualSLAM

gtsam/slam/visualSLAM.cpp

@@ -22,14 +22,14 @@ namespace visualSLAM {
 
   /* ************************************************************************* */
   void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
-       Key poseKey, Key pointKey, const shared_ptrK& K) {
+       Key poseKey, Key pointKey, const shared_ptrK K) {
     boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
     push_back(factor);
   }
 
   /* ************************************************************************* */
   void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
-       Key poseKey, Key pointKey, const shared_ptrKStereo& K) {
+       Key poseKey, Key pointKey, const shared_ptrKStereo K) {
     boost::shared_ptr<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K));
     push_back(factor);
   }

gtsam/slam/visualSLAM.h

@@ -26,6 +26,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
 
@@ -62,9 +63,15 @@ namespace visualSLAM {
     /// 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); }
+
     /// get a pose
     Pose3 pose(Key i) const { return at<Pose3>(i); }
 
+    /// get a point
+    Point3 point(Key j) const { return at<Point3>(j); }
+
   };
 
   /**
@@ -99,7 +106,7 @@ namespace visualSLAM {
      *  @param K shared pointer to calibration object
      */
     void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
-        Key poseKey, Key pointKey, const shared_ptrK& K);
+        Key poseKey, Key pointKey, const shared_ptrK K);
 
     /**
      *  Add a stereo factor measurement
@@ -110,7 +117,7 @@ namespace visualSLAM {
      *  @param K shared pointer to stereo calibration object
      */
     void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
-        Key poseKey, Key pointKey, const shared_ptrKStereo& K);
+        Key poseKey, Key pointKey, const shared_ptrKStereo K);
 
     /**
      *  Add a constraint on a pose (for now, *must* be satisfied in any Values)
@@ -162,6 +169,11 @@ namespace visualSLAM {
       return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
     }
 
+    /// Return a Marginals object
+    Marginals marginals(const Values& solution) const {
+      return Marginals(*this,solution);
+    }
+
   }; // Graph
 
 } // namespaces