From 5256130afde3cfa8b1bd142e1af43ae4ff0854eb Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Wed, 6 Jun 2012 09:31:18 +0000
Subject: [PATCH] support adding odometry factor to visualSLAM graph and add
 NonlinearISAM to visualSLAM namespace for uses in MATLAB

---
 gtsam/slam/visualSLAM.cpp |  6 +++++-
 gtsam/slam/visualSLAM.h   | 16 +++++++++++++++-
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index b8721b875..1331ef20f 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/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)));
+  }
 
+  /* ************************************************************************* */
 }
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index b0e034653..22e56b8a0 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/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