From 67564d043bdf189a16f5dfd4579c9ee2d3635c08 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 28 Feb 2012 01:17:42 +0000
Subject: [PATCH] LevenbergMarquardtOptimizer in progress

---
 .../nonlinear/LevenbergMarquardtOptimizer.cpp | 89 +++++++++++++++++++
 gtsam/nonlinear/LevenbergMarquardtOptimizer.h |  6 ++
 2 files changed, 95 insertions(+)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 270778afe..c599b1830 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -47,6 +47,95 @@ NonlinearOptimizer::auto_ptr LevenbergMarquardtOptimizer::iterate() const {
   else
     throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
 
+
+  const NonlinearOptimizerParams::Verbosity verbosity = params_->verbosity;
+  const double lambdaFactor = parameters_->lambdaFactor_ ;
+  double lambda = params_->lambda;
+
+  double next_error = error_;
+  SharedValues next_values = values_;
+
+  // Keep increasing lambda until we make make progress
+  while(true) {
+    if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
+
+    // add prior-factors
+    // TODO: replace this dampening with a backsubstitution approach
+    typename L::shared_ptr dampedSystem(new L(linearSystem));
+    {
+      double sigma = 1.0 / sqrt(lambda);
+      dampedSystem->reserve(dampedSystem->size() + dimensions_->size());
+      // for each of the variables, add a prior
+      for(Index j=0; j<dimensions_->size(); ++j) {
+        size_t dim = (*dimensions_)[j];
+        Matrix A = eye(dim);
+        Vector b = zero(dim);
+        SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
+        typename L::sharedFactor prior(new JacobianFactor(j, A, b, model));
+        dampedSystem->push_back(prior);
+      }
+    }
+    if (verbosity >= Parameters::DAMPED) dampedSystem->print("damped");
+
+    // Create a new solver using the damped linear system
+    // FIXME: remove spcg specific code
+    if (spcg_solver_) spcg_solver_->replaceFactors(dampedSystem);
+    shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(
+        new S(dampedSystem, structure_, parameters_->useQR_));
+
+    // Try solving
+    try {
+      VectorValues delta = *solver->optimize();
+      if (verbosity >= Parameters::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
+      if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
+
+      // update values
+      shared_values newValues(new Values(values_->retract(delta, *ordering_)));
+
+      // create new optimization state with more adventurous lambda
+      double error = graph_->error(*newValues);
+
+      if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << error << endl;
+
+      if( error <= error_ ) {
+        next_values = newValues;
+        next_error = error;
+        lambda /= lambdaFactor;
+        break;
+      }
+      else {
+        // Either we're not cautious, or the same lambda was worse than the current error.
+        // The more adventurous lambda was worse too, so make lambda more conservative
+        // and keep the same values.
+        if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) {
+          if(verbosity >= Parameters::ERROR)
+            cout << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
+          break;
+        } else {
+          lambda *= factor;
+        }
+      }
+    } catch(const NegativeMatrixException& e) {
+      if(verbosity >= Parameters::LAMBDA)
+        cout << "Negative matrix, increasing lambda" << endl;
+      // Either we're not cautious, or the same lambda was worse than the current error.
+      // The more adventurous lambda was worse too, so make lambda more conservative
+      // and keep the same values.
+      if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) {
+        if(verbosity >= Parameters::ERROR)
+          cout << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl;
+        break;
+      } else {
+        lambda *= factor;
+      }
+    } catch(...) {
+      throw;
+    }
+  } // end while
+
+  return newValuesErrorLambda_(next_values, next_error, lambda);
+
+
   // Maybe show output
   if(params_->verbosity >= NonlinearOptimizerParams::DELTA) delta->print("delta");
 
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index 83c4ebe88..c1535017d 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -39,11 +39,17 @@ public:
     QR,
   };
 
+  /** See LevenbergMarquardtParams::lmVerbosity */
+  enum LMVerbosity {
+
+  };
+
   Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
   Factorization factorization; ///< The numerical factorization (default: LDL)
   Ordering::shared_ptr ordering; ///< The variable elimination ordering (default: empty -> COLAMD)
   double lambda; ///< The initial (and current after each iteration) Levenberg-Marquardt damping term (default: 1e-5)
   double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0)
+  double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5)
 
   LevenbergMarquardtParams() :
     elimination(MULTIFRONTAL), factorization(LDL), lambda(1e-5), lambdaFactor(10.0) {}