support adding odometry factor to visualSLAM graph and add NonlinearISAM to visualSLAM namespace for uses in MATLAB

gtsam/slam/visualSLAM.cpp

@@ -16,7 +16,7 @@
  */
 
 #include <gtsam/slam/visualSLAM.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 namespace visualSLAM {
 
@@ -103,5 +103,9 @@ namespace visualSLAM {
   }
 
   /* ************************************************************************* */
-
+  void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) {
+    push_back(boost::shared_ptr<BetweenFactor<Pose3> >(new BetweenFactor<Pose3>(x1, x2, odometry, model)));
+  }
+
+  /* ************************************************************************* */
 }

gtsam/slam/visualSLAM.h

@@ -27,9 +27,9 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearISAM.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
-
 namespace visualSLAM {
 
 	using namespace gtsam;
@@ -173,6 +173,15 @@ namespace visualSLAM {
      */
     void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
 
+    /**
+     *  Add an odometry between two poses
+     *  @param x1 variable key of the first camera pose
+     *  @param x2 variable key of the second camera pose
+     *  @param odometry measurement from x1 to x2 (x1.between(x2))
+     *  @param model uncertainty model of this measurement
+     */
+    void addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model);
+
     /**
      *  Optimize the graph
      *  @param initialEstimate initial estimate of all variables in the graph
@@ -191,4 +200,9 @@ namespace visualSLAM {
 
   }; // Graph
 
+  /**
+   * Non-linear ISAM for vanilla incremental visual SLAM inference
+   */
+  typedef gtsam::NonlinearISAM<Graph> ISAM;
+
 } // namespaces