Moved ISAMLoop to gtsam/nonlinear and renamed it to NonlinearISAM so it can be used elsewhere

examples/vSLAMexample/Makefile.am

@@ -17,7 +17,6 @@ vSFMexample_SOURCES = vSFMexample.cpp Feature2D.cpp vSLAMutils.cpp
 noinst_PROGRAMS += vISAMexample
 vISAMexample_SOURCES = vISAMexample.cpp Feature2D.cpp vSLAMutils.cpp
 
-headers += ISAMLoop.h ISAMLoop-inl.h
 headers += Feature2D.h vSLAMutils.h
 
 noinst_HEADERS = $(headers)

examples/vSLAMexample/vISAMexample.cpp

@@ -26,11 +26,9 @@ using namespace boost;
 #include <gtsam/inference/graph-inl.h>
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/PriorFactor.h>
-
 #include <gtsam/inference/ISAM-inl.h>
 #include <gtsam/linear/GaussianISAM.h>
-#include "ISAMLoop.h"
-#include "ISAMLoop-inl.h"
+#include <gtsam/nonlinear/NonlinearISAM-inl.h>
 
 #include "vSLAMutils.h"
 #include "Feature2D.h"
@@ -124,9 +122,9 @@ int main(int argc, char* argv[]) {
   g_dataFolder = string(argv[1]) + "/";
   readAllDataISAM();
 
-  // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
+  // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
   int relinearizeInterval = 3;
-  ISAMLoop<Values> isam(relinearizeInterval);
+  NonlinearISAM<Values> isam(relinearizeInterval);
 
   // At each frame i with new camera pose and new set of measurements associated with it,
   // create a graph of new factors and update ISAM

gtsam/nonlinear/Makefile.am

@@ -27,6 +27,9 @@ headers += NonlinearFactor.h
 sources += NonlinearOptimizer.cpp Ordering.cpp
 check_PROGRAMS += tests/testKey
 
+# Nonlinear iSAM
+headers += NonlinearISAM.h NonlinearISAM-inl.h
+
 # Nonlinear constraints 
 headers += NonlinearEquality.h
 

gtsam/nonlinear/NonlinearISAM-inl.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- * ISAMLoop.cpp
+ * NonlinearISAM-inl.h
  *
  *  Created on: Jan 19, 2010
  *      Author: Viorela Ila and Richard Roberts
@@ -21,15 +21,14 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/inference/ISAM-inl.h>
 #include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/NonlinearISAM.h>
 #include <boost/foreach.hpp>
 
-#include "ISAMLoop.h"
-
 using namespace gtsam;
 
 /* ************************************************************************* */
 template<class Values>
-void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialValues) {
+void NonlinearISAM<Values>::update(const Factors& newFactors, const Values& initialValues) {
 
   if(newFactors.size() > 0) {
 
@@ -55,20 +54,20 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
     boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
 
     // Update ISAM
-    isam.update(*linearizedNewFactors);
+    isam_.update(*linearizedNewFactors);
   }
 }
 
 /* ************************************************************************* */
 template<class Values>
-void ISAMLoop<Values>::reorder_relinearize() {
+void NonlinearISAM<Values>::reorder_relinearize() {
 
-  cout << "Reordering, relinearizing..." << endl;
+//  cout << "Reordering, relinearizing..." << endl;
 
   // Obtain the new linearization point
   const Values newLinPoint = estimate();
 
-  isam.clear();
+  isam_.clear();
 
   // Compute an ordering
   ordering_ = *factors_.orderingCOLAMD(newLinPoint);
@@ -77,7 +76,7 @@ void ISAMLoop<Values>::reorder_relinearize() {
   boost::shared_ptr<GaussianFactorGraph> gfg(factors_.linearize(newLinPoint, ordering_));
 
   // Just recreate the whole BayesTree
-  isam.update(*gfg);
+  isam_.update(*gfg);
 
   // Update linearization point
   linPoint_ = newLinPoint;
@@ -85,9 +84,9 @@ void ISAMLoop<Values>::reorder_relinearize() {
 
 /* ************************************************************************* */
 template<class Values>
-Values ISAMLoop<Values>::estimate() {
-  if(isam.size() > 0)
-    return linPoint_.expmap(optimize(isam), ordering_);
+Values NonlinearISAM<Values>::estimate() {
+  if(isam_.size() > 0)
+    return linPoint_.expmap(optimize(isam_), ordering_);
   else
     return linPoint_;
 }

gtsam/nonlinear/NonlinearISAM.h

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /*
- * ISAMLoop.h
+ * NonlinearISAM.h
  *
  *  Created on: Jan 19, 2010
  *      Author: Viorela Ila and Richard Roberts
@@ -23,18 +23,19 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/linear/GaussianISAM.h>
 
-
+/**
+ * Wrapper class to manage ISAM in a nonlinear context
+ */
 template<class Values>
-class ISAMLoop {
+class NonlinearISAM {
 public:
 
   typedef gtsam::NonlinearFactorGraph<Values> Factors;
 
-public:
-//protected:
+protected:
 
   /** The internal iSAM object */
-  gtsam::GaussianISAM isam;
+  gtsam::GaussianISAM isam_;
 
   /** The current linearization point */
   Values linPoint_;
@@ -49,12 +50,13 @@ public:
   int reorderInterval_;
   int reorderCounter_;
 
-
 public:
-  ISAMLoop() : reorderInterval_(0), reorderCounter_(0) {}
+
+  /** default constructor will disable periodic reordering */
+  NonlinearISAM() : reorderInterval_(0), reorderCounter_(0) {}
 
   /** Periodically reorder and relinearize */
-  ISAMLoop(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
+  NonlinearISAM(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
 
   /** Add new factors along with their initial linearization points */
   void update(const Factors& newFactors, const Values& initialValues);
@@ -62,17 +64,21 @@ public:
   /** Return the current solution estimate */
   Values estimate();
 
+  /** Relinearization and reordering of variables */
+  void reorder_relinearize();
+
+  // access
+
   /** Return the current linearization point */
   const Values& getLinearizationPoint() { return linPoint_; }
 
   /** Get the ordering */
   const gtsam::Ordering& getOrdering() const { return ordering_; }
 
+  /** get underlying nonlinear graph */
   const Factors& getFactorsUnsafe() { return factors_; }
 
-  /**
-   * Relinearization and reordering of variables
-   */
-  void reorder_relinearize();
-
+  /** get counters */
+  int reorderInterval() const { return reorderInterval_; }
+  int reorderCounter() const { return reorderCounter_; }
 };