GaussNewtonOptimizer, started LevenbergMarquardtOptimizer

gtsam/nonlinear/GaussNewtonOptimizer.cpp

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GaussNewtonOptimizer.cpp
+ * @brief   
+ * @author  Richard Roberts
+ * @created Feb 26, 2012
+ */
+
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+
+using namespace std;
+
+namespace gtsam {
+
+NonlinearOptimizer::auto_ptr GaussNewtonOptimizer::iterate() const {
+
+  // Linearize graph
+  GaussianFactorGraph::shared_ptr linear = graph_->linearize(values_, gnParams_->ordering);
+
+  // Check whether to use QR
+  const bool useQR;
+  if(gnParams_->factorization == GaussNewtonParams::LDL)
+    useQR = false;
+  else if(gnParams_->factorization == GaussNewtonParams::QR)
+    useQR = true;
+  else
+    throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::factorization");
+
+  // Optimize
+  VectorValues::shared_ptr delta;
+  if(gnParams_->elimination == MULTIFRONTAL)
+    delta = GaussianMultifrontalSolver(*linear, useQR).optimize();
+  else if(gnParams_->elimination == SEQUENTIAL)
+    delta = GaussianSequentialSolver(*linear, useQR).optimize();
+  else
+    throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination");
+
+  // Maybe show output
+  if(params_->verbosity >= NonlinearOptimizerParams::DELTA) delta->print("delta");
+
+  // Update values
+  SharedValues newValues(new Values(values_->retract(*delta, gnParams_->ordering)));
+  double newError = graph_->error(newValues);
+
+  // Maybe show output
+  if (params_->verbosity >= NonlinearOptimizerParams::VALUES) newValues->print("newValues");
+  if (params_->verbosity >= NonlinearOptimizerParams::ERROR) cout << "error: " << newError << endl;
+
+  // Create new optimizer with new values and new error
+  auto_ptr<GaussNewtonOptimizer> newOptimizer(new GaussNewtonOptimizer(
+      graph_, newValues, gnParams_, newError, iterations_+1));
+
+  return newOptimizer;
+}
+
+} /* namespace gtsam */

gtsam/nonlinear/GaussNewtonOptimizer.h

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    GaussNewtonOptimizer.h
+ * @brief   
+ * @author  Richard Roberts
+ * @created Feb 26, 2012
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+
+namespace gtsam {
+
+/** Parameters for Gauss-Newton optimization, inherits from
+ * NonlinearOptimizationParams.
+ */
+class GaussNewtonParams : public NonlinearOptimizerParams {
+public:
+  /** See GaussNewtonParams::elimination */
+  enum Elimination {
+    MULTIFRONTAL,
+    SEQUENTIAL
+  };
+
+  /** See GaussNewtonParams::factorization */
+  enum Factorization {
+    LDL,
+    QR,
+  };
+
+  Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
+  Factorization factorization; ///< The numerical factorization (default: LDL)
+  Ordering ordering; ///< The variable elimination ordering (default: empty -> COLAMD)
+
+  GaussNewtonParams() :
+    elimination(MULTIFRONTAL), factorization(LDL) {}
+
+  virtual void print(const std::string& str = "") const {
+    NonlinearOptimizerParams::print(str);
+    if(elimination == MULTIFRONTAL)
+      std::cout << "         elimination method: MULTIFRONTAL\n";
+    else if(elimination == SEQUENTIAL)
+      std::cout << "         elimination method: SEQUENTIAL\n";
+    else
+      std::cout << "         elimination method: (invalid)\n";
+
+    if(factorization == LDL)
+      std::cout << "       factorization method: LDL\n";
+    else if(factorization == QR)
+      std::cout << "       factorization method: QR\n";
+    else if(factorization == CHOLESKY)
+      std::cout << "       factorization method: CHOLESKY\n";
+    else
+      std::cout << "       factorization method: (invalid)\n";
+
+    std::cout.flush();
+  }
+};
+
+/**
+ * This class performs Gauss-Newton nonlinear optimization
+ */
+class GaussNewtonOptimizer : public NonlinearOptimizer {
+
+public:
+
+  typedef boost::shared_ptr<GaussNewtonParams> SharedGNParams;
+
+  /// @name Standard interface
+  /// @{
+
+  /** Standard constructor, requires a nonlinear factor graph, initial
+   * variable assignments, and optimization parameters.  For convenience this
+   * version takes plain objects instead of shared pointers, but internally
+   * copies the objects.
+   * @param graph The nonlinear factor graph to optimize
+   * @param values The initial variable assignments
+   * @param params The optimization parameters
+   */
+  GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& values,
+      const GaussNewtonParams& params) :
+        NonlinearOptimizer(
+            SharedGraph(new NonlinearFactorGraph(graph)),
+            SharedValues(new Values(values)),
+            SharedGNParams(new GaussNewtonParams(params))),
+        gnParams_(boost::static_pointer_cast<GaussNewtonParams>(params_)) {}
+
+  /** Standard constructor, requires a nonlinear factor graph, initial
+   * variable assignments, and optimization parameters.
+   * @param graph The nonlinear factor graph to optimize
+   * @param values The initial variable assignments
+   * @param params The optimization parameters
+   */
+  GaussNewtonOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedGNParams& params) :
+        NonlinearOptimizer(graph, values, params), gnParams_(params) {}
+
+  /// @}
+
+  /// @name Advanced interface
+  /// @{
+
+  /** Virtual destructor */
+  virtual ~GaussNewtonOptimizer() {}
+
+  /** Perform a single iteration, returning a new NonlinearOptimizer class
+   * containing the updated variable assignments, which may be retrieved with
+   * values().
+   */
+  virtual auto_ptr iterate() const;
+
+  /** Update the graph, values, and/or parameters, leaving all other state
+   * the same.  Any of these that are empty shared pointers are left unchanged
+   * in the returned optimizer object.  Returns a new updated
+   * NonlinearOptimzier object, the original is not modified.
+   */
+  virtual auto_ptr update(
+      const SharedGraph& newGraph = SharedGraph(),
+      const SharedValues& newValues = SharedValues(),
+      const SharedParams& newParams = SharedParams()) const {
+    return new GaussNewtonOptimizer(*this, newGraph, newValues,
+        boost::dynamic_pointer_cast<GaussNewtonParams>(newParams));
+  }
+
+  /** Create a copy of the NonlinearOptimizer */
+  virtual auto_ptr clone() const {
+    return new GaussNewtonOptimizer(*this);
+  }
+
+  /// @}
+
+protected:
+
+  const SharedGNParams gnParams_;
+
+  GaussNewtonOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedGNParams& params, double error, int iterations) :
+    NonlinearOptimizer(graph, values, params, error, iterations), gnParams_(params) {}
+
+  GaussNewtonOptimizer(const GaussNewtonOptimizer& original, const SharedGraph& newGraph,
+      const SharedValues& newValues, const SharedGNParams& newParams) :
+    NonlinearOptimizer(original, newGraph, newValues, newParams),
+    gnParams_(newParams ? newParams : original.gnParams_) {}
+
+};
+
+}

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -0,0 +1,68 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    LevenbergMarquardtOptimizer.cpp
+ * @brief   
+ * @author  Richard Roberts
+ * @created Feb 26, 2012
+ */
+
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+
+using namespace std;
+
+namespace gtsam {
+
+NonlinearOptimizer::auto_ptr LevenbergMarquardtOptimizer::iterate() const {
+
+  // Linearize graph
+  GaussianFactorGraph::shared_ptr linear = graph_->linearize(values_, gnParams_->ordering);
+
+  // Check whether to use QR
+  const bool useQR;
+  if(gnParams_->factorization == LevenbergMarquardtParams::LDL)
+    useQR = false;
+  else if(gnParams_->factorization == LevenbergMarquardtParams::QR)
+    useQR = true;
+  else
+    throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::factorization");
+
+  // Optimize
+  VectorValues::shared_ptr delta;
+  if(gnParams_->elimination == MULTIFRONTAL)
+    delta = GaussianMultifrontalSolver(*linear, useQR).optimize();
+  else if(gnParams_->elimination == SEQUENTIAL)
+    delta = GaussianSequentialSolver(*linear, useQR).optimize();
+  else
+    throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination");
+
+  // Maybe show output
+  if(params_->verbosity >= NonlinearOptimizerParams::DELTA) delta->print("delta");
+
+  // Update values
+  SharedValues newValues(new Values(values_->retract(*delta, gnParams_->ordering)));
+  double newError = graph_->error(newValues);
+
+  // Maybe show output
+  if (params_->verbosity >= NonlinearOptimizerParams::VALUES) newValues->print("newValues");
+  if (params_->verbosity >= NonlinearOptimizerParams::ERROR) cout << "error: " << newError << endl;
+
+  // Create new optimizer with new values and new error
+  auto_ptr<LevenbergMarquardtOptimizer> newOptimizer(new LevenbergMarquardtOptimizer(
+      graph_, newValues, gnParams_, newError, iterations_+1));
+
+  return newOptimizer;
+}
+
+} /* namespace gtsam */

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -0,0 +1,163 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    LevenbergMarquardtOptimizer.h
+ * @brief   
+ * @author  Richard Roberts
+ * @created Feb 26, 2012
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+
+namespace gtsam {
+
+/** Parameters for Levenberg-Marquardt optimization, inherits from
+ * NonlinearOptimizationParams.
+ */
+class LevenbergMarquardtParams : public NonlinearOptimizerParams {
+public:
+  /** See LevenbergMarquardtParams::elimination */
+  enum Elimination {
+    MULTIFRONTAL,
+    SEQUENTIAL
+  };
+
+  /** See LevenbergMarquardtParams::factorization */
+  enum Factorization {
+    LDL,
+    QR,
+  };
+
+  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)
+
+  LevenbergMarquardtParams() :
+    elimination(MULTIFRONTAL), factorization(LDL), lambda(1e-5), lambdaFactor(10.0) {}
+
+  virtual void print(const std::string& str = "") const {
+    NonlinearOptimizerParams::print(str);
+    if(elimination == MULTIFRONTAL)
+      std::cout << "         elimination method: MULTIFRONTAL\n";
+    else if(elimination == SEQUENTIAL)
+      std::cout << "         elimination method: SEQUENTIAL\n";
+    else
+      std::cout << "         elimination method: (invalid)\n";
+
+    if(factorization == LDL)
+      std::cout << "       factorization method: LDL\n";
+    else if(factorization == QR)
+      std::cout << "       factorization method: QR\n";
+    else if(factorization == CHOLESKY)
+      std::cout << "       factorization method: CHOLESKY\n";
+    else
+      std::cout << "       factorization method: (invalid)\n";
+
+    std::cout << "                     lambda: " << lambda << "\n";
+    std::cout << "               lambdaFactor: " << lambdaFactor << "\n";
+
+    std::cout.flush();
+  }
+};
+
+/**
+ * This class performs Levenberg-Marquardt nonlinear optimization
+ */
+class LevenbergMarquardtOptimizer : public NonlinearOptimizer {
+
+public:
+
+  typedef boost::shared_ptr<LevenbergMarquardtParams> SharedLMParams;
+
+  /// @name Standard interface
+  /// @{
+
+  /** Standard constructor, requires a nonlinear factor graph, initial
+   * variable assignments, and optimization parameters.  For convenience this
+   * version takes plain objects instead of shared pointers, but internally
+   * copies the objects.
+   * @param graph The nonlinear factor graph to optimize
+   * @param values The initial variable assignments
+   * @param params The optimization parameters
+   */
+  LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& values,
+      const LevenbergMarquardtParams& params) :
+        NonlinearOptimizer(
+            SharedGraph(new NonlinearFactorGraph(graph)),
+            SharedValues(new Values(values)),
+            SharedLMParams(new LevenbergMarquardtParams(params))),
+        lmParams_(boost::static_pointer_cast<LevenbergMarquardtParams>(params_)) {}
+
+  /** Standard constructor, requires a nonlinear factor graph, initial
+   * variable assignments, and optimization parameters.
+   * @param graph The nonlinear factor graph to optimize
+   * @param values The initial variable assignments
+   * @param params The optimization parameters
+   */
+  LevenbergMarquardtOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedLMParams& params) :
+        NonlinearOptimizer(graph, values, params), lmParams_(params) {}
+
+  /// @}
+
+  /// @name Advanced interface
+  /// @{
+
+  /** Virtual destructor */
+  virtual ~LevenbergMarquardtOptimizer() {}
+
+  /** Perform a single iteration, returning a new NonlinearOptimizer class
+   * containing the updated variable assignments, which may be retrieved with
+   * values().
+   */
+  virtual auto_ptr iterate() const;
+
+  /** Update the graph, values, and/or parameters, leaving all other state
+   * the same.  Any of these that are empty shared pointers are left unchanged
+   * in the returned optimizer object.  Returns a new updated
+   * NonlinearOptimzier object, the original is not modified.
+   */
+  virtual auto_ptr update(
+      const SharedGraph& newGraph = SharedGraph(),
+      const SharedValues& newValues = SharedValues(),
+      const SharedParams& newParams = SharedParams()) const {
+    return new LevenbergMarquardtOptimizer(*this, newGraph, newValues,
+        boost::dynamic_pointer_cast<LevenbergMarquardtParams>(newParams));
+  }
+
+  /** Create a copy of the NonlinearOptimizer */
+  virtual auto_ptr clone() const {
+    return new LevenbergMarquardtOptimizer(*this);
+  }
+
+  /// @}
+
+protected:
+
+  const SharedLMParams lmParams_;
+
+  LevenbergMarquardtOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedLMParams& params, double error, int iterations) :
+    NonlinearOptimizer(graph, values, params, error, iterations), lmParams_(params) {}
+
+  LevenbergMarquardtOptimizer(const LevenbergMarquardtOptimizer& original, const SharedGraph& newGraph,
+      const SharedValues& newValues, const SharedLMParams& newParams) :
+    NonlinearOptimizer(original, newGraph, newValues, newParams),
+    lmParams_(newParams ? newParams : original.lmParams_) {}
+
+};
+
+}

gtsam/nonlinear/NonlinearOptimizer.h

@@ -11,15 +11,14 @@
 
 /**
  * @file NonlinearOptimizer.h
- * @brief Encapsulates nonlinear optimization state
+ * @brief Base class and parameters for nonlinear optimization algorithms
- * @author Frank Dellaert
+ * @author Richard Roberts
  * @date Sep 7, 2009
  */
 
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizationParameters.h>
 
 namespace gtsam {
 
@@ -28,8 +27,7 @@ namespace gtsam {
  */
 class NonlinearOptimizerParams {
 public:
-
+  /** See NonlinearOptimizationParams::verbosity */
-  /** Control the printing verbosity */
   enum Verbosity {
     SILENT,
     ERROR,
@@ -53,14 +51,16 @@ public:
     maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(0.0),
     errorTol(0.0), verbosity(SILENT) {}
 
-  void print(const std::string& str = "") const {
+  virtual void print(const std::string& str = "") const {
-    cout << s << "\n";
+    std::cout << s << "\n";
-    cout << "relative decrease threshold: " << relativeErrorTol << "\n";
+    std::cout << "relative decrease threshold: " << relativeErrorTol << "\n";
-    cout << "absolute decrease threshold: " << absoluteErrorTol << "\n";
+    std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n";
-    cout << "      total error threshold: " << errorTol << "\n";
+    std::cout << "      total error threshold: " << errorTol << "\n";
-    cout << "         maximum iterations: " << maxIterations << "\n";
+    std::cout << "         maximum iterations: " << maxIterations << "\n";
-    cout << "            verbosity level: " << verbosity << "\n";
+    std::cout << "            verbosity level: " << verbosity << std::endl;
   }
+
+  virtual ~NonlinearOptimizationParams() {}
 };
 
 
@@ -80,7 +80,7 @@ public:
  *
  * Example:
  * \code
-NonlinearOptimizer::shared_ptr optimizer = DoglegOptimizer::Create(graph, initialValues);
+NonlinearOptimizer::auto_ptr optimizer = DoglegOptimizer::Create(graph, initialValues);
 optimizer = optimizer->optimizer();
 Values result = optimizer->values();
 useTheResult(result);
@@ -123,26 +123,6 @@ public:
   /** A const shared_ptr to the parameters */
   typedef boost::shared_ptr<const NonlinearOptimizerParams> SharedParams;
 
-protected:
-
-  const SharedGraph graph_;
-  const SharedValues values_;
-  const SharedParams params_;
-  const double error_;
-  const int iterations_;
-
-  NonlinearOptimizer(const SharedGraph& graph, const SharedValues& values,
-      const SharedParams& params) :
-    graph_(graph), values_(values), params_(params),
-    error_(graph_->error(*values_)), iterations_(0) {}
-
-  NonlinearOptimizer(const SharedGraph& graph, const SharedValues& values,
-      const SharedParams& params, double error, int iterations) :
-    graph_(graph), values_(values), params_(params),
-    error_(error), iterations_(iterations) {}
-
-public:
-
   /// @name Standard interface
   /// @{
 
@@ -169,29 +149,24 @@ public:
   /// @name Advanced interface
   /// @{
 
+  /** Virtual destructor */
+  virtual ~NonlinearOptimizer() {}
+
   /** Perform a single iteration, returning a new NonlinearOptimizer class
    * containing the updated variable assignments, which may be retrieved with
    * values().
    */
   virtual auto_ptr iterate() const = 0;
 
-  /** Update the nonlinear factor graph, leaving all other state the same.
+  /** Update the graph, values, and/or parameters, leaving all other state
-   * Returns a new updated NonlinearOptimzier object, the original is not
+   * the same.  Any of these that are empty shared pointers are left unchanged
-   * modified.
+   * in the returned optimizer object.  Returns a new updated
+   * NonlinearOptimzier object, the original is not modified.
    */
-  virtual auto_ptr update(const SharedGraph& newGraph) const = 0;
+  virtual auto_ptr update(
-
+      const SharedGraph& newGraph = SharedGraph(),
-  /** Update the variable assignments, leaving all other state the same.
+      const SharedValues& newValues = SharedValues(),
-   * Returns a new updated NonlinearOptimzier object, the original is not
+      const SharedParams& newParams = SharedParams()) const = 0;
-   * modified.
-   */
-  virtual auto_ptr update(const SharedValues& newValues) const = 0;
-
-  /** Update the parameters, leaving all other state the same.
-   * Returns a new updated NonlinearOptimzier object, the original is not
-   * modified.
-   */
-  virtual auto_ptr update(const SharedParams& newParams) const = 0;
 
   /** Create a copy of the NonlinearOptimizer */
   virtual auto_ptr clone() const = 0;
@@ -205,6 +180,37 @@ protected:
    */
   auto_ptr defaultOptimize() const;
 
+protected:
+
+  const SharedGraph graph_;
+  const SharedValues values_;
+  const SharedParams params_;
+  const double error_;
+  const int iterations_;
+
+  /** Constructor for initial construction of base classes, computes error and
+   * sets iterations to zero.
+   */
+  NonlinearOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedParams& params) :
+    graph_(graph), values_(values), params_(params),
+    error_(graph_->error(*values_)), iterations_(0) {}
+
+  /** Constructor that specifies all parts of the state, used for updates */
+  NonlinearOptimizer(const SharedGraph& graph, const SharedValues& values,
+      const SharedParams& params, double error, int iterations) :
+    graph_(graph), values_(values), params_(params),
+    error_(error), iterations_(iterations) {}
+
+  /** Convenience constructor for modifying only some of the state. */
+  NonlinearOptimizer(const NonlinearOptimizer& original, const SharedGraph& newGraph,
+      const SharedValues& newValues, const SharedParams& newParams) :
+    graph_(newGraph ? newGraph : original.graph_),
+    values_(newValues ? newValues : original.values_),
+    params_(newParams ? newParams : original.params_),
+    error_(newGraph || newValues ? graph_->error(*values_) : original.error_),
+    iterations_(original.iterations_) {}
+
 };
 
 /** Check whether the relative error decrease is less than relativeErrorTreshold,

tests/testNonlinearOptimizer.cpp

@@ -35,7 +35,6 @@ using namespace boost;
 // template definitions
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
 
 using namespace gtsam;
 
@@ -235,6 +234,10 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, optimization_method )
 {
+  GaussNewtonParams params;
+  params.elimination = GaussNewtonParams::QR;
+  EXPECT(false);
+
 	example::Graph fg = example::createReallyNonlinearFactorGraph();
 
 	Point2 x0(3,3);