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),
57  currentFactor(currentFactor),
58  totalNumberInnerIterations(totalNumberInnerIterations) {}
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),
65  currentFactor(currentFactor),
66  totalNumberInnerIterations(totalNumberInnerIterations) {}
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
71  lambda *= currentFactor;
72  totalNumberInnerIterations += 1;
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,
93  iterations + 1, totalNumberInnerIterations + 1));
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 */
const gtsam::Symbol key('X', 0)
#define max(a, b)
Definition: datatypes.h:20
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
A non-templated config holding any types of Manifold-group elements.
void diagonal(const MatrixType &m)
Definition: diagonal.cpp:12
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
LevenbergMarquardtState(Values &&initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
static const SmartProjectionParams params
CachedModel * getCachedModel(size_t dim) const
size_t size() const
Definition: FactorGraph.h:334
Eigen::VectorXd Vector
Definition: Vector.h:38
std::unique_ptr< This > decreaseLambda(const LevenbergMarquardtParams &params, double stepQuality, Values &&newValues, double newError) const
CachedModel(int dim, double sigma, const Vector &diagonal)
bool useFixedLambdaFactor
if true applies constant increase (or decrease) to lambda according to lambdaFactor ...
Linear Factor Graph where all factors are Gaussians.
size_t size() const
Definition: Values.h:178
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped) const
Build a damped system for a specific lambda, vanilla version.
std::map< Key, size_t > dims() const
Definition: Values.cpp:251
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped, const VectorValues &sqrtHessianDiagonal) const
Build a damped system, use hessianDiagonal per variable (more expensive)
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
double lambdaLowerBound
The minimum lambda used in LM (default: 0)
virtual void resize(size_t size)
Definition: FactorGraph.h:389
Private class for NonlinearOptimizer state.
static const double sigma
int totalNumberInnerIterations
The total number of inner iterations in the.
void reserve(size_t size)
Definition: FactorGraph.h:186
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const int Dynamic
Definition: Constants.h:22
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
LevenbergMarquardtState(const Values &initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
void increaseLambda(const LevenbergMarquardtParams &params)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33