Added Cholesky/LDL switch in NonlinearOptimizer, preparing to remove

LDL, remove dependency of NonlinearOptimizer on linear solvers.

gtsam/nonlinear/DoglegOptimizer.cpp

@@ -18,8 +18,8 @@
 
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 
-#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/inference/EliminationTree.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
 
 using namespace std;
@@ -33,14 +33,8 @@ void DoglegOptimizer::iterate(void) {
   const Ordering& ordering = *params_.ordering;
   GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering);
 
-  // Check whether to use QR
+  // Get elimination method
-  bool useQR;
+  GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
-  if(params_.factorization == DoglegParams::LDL)
-    useQR = false;
-  else if(params_.factorization == DoglegParams::QR)
-    useQR = true;
-  else
-    throw runtime_error("Optimization parameter is invalid: DoglegParams::factorization");
 
   // Pull out parameters we'll use
   const bool dlVerbose = (params_.dlVerbosity > DoglegParams::SILENT);
@@ -49,11 +43,12 @@ void DoglegOptimizer::iterate(void) {
   DoglegOptimizerImpl::IterationResult result;
 
   if(params_.elimination == DoglegParams::MULTIFRONTAL) {
-    GaussianBayesTree::shared_ptr bt = GaussianMultifrontalSolver(*linear, useQR).eliminate();
+    GaussianBayesTree bt;
-    result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bt, graph_, state_.values, ordering, state_.error, dlVerbose);
+    bt.insert(GaussianJunctionTree(*linear).eliminate(eliminationMethod));
+    result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose);
 
   } else if(params_.elimination == DoglegParams::SEQUENTIAL) {
-    GaussianBayesNet::shared_ptr bn = GaussianSequentialSolver(*linear, useQR).eliminate();
+    GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod);
     result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose);
 
   } else {

gtsam/nonlinear/GaussNewtonOptimizer.cpp

@@ -16,9 +16,9 @@
  * @created Feb 26, 2012
  */
 
+#include <gtsam/inference/EliminationTree.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/linear/GaussianMultifrontalSolver.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
 
 using namespace std;
 
@@ -32,29 +32,23 @@ void GaussNewtonOptimizer::iterate() {
   // Linearize graph
   GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values, *params_.ordering);
 
-  // Check whether to use QR
-  bool useQR;
-  if(params_.factorization == GaussNewtonParams::LDL)
-    useQR = false;
-  else if(params_.factorization == GaussNewtonParams::QR)
-    useQR = true;
-  else
-    throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::factorization");
-
   // Optimize
-  VectorValues::shared_ptr delta;
+  VectorValues delta;
-  if(params_.elimination == GaussNewtonParams::MULTIFRONTAL)
+  {
-    delta = GaussianMultifrontalSolver(*linear, useQR).optimize();
+    GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
-  else if(params_.elimination == GaussNewtonParams::SEQUENTIAL)
+    if(params_.elimination == GaussNewtonParams::MULTIFRONTAL)
-    delta = GaussianSequentialSolver(*linear, useQR).optimize();
+      delta = GaussianJunctionTree(*linear).optimize(eliminationMethod);
-  else
+    else if(params_.elimination == GaussNewtonParams::SEQUENTIAL)
-    throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
+      delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod));
+    else
+      throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
+  }
 
   // Maybe show output
-  if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta->print("delta");
+  if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta");
 
   // Create new state with new values and new error
-  state_.values = current.values.retract(*delta, *params_.ordering);
+  state_.values = current.values.retract(delta, *params_.ordering);
   state_.error = graph_.error(state_.values);
   ++ state_.iterations;
 }

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -19,8 +19,8 @@
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 #include <gtsam/base/cholesky.h> // For NegativeMatrixException
-#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/inference/EliminationTree.h>
-#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
 
 using namespace std;
 
@@ -32,14 +32,8 @@ void LevenbergMarquardtOptimizer::iterate() {
   // Linearize graph
   GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering);
 
-  // Check whether to use QR
+  // Get elimination method
-  bool useQR;
+  GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction();
-  if(params_.factorization == LevenbergMarquardtParams::LDL)
-    useQR = false;
-  else if(params_.factorization == LevenbergMarquardtParams::QR)
-    useQR = true;
-  else
-    throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::factorization");
 
   // Pull out parameters we'll use
   const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
@@ -72,19 +66,19 @@ void LevenbergMarquardtOptimizer::iterate() {
     try {
 
       // Optimize
-      VectorValues::shared_ptr delta;
+      VectorValues delta;
-      if(params_.elimination == LevenbergMarquardtParams::MULTIFRONTAL)
+      if(params_.elimination == SuccessiveLinearizationParams::MULTIFRONTAL)
-        delta = GaussianMultifrontalSolver(dampedSystem, useQR).optimize();
+        delta = GaussianJunctionTree(*linear).optimize(eliminationMethod);
-      else if(params_.elimination == LevenbergMarquardtParams::SEQUENTIAL)
+      else if(params_.elimination == SuccessiveLinearizationParams::SEQUENTIAL)
-        delta = GaussianSequentialSolver(dampedSystem, useQR).optimize();
+        delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod));
       else
         throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
 
-      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta->vector().norm() << endl;
+      if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl;
-      if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta->print("delta");
+      if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta");
 
       // update values
-      Values newValues = state_.values.retract(*delta, *params_.ordering);
+      Values newValues = state_.values.retract(delta, *params_.ordering);
 
       // create new optimization state with more adventurous lambda
       double error = graph_.error(newValues);

gtsam/nonlinear/SuccessiveLinearizationOptimizer.h

@@ -32,6 +32,7 @@ public:
 
   /** See SuccessiveLinearizationParams::factorization */
   enum Factorization {
+    CHOLESKY,
     LDL,
     QR,
   };
@@ -54,7 +55,9 @@ public:
     else
       std::cout << "         elimination method: (invalid)\n";
 
-    if(factorization == LDL)
+    if(factorization == CHOLESKY)
+      std::cout << "       factorization method: CHOLESKY\n";
+    else if(factorization == LDL)
       std::cout << "       factorization method: LDL\n";
     else if(factorization == QR)
       std::cout << "       factorization method: QR\n";
@@ -68,6 +71,17 @@ public:
 
     std::cout.flush();
   }
+
+  GaussianFactorGraph::Eliminate getEliminationFunction() const {
+    if(factorization == SuccessiveLinearizationParams::CHOLESKY)
+      return EliminatePreferCholesky;
+    else if(factorization == SuccessiveLinearizationParams::LDL)
+      return EliminatePreferLDL;
+    else if(factorization == SuccessiveLinearizationParams::QR)
+      return EliminateQR;
+    else
+      throw runtime_error("Nonlinear optimization parameter \"factorization\" is invalid");
+  }
 };
 
 } /* namespace gtsam */

tests/testNonlinearOptimizer.cpp

@@ -153,6 +153,8 @@ TEST( NonlinearOptimizer, optimization_method )
   paramsQR.factorization = LevenbergMarquardtParams::QR;
   LevenbergMarquardtParams paramsLDL;
   paramsLDL.factorization = LevenbergMarquardtParams::LDL;
+  LevenbergMarquardtParams paramsChol;
+  paramsLDL.factorization = LevenbergMarquardtParams::CHOLESKY;
 
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
@@ -165,6 +167,9 @@ TEST( NonlinearOptimizer, optimization_method )
 
 	Values actualMFLDL = LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize();
 	DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
+
+  Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize();
+  DOUBLES_EQUAL(0,fg.error(actualMFChol),tol);
 }
 
 /* ************************************************************************* */