LevenbergMarquardtOptimizer.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/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/linear/VectorValues.h>
#include <chrono>
 
class NonlinearOptimizerMoreOptimizationTest;
 
namespace gtsam {

class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer {
 
protected:
  const LevenbergMarquardtParams params_; 
  
  // startTime_ is a chrono time point
  std::chrono::time_point<std::chrono::high_resolution_clock> startTime_; 
  std::chrono::time_point<std::chrono::high_resolution_clock> iterationStart_;
  double lastLinearizeTime_{0.0};
  double lastSolveTime_{0.0};
 
  void initTime();
 
public:
  typedef std::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;


  LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
                              const LevenbergMarquardtParams& params = LevenbergMarquardtParams());

  LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
                              const Ordering& ordering,
                              const LevenbergMarquardtParams& params = LevenbergMarquardtParams());

  ~LevenbergMarquardtOptimizer() override {
  }



  double lambda() const;

  int getInnerIterations() const;

  void print(const std::string& str = "") const {
    std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;
    this->params_.print("  parameters:\n");
  }



  GaussianFactorGraph::shared_ptr iterate() override;

  const LevenbergMarquardtParams& params() const {
    return params_;
  }
 
  void writeLogFile(double currentError);

  virtual GaussianFactorGraph::shared_ptr linearize() const;

  virtual GaussianFactorGraph buildDampedSystem(
      const GaussianFactorGraph& linear,
      const VectorValues& sqrtHessianDiagonal) const;

  bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);

 
protected:

  virtual double linearDeltaError(const GaussianFactorGraph& linear,
                                  const VectorValues& delta,
                                  double* oldError,
                                  double* newError) const;

  const NonlinearOptimizerParams& _params() const override {
    return params_;
  }
 
};
 
}