LevenbergMarquardtState.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
22 
23 #include <gtsam/nonlinear/Values.h>
27 #include <gtsam/base/Matrix.h>
28 #include <gtsam/base/Vector.h>
29 
30 #include <algorithm>
31 #include <cmath>
32 #include <stdexcept>
33 #include <vector>
34 
35 namespace gtsam {
36 
37 namespace internal {
38 
39 // TODO(frank): once Values supports move, we can make State completely functional.
40 // As it is now, increaseLambda is non-const or otherwise we make a Values copy every time
41 // decreaseLambda would also benefit from a working Values move constructor
44 
45  // TODO(frank): make these const once increaseLambda can be made const
46  double lambda;
47  double currentFactor;
49  // optimization (for each iteration, LM tries multiple
50  // inner iterations with different lambdas)
51 
52  LevenbergMarquardtState(const Values& initialValues, double _error, double _lambda,
53  double currentFactor, unsigned int _iterations = 0,
54  unsigned int totalNumberInnerIterations = 0)
55  : NonlinearOptimizerState(initialValues, _error, _iterations),
56  lambda(_lambda),
59 
60  // Constructor version that takes ownership of values
61  LevenbergMarquardtState(Values&& initialValues, double _error, double _lambda, double currentFactor,
62  unsigned int _iterations = 0, unsigned int totalNumberInnerIterations = 0)
63  : NonlinearOptimizerState(initialValues, _error, _iterations),
64  lambda(_lambda),
67 
68  // Applies policy to *increase* lambda: should be used if the current update was NOT successful
69  // TODO(frank): non-const method until Values properly support std::move
73  if (!params.useFixedLambdaFactor) {
74  currentFactor *= 2.0;
75  }
76  }
77 
78  // Apply policy to decrease lambda if the current update was successful
79  // stepQuality not used in the naive policy)
80  // Take ownsership of newValues, must be passed an rvalue
81  std::unique_ptr<This> decreaseLambda(const LevenbergMarquardtParams& params, double stepQuality,
82  Values&& newValues, double newError) const {
83  double newLambda = lambda, newFactor = currentFactor;
84  if (params.useFixedLambdaFactor) {
85  newLambda /= currentFactor;
86  } else {
87  // TODO(frank): odd that currentFactor is not used to change lambda here...
88  newLambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3));
89  newFactor = 2.0 * currentFactor;
90  }
91  newLambda = std::max(params.lambdaLowerBound, newLambda);
92  return std::unique_ptr<This>(new This(std::move(newValues), newError, newLambda, newFactor,
94  }
95 
98  struct CachedModel {
99  CachedModel() {} // default int makes zero-size matrices
100  CachedModel(int dim, double sigma)
101  : A(Matrix::Identity(dim, dim)),
102  b(Vector::Zero(dim)),
103  model(noiseModel::Isotropic::Sigma(dim, sigma)) {}
104  CachedModel(int dim, double sigma, const Vector& diagonal)
105  : A(Eigen::DiagonalMatrix<double, Eigen::Dynamic>(diagonal)),
106  b(Vector::Zero(dim)),
107  model(noiseModel::Isotropic::Sigma(dim, sigma)) {}
111  };
112 
113  // Small cache of A|b|model indexed by dimension. Can save many mallocs.
114  mutable std::vector<CachedModel> noiseModelCache;
115  CachedModel* getCachedModel(size_t dim) const {
116  if (dim >= noiseModelCache.size())
117  noiseModelCache.resize(dim+1);
118  CachedModel* item = &noiseModelCache[dim];
119  if (!item->model)
120  *item = CachedModel(dim, 1.0 / std::sqrt(lambda));
121  return item;
122  }
123 
126  noiseModelCache.resize(0);
127  // for each of the variables, add a prior
128  damped.reserve(damped.size() + values.size());
129  std::map<Key,size_t> dims = values.dims();
130  for (const auto& key_dim : dims) {
131  const Key& key = key_dim.first;
132  const size_t& dim = key_dim.second;
133  const CachedModel* item = getCachedModel(dim);
134  damped.emplace_shared<JacobianFactor>(key, item->A, item->b, item->model);
135  }
136  return damped;
137  }
138 
141  const VectorValues& sqrtHessianDiagonal) const {
142  noiseModelCache.resize(0);
143  damped.reserve(damped.size() + values.size());
144  for (const auto& key_vector : sqrtHessianDiagonal) {
145  try {
146  const Key key = key_vector.first;
147  const size_t dim = key_vector.second.size();
148  CachedModel* item = getCachedModel(dim);
149  item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian)
150  damped.emplace_shared<JacobianFactor>(key, item->A, item->b, item->model);
151  } catch (const std::out_of_range&) {
152  continue; // Don't attempt any damping if no key found in diagonal
153  }
154  }
155  return damped;
156  }
157 };
158 
159 } // namespace internal
160 
161 } /* namespace gtsam */
gtsam::internal::LevenbergMarquardtState::CachedModel::A
Matrix A
Definition: LevenbergMarquardtState.h:108
gtsam::internal::LevenbergMarquardtState::CachedModel::CachedModel
CachedModel(int dim, double sigma)
Definition: LevenbergMarquardtState.h:100
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::Values::dims
std::map< Key, size_t > dims() const
Definition: Values.cpp:251
gtsam::internal::NonlinearOptimizerState::values
const Values values
Definition: NonlinearOptimizerState.h:37
gtsam::internal::NonlinearOptimizerState::iterations
const size_t iterations
Definition: NonlinearOptimizerState.h:43
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::Values::size
size_t size() const
Definition: Values.h:178
NonlinearOptimizerState.h
Private class for NonlinearOptimizer state.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::internal::LevenbergMarquardtState::CachedModel::b
Vector b
Definition: LevenbergMarquardtState.h:109
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
diagonal
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
gtsam::internal::LevenbergMarquardtState::CachedModel::CachedModel
CachedModel()
Definition: LevenbergMarquardtState.h:99
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
gtsam::internal::LevenbergMarquardtState::lambda
double lambda
Definition: LevenbergMarquardtState.h:46
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
A
Definition: test_numpy_dtypes.cpp:298
gtsam::internal::LevenbergMarquardtState::getCachedModel
CachedModel * getCachedModel(size_t dim) const
Definition: LevenbergMarquardtState.h:115
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::internal::LevenbergMarquardtState::CachedModel::CachedModel
CachedModel(int dim, double sigma, const Vector &diagonal)
Definition: LevenbergMarquardtState.h:104
gtsam::internal::LevenbergMarquardtState::totalNumberInnerIterations
int totalNumberInnerIterations
The total number of inner iterations in the.
Definition: LevenbergMarquardtState.h:48
gtsam::internal::LevenbergMarquardtState::noiseModelCache
std::vector< CachedModel > noiseModelCache
Definition: LevenbergMarquardtState.h:114
Eigen::Dynamic
const int Dynamic
Definition: Constants.h:22
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
gtsam::internal::LevenbergMarquardtState::CachedModel
Definition: LevenbergMarquardtState.h:98
gtsam::internal::LevenbergMarquardtState::buildDampedSystem
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped, const VectorValues &sqrtHessianDiagonal) const
Build a damped system, use hessianDiagonal per variable (more expensive)
Definition: LevenbergMarquardtState.h:140
gtsam::internal::LevenbergMarquardtState::CachedModel::model
SharedDiagonal model
Definition: LevenbergMarquardtState.h:110
ceres::pow
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
gtsam::internal::LevenbergMarquardtState
Definition: LevenbergMarquardtState.h:42
gtsam::internal::LevenbergMarquardtState::LevenbergMarquardtState
LevenbergMarquardtState(Values &&initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
Definition: LevenbergMarquardtState.h:61
gtsam::internal::NonlinearOptimizerState
Definition: NonlinearOptimizerState.h:34
key
const gtsam::Symbol key('X', 0)
JacobianFactor.h
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
gtsam::Values
Definition: Values.h:65
gtsam::internal::LevenbergMarquardtState::currentFactor
double currentFactor
Definition: LevenbergMarquardtState.h:47
gtsam::internal::LevenbergMarquardtState::This
LevenbergMarquardtState This
Definition: LevenbergMarquardtState.h:43
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::internal::LevenbergMarquardtState::buildDampedSystem
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped) const
Build a damped system for a specific lambda, vanilla version.
Definition: LevenbergMarquardtState.h:125
gtsam::FactorGraph::reserve
void reserve(size_t size)
Definition: FactorGraph.h:143
internal
Definition: BandTriangularSolver.h:13
gtsam::internal::LevenbergMarquardtState::increaseLambda
void increaseLambda(const LevenbergMarquardtParams &params)
Definition: LevenbergMarquardtState.h:70
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::internal::LevenbergMarquardtState::decreaseLambda
std::unique_ptr< This > decreaseLambda(const LevenbergMarquardtParams &params, double stepQuality, Values &&newValues, double newError) const
Definition: LevenbergMarquardtState.h:81
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::internal::LevenbergMarquardtState::LevenbergMarquardtState
LevenbergMarquardtState(const Values &initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
Definition: LevenbergMarquardtState.h:52


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07