From 615cfee44c0c9561491c23236a2c0ae0e1f48a74 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 19 Jun 2012 15:00:41 +0000
Subject: [PATCH] Added verbosity flag in optimize. Really, we should have more
 sophisticated optimize.

---
 gtsam/slam/visualSLAM.cpp     |  8 ++++++++
 gtsam/slam/visualSLAM.h       | 16 ++++++++--------
 matlab/VisualISAMInitialize.m |  2 +-
 matlab/examples/SFMExample.m  | 10 ++++++----
 4 files changed, 23 insertions(+), 13 deletions(-)

diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp
index 26123dda6..f8aef8bb9 100644
--- a/gtsam/slam/visualSLAM.cpp
+++ b/gtsam/slam/visualSLAM.cpp
@@ -17,6 +17,7 @@
 
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <boost/make_shared.hpp>
 
 using boost::make_shared;
@@ -120,4 +121,11 @@ namespace visualSLAM {
   }
 
   /* ************************************************************************* */
+  Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
+    LevenbergMarquardtParams params;
+    params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
+    LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
+    return optimizer.optimize();
+  }
+  /* ************************************************************************* */
 }
diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h
index d577ee527..6e5109041 100644
--- a/gtsam/slam/visualSLAM.h
+++ b/gtsam/slam/visualSLAM.h
@@ -20,14 +20,16 @@
 #pragma once
 
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/ProjectionFactor.h>
-#include <gtsam/slam/StereoFactor.h>
 #include <gtsam/slam/RangeFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/slam/StereoFactor.h>
+#include <gtsam/slam/ProjectionFactor.h>
+
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
 #include <gtsam/geometry/SimpleCamera.h>
 
 namespace visualSLAM {
@@ -213,9 +215,7 @@ namespace visualSLAM {
      *  @param range approximate range to landmark
      *  @param model uncertainty model of this prior
      */
-    Values optimize(const Values& initialEstimate) {
-      return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
-    }
+    Values optimize(const Values& initialEstimate, size_t verbosity) const;
 
     /// Return a Marginals object
     Marginals marginals(const Values& solution) const {
diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m
index 4d0b13931..43e8162dd 100644
--- a/matlab/VisualISAMInitialize.m
+++ b/matlab/VisualISAMInitialize.m
@@ -49,7 +49,7 @@ newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseMode
 
 %% Update ISAM
 if options.batchInitialization % Do a full optimize for first two poses
-    fullyOptimized = newFactors.optimize(initialEstimates);
+    fullyOptimized = newFactors.optimize(initialEstimates,0);
     isam.update(newFactors, fullyOptimized);
 else
     isam.update(newFactors, initialEstimates);
diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m
index 3f8733fc3..ac25a4328 100644
--- a/matlab/examples/SFMExample.m
+++ b/matlab/examples/SFMExample.m
@@ -49,18 +49,20 @@ graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
-%% Initialize cameras and points to ground truth in this example
+%% Initialize cameras and points close to ground truth in this example
 initialEstimate = visualSLAMValues;
 for i=1:size(truth.cameras,2)
-    initialEstimate.insertPose(symbol('x',i), truth.cameras{i}.pose);
+    pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
+    initialEstimate.insertPose(symbol('x',i), pose_i);
 end
 for j=1:size(truth.points,2)
-    initialEstimate.insertPoint(symbol('p',j), truth.points{j});
+    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    initialEstimate.insertPoint(symbol('p',j), point_j);
 end
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-result = graph.optimize(initialEstimate);
+result = graph.optimize(initialEstimate,1);
 result.print(sprintf('\nFinal result:\n  '));
 
 %% Plot results with covariance ellipses