Changes in progress

gtsam/nonlinear/GaussNewtonOptimizer.cpp

@@ -25,49 +25,39 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-NonlinearOptimizer::SharedState GaussNewtonOptimizer::iterate(const NonlinearOptimizer::SharedState& _current) const {
+void GaussNewtonOptimizer::iterate() const {
 
-  const GaussNewtonState& current = dynamic_cast<const GaussNewtonState&>(*_current);
+  const NonlinearOptimizerState& current = state_;
 
   // Linearize graph
-  const Ordering& ordering = this->ordering(current.values);
+  GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values, *params_.ordering);
-  GaussianFactorGraph::shared_ptr linear = graph_->linearize(current.values, ordering);
 
   // Check whether to use QR
   bool useQR;
-  if(params_->factorization == GaussNewtonParams::LDL)
+  if(params_.factorization == GaussNewtonParams::LDL)
     useQR = false;
-  else if(params_->factorization == GaussNewtonParams::QR)
+  else if(params_.factorization == GaussNewtonParams::QR)
     useQR = true;
   else
     throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::factorization");
 
   // Optimize
   VectorValues::shared_ptr delta;
-  if(params_->elimination == GaussNewtonParams::MULTIFRONTAL)
+  if(params_.elimination == GaussNewtonParams::MULTIFRONTAL)
     delta = GaussianMultifrontalSolver(*linear, useQR).optimize();
-  else if(params_->elimination == GaussNewtonParams::SEQUENTIAL)
+  else if(params_.elimination == GaussNewtonParams::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");
+  if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta->print("delta");
 
   // Create new state with new values and new error
-  SharedState newState = boost::make_shared<GaussNewtonState>();
+  NonlinearOptimizerState newState;
-  newState->values = current.values.retract(*delta, ordering);
+  state_.values = current.values.retract(*delta, *params_.ordering);
-  newState->error = graph_->error(newState->values);
+  state_.error = graph_.error(state_.values);
-  newState->iterations = current.iterations + 1;
+  ++ state_.iterations;
-
-  return newState;
-}
-
-/* ************************************************************************* */
-NonlinearOptimizer::SharedState GaussNewtonOptimizer::initialState(const Values& initialValues) const {
-  SharedState initial = boost::make_shared<GaussNewtonState>();
-  defaultInitialState(initialValues, *initial);
-  return initial;
 }
 
 } /* namespace gtsam */

gtsam/nonlinear/GaussNewtonOptimizer.h

@@ -22,25 +22,13 @@
 
 namespace gtsam {
 
-/** Parameters for Gauss-Newton optimization, inherits from
- * NonlinearOptimizationParams.
- */
-class GaussNewtonParams : public SuccessiveLinearizationParams {
-};
-
-class GaussNewtonState : public SuccessiveLinearizationState {
-};
-
 /**
  * This class performs Gauss-Newton nonlinear optimization
- * TODO: use make_shared
  */
-class GaussNewtonOptimizer : public SuccessiveLinearizationOptimizer {
+class GaussNewtonOptimizer : public NonlinearOptimizer {
 
 public:
 
-  typedef boost::shared_ptr<GaussNewtonParams> SharedParams;
-
   /// @name Standard interface
   /// @{
 
@@ -52,10 +40,9 @@ public:
    * @param values The initial variable assignments
    * @param params The optimization parameters
    */
-  GaussNewtonOptimizer(const NonlinearFactorGraph& graph,
+  GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
       const GaussNewtonParams& params = GaussNewtonParams()) :
-        SuccessiveLinearizationOptimizer(SharedGraph(new NonlinearFactorGraph(graph))),
+        NonlinearOptimizer(graph), params_(ensureHasOrdering(params)), state_(graph, initialValues) {}
-        params_(new GaussNewtonParams(params)) {}
 
   /** Standard constructor, requires a nonlinear factor graph, initial
    * variable assignments, and optimization parameters.  For convenience this
@@ -65,24 +52,9 @@ public:
    * @param values The initial variable assignments
    * @param params The optimization parameters
    */
-  GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Ordering& ordering) :
+  GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
-        SuccessiveLinearizationOptimizer(SharedGraph(new NonlinearFactorGraph(graph))),
+        NonlinearOptimizer(graph), state_(graph, initialValues) {
-        params_(new GaussNewtonParams()) {
+    params_.ordering = ordering; }
-    params_->ordering = ordering; }
-
-  /** 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 GaussNewtonParams& params = GaussNewtonParams()) :
-        SuccessiveLinearizationOptimizer(graph),
-        params_(new GaussNewtonParams(params)) {}
-
-  /** Access the parameters */
-  virtual NonlinearOptimizer::SharedParams params() const { return params_; }
 
   /// @}
 
@@ -96,18 +68,40 @@ public:
    * containing the updated variable assignments, which may be retrieved with
    * values().
    */
-  virtual NonlinearOptimizer::SharedState iterate(const NonlinearOptimizer::SharedState& current) const;
+  virtual void iterate() const;
 
-  /** Create an initial state with the specified variable assignment values and
+  /** Access the parameters */
-   * all other default state.
+  const GaussNewtonParams& params() const { return params_; }
-   */
+
-  virtual NonlinearOptimizer::SharedState initialState(const Values& initialValues) const;
+  /** Access the last state */
+  const NonlinearOptimizerState& state() const { return state_; }
 
   /// @}
 
 protected:
 
-  const SharedParams params_;
+  GaussNewtonParams params_;
+  NonlinearOptimizerState state_;
+
+  /** Access the parameters (base class version) */
+  virtual const NonlinearOptimizerParams& _params() const { return params_; }
+
+  /** Access the state (base class version) */
+  virtual const NonlinearOptimizerState& _state() const { return state_; }
+
+  /** Internal function for computing a COLAMD ordering if no ordering is specified */
+  GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph, const Values& values) const {
+    if(!params.ordering)
+      params.ordering = graph.orderingCOLAMD(values);
+    return params;
+  }
+
+};
+
+/** Parameters for Gauss-Newton optimization, inherits from
+ * NonlinearOptimizationParams.
+ */
+class GaussNewtonParams : public SuccessiveLinearizationParams {
 };
 
 }

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -22,6 +22,88 @@
 
 namespace gtsam {
 
+/**
+ * This class performs Levenberg-Marquardt nonlinear optimization
+ * TODO: use make_shared
+ */
+class LevenbergMarquardtOptimizer : public NonlinearOptimizer {
+
+public:
+
+  typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
+
+  /// @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& initialValues,
+      const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) :
+        NonlinearOptimizer(graph), params_(ensureHasOrdering(params)), state_(graph, initialValues) {}
+
+  /** 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& initialValues, const Ordering& ordering) :
+        NonlinearOptimizer(graph), state_(graph, initialValues) {
+    params_.ordering = ordering; }
+
+  /// @}
+
+  /// @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 void iterate() const;
+
+  /** Access the parameters */
+  const GaussNewtonParams& params() const { return params_; }
+
+  /** Access the last state */
+  const NonlinearOptimizerState& state() const { return state_; }
+
+  /// @}
+
+protected:
+
+  typedef boost::shared_ptr<const std::vector<size_t> > SharedDimensions;
+
+  mutable SharedDimensions dimensions_; // Mutable because we compute it when needed and cache it
+
+  LevenbergMarquardtParams params_;
+  LevenbergMarquardtState state_;
+
+  /** Access the parameters (base class version) */
+  virtual const NonlinearOptimizerParams& _params() const { return params_; }
+
+  /** Access the state (base class version) */
+  virtual const NonlinearOptimizerState& _state() const { return state_; }
+
+  /** Internal function for computing a COLAMD ordering if no ordering is specified */
+  GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph, const Values& values) const {
+    if(!params.ordering)
+      params.ordering = graph.orderingCOLAMD(values);
+    return params;
+  }
+};
+
 /** Parameters for Levenberg-Marquardt optimization.  Note that this parameters
  * class inherits from NonlinearOptimizerParams, which specifies the parameters
  * common to all nonlinear optimization algorithms.  This class also contains
@@ -61,95 +143,11 @@ public:
 /**
  * State for LevenbergMarquardtOptimizer
  */
-class LevenbergMarquardtState : public SuccessiveLinearizationState {
+class LevenbergMarquardtState : public NonlinearOptimizerState {
 public:
 
   double lambda;
 
 };
 
-/**
- * This class performs Levenberg-Marquardt nonlinear optimization
- * TODO: use make_shared
- */
-class LevenbergMarquardtOptimizer : public SuccessiveLinearizationOptimizer {
-
-public:
-
-  typedef boost::shared_ptr<LevenbergMarquardtParams> SharedParams;
-  typedef boost::shared_ptr<LevenbergMarquardtState> SharedState;
-  typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
-
-  /// @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 LevenbergMarquardtParams& params = LevenbergMarquardtParams()) :
-        SuccessiveLinearizationOptimizer(SharedGraph(new NonlinearFactorGraph(graph))),
-        params_(new LevenbergMarquardtParams(params)) {}
-
-  /** 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 Ordering& ordering) :
-        SuccessiveLinearizationOptimizer(SharedGraph(new NonlinearFactorGraph(graph))),
-        params_(new LevenbergMarquardtParams()) {
-    params_->ordering = ordering; }
-
-  /** 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 LevenbergMarquardtParams& params = LevenbergMarquardtParams()) :
-        SuccessiveLinearizationOptimizer(graph),
-        params_(new LevenbergMarquardtParams(params)) {}
-
-  /** Access the parameters */
-  virtual NonlinearOptimizer::SharedParams params() const { return 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 NonlinearOptimizer::SharedState iterate(const NonlinearOptimizer::SharedState& current) const;
-
-  /** Create an initial state with the specified variable assignment values and
-   * all other default state.
-   */
-  virtual NonlinearOptimizer::SharedState initialState(const Values& initialValues) const;
-
-  /// @}
-
-protected:
-
-  typedef boost::shared_ptr<const std::vector<size_t> > SharedDimensions;
-
-  SharedParams params_;
-  mutable SharedDimensions dimensions_; // Mutable because we compute it when needed and cache it
-};
-
 }

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -25,54 +25,43 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-NonlinearOptimizer::SharedState NonlinearOptimizer::defaultOptimize(const SharedState& initial) const {
+void NonlinearOptimizer::defaultOptimize() {
 
-  const SharedParams& params = this->params();
+  const NonlinearOptimizerParams& params = this->params();
-  double currentError = initial->error;
+  double currentError = this->error();
 
   // check if we're already close enough
-  if(currentError <= params->errorTol) {
+  if(currentError <= params.errorTol) {
-    if (params->verbosity >= NonlinearOptimizerParams::ERROR)
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-      cout << "Exiting, as error = " << currentError << " < " << params->errorTol << endl;
+      cout << "Exiting, as error = " << currentError << " < " << params.errorTol << endl;
-    return initial;
+    return;
   }
 
   // Maybe show output
-  if (params->verbosity >= NonlinearOptimizerParams::VALUES) initial->values.print("Initial values");
+  if (params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("Initial values");
-  if (params->verbosity >= NonlinearOptimizerParams::ERROR) cout << "Initial error: " << initial->error << endl;
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "Initial error: " << currentError << endl;
 
   // Return if we already have too many iterations
-  if(initial->iterations >= params->maxIterations)
+  if(this->iterations() >= params.maxIterations)
-    return initial;
+    return;
 
   // Iterative loop
-  SharedState next = initial;
   do {
     // Do next iteration
-    currentError = next->error;
+    currentError = this->error();
-    next = this->iterate(next);
+    this->iterate();
 
     // Maybe show output
-    if(params->verbosity >= NonlinearOptimizerParams::VALUES) next->values.print("newValues");
+    if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues");
-    if(params->verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << next->error << endl;
+    if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << this->error() << endl;
-  } while(next->iterations < params->maxIterations &&
+  } while(this->iterations() < params.maxIterations &&
-      !checkConvergence(params->relativeErrorTol, params->absoluteErrorTol,
+      !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
-            params->errorTol, currentError, next->error, params->verbosity));
+            params.errorTol, currentError, this->error(), params.verbosity));
 
   // Printing if verbose
-  if (params->verbosity >= NonlinearOptimizerParams::ERROR &&
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR &&
-      next->iterations >= params->maxIterations)
+      this->iterations() >= params.maxIterations)
     cout << "Terminating because reached maximum iterations" << endl;
-
-  // Return optimizer from final iteration
-  return next;
-}
-
-/* ************************************************************************* */
-void NonlinearOptimizer::defaultInitialState(const Values& initialValues, NonlinearOptimizerState& initialState) const {
-  initialState.values = initialValues;
-  initialState.error = graph_->error(initialValues);
-  initialState.iterations = 0;
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/NonlinearOptimizer.h

@@ -22,67 +22,6 @@
 
 namespace gtsam {
 
-/** The common parameters for Nonlinear optimizers.  Most optimizers
- * deriving from NonlinearOptimizer also subclass the parameters.
- */
-class NonlinearOptimizerParams {
-public:
-  /** See NonlinearOptimizerParams::verbosity */
-  enum Verbosity {
-    SILENT,
-    ERROR,
-    VALUES,
-    DELTA,
-    LINEAR
-  };
-
-  size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
-  double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
-  double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
-  double errorTol; ///< The maximum total error to stop iterating (default 0.0)
-  Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
-
-  NonlinearOptimizerParams() :
-    maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(1e-5),
-    errorTol(0.0), verbosity(SILENT) {}
-
-  virtual void print(const std::string& str = "") const {
-    std::cout << str << "\n";
-    std::cout << "relative decrease threshold: " << relativeErrorTol << "\n";
-    std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n";
-    std::cout << "      total error threshold: " << errorTol << "\n";
-    std::cout << "         maximum iterations: " << maxIterations << "\n";
-    std::cout << "            verbosity level: " << verbosity << std::endl;
-  }
-
-  virtual ~NonlinearOptimizerParams() {}
-};
-
-
-/**
- * Base class for a nonlinear optimization state, including the current estimate
- * of the variable values, error, and number of iterations.  Optimizers derived
- * from NonlinearOptimizer usually also define a derived state class containing
- * additional state specific to the algorithm (for example, Dogleg state
- * contains the current trust region radius).
- */
-class NonlinearOptimizerState {
-public:
-
-  /** The current estimate of the variable values. */
-  Values values;
-
-  /** The factor graph error on the current values. */
-  double error;
-
-  /** The number of optimization iterations performed. */
-  unsigned int iterations;
-
-  /** Virtual destructor */
-  virtual ~NonlinearOptimizerState() {}
-};
-
-
 /**
  * This is the abstract interface for classes that can optimize for the
  * maximum-likelihood estimate of a NonlinearFactorGraph.
@@ -162,15 +101,6 @@ public:
   /** A shared pointer to this class */
   typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr;
 
-  /** A const shared_ptr to a NonlinearFactorGraph */
-  typedef boost::shared_ptr<const NonlinearFactorGraph> SharedGraph;
-
-  /** A const shared_ptr to the parameters */
-  typedef boost::shared_ptr<const NonlinearOptimizerParams> SharedParams;
-
-  /** A shared_ptr to an optimizer state */
-  typedef boost::shared_ptr<NonlinearOptimizerState> SharedState;
-
   /// @name Standard interface
   /// @{
 
@@ -183,21 +113,13 @@ public:
    * process, you may call iterate() and check_convergence() yourself, and if
    * needed modify the optimization state between iterations.
    */
-  virtual SharedState optimize(const SharedState& initial) const { return defaultOptimize(initial); }
+  virtual const Values& optimize() const { return defaultOptimize(); }
 
-  SharedState optimize(const Values& initialization) const { return optimize(initialState(initialization)); }
+  double error() const { return _state().error; }
 
-  /** Shortcut to optimize and return the resulting Values of the maximum-
+  unsigned int iterations() const { return _state().iterations; }
-   * likelihood estimate.  To access statistics and information such as the
-   * final error and number of iterations, use optimize() instead.
-   * @return The maximum-likelihood estimate.
-   */
-  virtual Values optimized(const SharedState& initial) const { return this->optimize(initial)->values; }
 
-  Values optimized(const Values& initialization) const { return optimized(initialState(initialization)); }
+  const Values& values() const { return _state().values; }
-
-  /** Retrieve the parameters. */
-  virtual SharedParams params() const = 0;
 
   /// @}
 
@@ -211,31 +133,93 @@ public:
    * containing the updated variable assignments, which may be retrieved with
    * values().
    */
-  virtual SharedState iterate(const SharedState& current) const = 0;
+  virtual void iterate() const = 0;
-
-  /** Create an initial state from a variable assignment Values, with all
-   * other state values at their default initial values.
-   */
-  virtual SharedState initialState(const Values& initialValues) const = 0;
 
   /// @}
 
 protected:
 
-  const SharedGraph graph_;
+  NonlinearFactorGraph graph_;
 
   /** A default implementation of the optimization loop, which calls iterate()
    * until checkConvergence returns true.
    */
-  SharedState defaultOptimize(const SharedState& initial) const;
+  void defaultOptimize();
 
-  /** Initialize a state, using the current error and 0 iterations */
+  virtual const NonlinearOptimizerState& _state() const = 0;
-  void defaultInitialState(const Values& initialValues, NonlinearOptimizerState& initialState) const;
 
-  /** Constructor for initial construction of base classes.
+  virtual const NonlinearOptimizerParams& _params() const = 0;
+
+  /** Constructor for initial construction of base classes. */
+  NonlinearOptimizer(const NonlinearFactorGraph& graph) : graph_(graph) {}
+
+};
+
+
+/** The common parameters for Nonlinear optimizers.  Most optimizers
+ * deriving from NonlinearOptimizer also subclass the parameters.
  */
-  NonlinearOptimizer(const SharedGraph& graph) : graph_(graph) {}
+class NonlinearOptimizerParams {
+public:
+  /** See NonlinearOptimizerParams::verbosity */
+  enum Verbosity {
+    SILENT,
+    ERROR,
+    VALUES,
+    DELTA,
+    LINEAR
+  };
 
+  size_t maxIterations; ///< The maximum iterations to stop iterating (default 100)
+  double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5)
+  double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5)
+  double errorTol; ///< The maximum total error to stop iterating (default 0.0)
+  Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT)
+
+  NonlinearOptimizerParams() :
+    maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(1e-5),
+    errorTol(0.0), verbosity(SILENT) {}
+
+  virtual void print(const std::string& str = "") const {
+    std::cout << str << "\n";
+    std::cout << "relative decrease threshold: " << relativeErrorTol << "\n";
+    std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n";
+    std::cout << "      total error threshold: " << errorTol << "\n";
+    std::cout << "         maximum iterations: " << maxIterations << "\n";
+    std::cout << "            verbosity level: " << verbosity << std::endl;
+  }
+
+  virtual ~NonlinearOptimizerParams() {}
+};
+
+
+/**
+ * Base class for a nonlinear optimization state, including the current estimate
+ * of the variable values, error, and number of iterations.  Optimizers derived
+ * from NonlinearOptimizer usually also define a derived state class containing
+ * additional state specific to the algorithm (for example, Dogleg state
+ * contains the current trust region radius).
+ */
+class NonlinearOptimizerState {
+public:
+
+  /** The current estimate of the variable values. */
+  Values values;
+
+  /** The factor graph error on the current values. */
+  double error;
+
+  /** The number of optimization iterations performed. */
+  unsigned int iterations;
+
+  NonlinearOptimizerState(const NonlinearFactorGraph& graph, const Values& values, unsigned int iterations = 0) :
+    values(values), error(graph.error(values)), iterations(iterations) {}
+
+  NonlinearOptimizerState(const Values& values, double error, unsigned int iterations) :
+    values(values), error(error), iterations(iterations) {}
+
+  /** Virtual destructor */
+  virtual ~NonlinearOptimizerState() {}
 };
 
 /** Check whether the relative error decrease is less than relativeErrorTreshold,

gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp

@@ -1,41 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 DirectOptimizer.cpp
- * @brief 
- * @author Richard Roberts
- * @date Apr 1, 2012
- */
-
-#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
-
-namespace gtsam {
-
-/* ************************************************************************* */
-const Ordering& SuccessiveLinearizationOptimizer::ordering(const Values& values) const {
-
-  if(!ordering_) {
-    SharedParams params =
-        boost::dynamic_pointer_cast<const SuccessiveLinearizationParams>(this->params());
-
-    // If we're using a COLAMD ordering, compute it
-    if(params->ordering)
-      ordering_ = params->ordering;
-    else
-      ordering_ = *graph_->orderingCOLAMD(values);
-  }
-
-  return *ordering_;
-}
-
-} /* namespace gtsam */

gtsam/nonlinear/SuccessiveLinearizationOptimizer.h

@@ -18,38 +18,10 @@
 
 #pragma once
 
-#include <boost/variant.hpp>
-
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 
 namespace gtsam {
 
-class SuccessiveLinearizationParams;
-class SuccessiveLinearizationState;
-
-/**
- * This is an intermediate base class for NonlinearOptimizers that use direct
- * solving, i.e. factorization.  This class is here to reduce code duplication
- * for storing the ordering, having factorization and elimination parameters,
- * etc.
- */
-class SuccessiveLinearizationOptimizer : public NonlinearOptimizer {
-public:
-
-  typedef boost::shared_ptr<const SuccessiveLinearizationParams> SharedParams;
-  typedef boost::shared_ptr<const SuccessiveLinearizationOptimizer> shared_ptr;
-
-protected:
-
-  SuccessiveLinearizationOptimizer(const SharedGraph& graph) : NonlinearOptimizer(graph) {}
-
-  const Ordering& ordering(const Values& values) const;
-
-private:
-
-  mutable boost::optional<Ordering> ordering_; // Mutable because we set/compute it when needed and cache it
-};
-
 class SuccessiveLinearizationParams : public NonlinearOptimizerParams {
 public:
   /** See SuccessiveLinearizationParams::elimination */
@@ -98,7 +70,4 @@ public:
   }
 };
 
-class SuccessiveLinearizationState : public NonlinearOptimizerState {
-};
-
 } /* namespace gtsam */