53 double currentFactor,
unsigned int _iterations = 0,
54 unsigned int totalNumberInnerIterations = 0)
57 currentFactor(currentFactor),
58 totalNumberInnerIterations(totalNumberInnerIterations) {}
62 unsigned int _iterations = 0,
unsigned int totalNumberInnerIterations = 0)
65 currentFactor(currentFactor),
66 totalNumberInnerIterations(totalNumberInnerIterations) {}
72 totalNumberInnerIterations += 1;
82 Values&& newValues,
double newError)
const {
88 newLambda *=
std::max(1.0 / 3.0, 1.0 -
pow(2.0 * stepQuality - 1.0, 3));
92 return std::unique_ptr<This>(
new This(std::move(newValues), newError, newLambda, newFactor,
93 iterations + 1, totalNumberInnerIterations + 1));
101 :
A(
Matrix::Identity(dim, dim)),
103 model(noiseModel::Isotropic::Sigma(dim, sigma)) {}
107 model(noiseModel::Isotropic::Sigma(dim, sigma)) {}
116 if (dim >= noiseModelCache.size())
117 noiseModelCache.resize(dim+1);
126 noiseModelCache.
resize(0);
130 for (
const auto& key_dim : dims) {
131 const Key&
key = key_dim.first;
132 const size_t& dim = key_dim.second;
142 noiseModelCache.
resize(0);
144 for (
const auto& key_vector : sqrtHessianDiagonal) {
146 const Key key = key_vector.first;
147 const size_t dim = key_vector.second.size();
149 item->
A.diagonal() = sqrtHessianDiagonal.at(key);
151 }
catch (
const std::out_of_range&) {
const gtsam::Symbol key('X', 0)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
A non-templated config holding any types of Manifold-group elements.
LevenbergMarquardtState This
void diagonal(const MatrixType &m)
Namespace containing all symbols from the Eigen library.
LevenbergMarquardtState(Values &&initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
CachedModel(int dim, double sigma)
static const SmartProjectionParams params
std::vector< CachedModel > noiseModelCache
CachedModel * getCachedModel(size_t dim) const
std::unique_ptr< This > decreaseLambda(const LevenbergMarquardtParams ¶ms, 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.
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped) const
Build a damped system for a specific lambda, vanilla version.
std::map< Key, size_t > dims() const
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped, const VectorValues &sqrtHessianDiagonal) const
Build a damped system, use hessianDiagonal per variable (more expensive)
noiseModel::Diagonal::shared_ptr SharedDiagonal
typedef and functions to augment Eigen's VectorXd
double lambdaLowerBound
The minimum lambda used in LM (default: 0)
virtual void resize(size_t size)
Private class for NonlinearOptimizer state.
static const double sigma
int totalNumberInnerIterations
The total number of inner iterations in the.
void reserve(size_t size)
Jet< T, N > sqrt(const Jet< T, N > &f)
Jet< T, N > pow(const Jet< T, N > &f, double g)
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.
void increaseLambda(const LevenbergMarquardtParams ¶ms)