From c558a67786158374b75dfe133d79ec5034308093 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Mon, 4 Jun 2012 00:36:21 +0000
Subject: [PATCH] fleshed out visualSLAM namespace for Matlab wrapping

---
 gtsam/slam/tests/testSerializationSLAM.cpp |  2 +-
 gtsam/slam/visualSLAM.cpp                  |  7 +++++
 gtsam/slam/visualSLAM.h                    | 32 ++++++++++++++++++++++
 3 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp
index 51d37db74..275abe1b8 100644
--- a/gtsam/slam/tests/testSerializationSLAM.cpp
+++ b/gtsam/slam/tests/testSerializationSLAM.cpp
@@ -181,7 +181,7 @@ Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
 /* ************************************************************************* */
 TEST (Serialization, visual_system) {
   using namespace visualSLAM;
-  Values values;
+  visualSLAM::Values values;
   Symbol x1('x',1), x2('x',2);
   Symbol l1('l',1), l2('l',2);
   Pose3 pose1 = pose3, pose2 = pose3.inverse();
diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index cc37e8bff..346455063 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/gtsam/slam/visualSLAM.cpp
@@ -27,6 +27,13 @@ namespace visualSLAM {
     push_back(factor);
   }
 
+  /* ************************************************************************* */
+  void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
+       Key poseKey, Key pointKey, const shared_ptrKStereo& K) {
+    boost::shared_ptr<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K));
+    push_back(factor);
+  }
+
   /* ************************************************************************* */
   void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
     boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(poseKey, p));
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index 32824b60e..f13755836 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -46,6 +46,27 @@ namespace visualSLAM {
   typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
   typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
 
+  /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
+  struct Values: public gtsam::Values {
+
+    typedef boost::shared_ptr<Values> shared_ptr;
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) :
+        gtsam::Values(values) {
+    }
+
+    /// insert a pose
+    void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
+
+    /// get a pose
+    Pose3 pose(Key i) const { return at<Pose3>(i); }
+
+  };
+
   /**
    * Non-linear factor graph for vanilla visual SLAM
    */
@@ -80,6 +101,17 @@ namespace visualSLAM {
     void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
         Key poseKey, Key pointKey, const shared_ptrK& K);
 
+    /**
+     *  Add a stereo factor measurement
+     *  @param measured the measurement
+     *  @param model the noise model for the measurement
+     *  @param poseKey variable key for the camera pose
+     *  @param pointKey variable key for the landmark
+     *  @param K shared pointer to stereo calibration object
+     */
+    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