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_; 
 
  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;
 
  GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear,
                                        const VectorValues& sqrtHessianDiagonal) const;
 
  bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);
 
 
protected:
 
  const NonlinearOptimizerParams& _params() const override {
    return params_;
  }
};
 
}