Go to the documentation of this file.
73 if (!
params.useFixedLambdaFactor) {
82 Values&& newValues,
double newError)
const {
84 if (
params.useFixedLambdaFactor) {
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,
101 :
A(
Matrix::Identity(dim, dim)),
103 model(noiseModel::Isotropic::Sigma(dim,
sigma)) {}
107 model(noiseModel::Isotropic::Sigma(dim,
sigma)) {}
130 for (
const auto& key_dim : dims) {
131 const Key&
key = key_dim.first;
132 const size_t& dim = key_dim.second;
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&) {
CachedModel(int dim, double sigma)
Namespace containing all symbols from the Eigen library.
Linear Factor Graph where all factors are Gaussians.
std::map< Key, size_t > dims() const
typedef and functions to augment Eigen's VectorXd
Private class for NonlinearOptimizer state.
typedef and functions to augment Eigen's MatrixXd
static const SmartProjectionParams params
void diagonal(const MatrixType &m)
static const double sigma
CachedModel * getCachedModel(size_t dim) const
CachedModel(int dim, double sigma, const Vector &diagonal)
int totalNumberInnerIterations
The total number of inner iterations in the.
std::vector< CachedModel > noiseModelCache
noiseModel::Diagonal::shared_ptr SharedDiagonal
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped, const VectorValues &sqrtHessianDiagonal) const
Build a damped system, use hessianDiagonal per variable (more expensive)
Jet< T, N > pow(const Jet< T, N > &f, double g)
LevenbergMarquardtState(Values &&initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
const gtsam::Symbol key('X', 0)
LevenbergMarquardtState This
GaussianFactorGraph buildDampedSystem(GaussianFactorGraph damped) const
Build a damped system for a specific lambda, vanilla version.
void reserve(size_t size)
void increaseLambda(const LevenbergMarquardtParams ¶ms)
std::uint64_t Key
Integer nonlinear key type.
std::unique_ptr< This > decreaseLambda(const LevenbergMarquardtParams ¶ms, double stepQuality, Values &&newValues, double newError) const
Jet< T, N > sqrt(const Jet< T, N > &f)
A non-templated config holding any types of Manifold-group elements.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
LevenbergMarquardtState(const Values &initialValues, double _error, double _lambda, double currentFactor, unsigned int _iterations=0, unsigned int totalNumberInnerIterations=0)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:42