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_;
  }
};

}