NonlinearOptimizer.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizerParams.h>

namespace gtsam {

class NonlinearOptimizer;

class GTSAM_EXPORT NonlinearOptimizerState {
public:

  Values values;

  double error;

  int iterations;

  NonlinearOptimizerState() {}

  virtual ~NonlinearOptimizerState() {}

protected:
  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) {}

  friend class NonlinearOptimizer;
};

class GTSAM_EXPORT NonlinearOptimizer {

protected:
  NonlinearFactorGraph graph_;

public:
  typedef boost::shared_ptr<const NonlinearOptimizer> shared_ptr;


  virtual const Values& optimize() { defaultOptimize(); return values(); }

  const Values& optimizeSafely();

  double error() const { return _state().error; }

  int iterations() const { return _state().iterations; }

  const Values& values() const { return _state().values; }



  virtual ~NonlinearOptimizer() {}

  virtual VectorValues solve(const GaussianFactorGraph &gfg,
      const Values& initial, const NonlinearOptimizerParams& params) const;

  virtual void iterate() = 0;


protected:  
  void defaultOptimize();

  virtual const NonlinearOptimizerState& _state() const = 0;

  virtual const NonlinearOptimizerParams& _params() const = 0;

  NonlinearOptimizer(const NonlinearFactorGraph& graph) : graph_(graph) {}

};

GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold,
    double absoluteErrorTreshold, double errorThreshold,
    double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT);

} // gtsam