(With Duy) add a matlab wrapper for the LM optimizer and parameters in visualSLAM, have an example in SFMExample.m

gtsam.h

@@ -777,6 +777,13 @@ class Marginals {
 	Matrix marginalInformation(size_t variable) const;
 };
 
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+class LevenbergMarquardtParams {
+  LevenbergMarquardtParams();
+  LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose);
+  void print(string s) const;
+};
+
 }///\namespace gtsam
 
 //*************************************************************************
@@ -1002,6 +1009,7 @@ class Graph {
 
   visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const;
   gtsam::Marginals marginals(const visualSLAM::Values& solution) const;
+  visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const;
 };
 
 class ISAM {
@@ -1026,4 +1034,13 @@ class ISAM {
   gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
 };
 
+class LevenbergMarquardtOptimizer {
+  double lambda() const;
+  void iterate();
+  visualSLAM::Values optimize();
+  double error() const;
+  size_t iterations() const;
+  visualSLAM::Values values() const;
+};
+
 }///\namespace visualSLAM

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -33,7 +33,7 @@ class LevenbergMarquardtParams : public SuccessiveLinearizationParams {
 public:
   /** See LevenbergMarquardtParams::lmVerbosity */
   enum VerbosityLM {
-    SILENT,
+    SILENT = 0,
     LAMBDA,
     TRYLAMBDA,
     TRYCONFIG,
@@ -49,6 +49,9 @@ public:
   LevenbergMarquardtParams() :
     lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {}
 
+  LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose) :
+    lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {}
+
   virtual ~LevenbergMarquardtParams() {}
 
   virtual void print(const std::string& str = "") const {
@@ -56,6 +59,7 @@ public:
     std::cout << "              lambdaInitial: " << lambdaInitial << "\n";
     std::cout << "               lambdaFactor: " << lambdaFactor << "\n";
     std::cout << "           lambdaUpperBound: " << lambdaUpperBound << "\n";
+    std::cout << "                verbosityLM: " << verbosityLM << "\n";
     std::cout.flush();
   }
 };

gtsam/slam/tests/testVisualSLAM.cpp

@@ -154,6 +154,38 @@ TEST( VisualSLAM, optimizeLM2)
   CHECK(assert_equal(initialEstimate, optimizer.values()));
 }
 
+/* ************************************************************************* */
+TEST( VisualSLAM, LMoptimizer)
+{
+  // build a graph
+  visualSLAM::Graph graph(testGraph());
+  // add 2 camera constraints
+  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(2), camera2);
+
+  // Create an initial values structure corresponding to the ground truth
+  Values initialEstimate;
+  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(L(1), landmark1);
+  initialEstimate.insert(L(2), landmark2);
+  initialEstimate.insert(L(3), landmark3);
+  initialEstimate.insert(L(4), landmark4);
+
+  // Create an optimizer and check its error
+  // We expect the initial to be zero because config is the ground truth
+  LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate);
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // Iterate once, and the config should not have changed because we started
+  // with the ground truth
+  optimizer.iterate();
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // check if correct
+  CHECK(assert_equal(initialEstimate, optimizer.values()));
+}
+
 
 /* ************************************************************************* */
 TEST( VisualSLAM, CHECK_ORDERING)

gtsam/slam/visualSLAM.h

@@ -29,6 +29,7 @@
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 #include <gtsam/geometry/SimpleCamera.h>
 
@@ -217,6 +218,16 @@ namespace visualSLAM {
      */
     Values optimize(const Values& initialEstimate, size_t verbosity) const;
 
+    /**
+     *  Setup and return a LevenbargMarquardtOptimizer
+     *  @param initialEstimate initial estimate of all variables in the graph
+     *  @param parameters optimizer's parameters
+     *  @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process
+     */
+    LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const {
+      return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters);
+    }
+
     /// Return a Marginals object
     Marginals marginals(const Values& solution) const {
       return Marginals(*this,solution);

matlab/examples/SFMExample.m

@@ -61,8 +61,18 @@ for j=1:size(truth.points,2)
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
-%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+%% One-shot Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initialEstimate,1);
+%result = graph.optimize(initialEstimate,1);
+%result.print(sprintf('\nFinal result:\n  '));
+
+%% Fine grain optimization, allowing user to iterate step by step
+
+parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 2);
+optimizer = graph.optimizer(initialEstimate, parameters);
+for i=1:5
+    optimizer.iterate();
+end
+result = optimizer.values();
 result.print(sprintf('\nFinal result:\n  '));
 
 %% Plot results with covariance ellipses