Go to the documentation of this file.
30 std::cout <<
" Factors:" << std::endl;
50 const std::optional< std::vector<size_t> >& removeFactorIndices) {
58 gttic(augment_system);
64 for(
const auto key: newTheta.
keys()) {
74 if(removeFactorIndices)
77 gttoc(augment_system);
117 gttic(get_summarized_factors);
125 gttoc(get_summarized_factors);
138 for(
const auto key: smootherValues.
keys()) {
151 for(
const auto key: separatorValues.
keys()) {
188 gttic(insert_factors);
191 std::vector<size_t> slots;
205 slots.push_back(slot);
208 gttoc(insert_factors);
216 gttic(remove_factors);
219 for(
size_t slot: slots) {
228 gttoc(remove_factors);
262 double previousError;
265 previousError =
result.error;
268 gttic(optimizer_iteration);
276 std::cout <<
"trying lambda = " <<
lambda << std::endl;
285 size_t dim = key_value.second.size();
286 Matrix A = Matrix::Identity(dim,dim);
295 dampedFactorGraph.
print(
"damped");
306 std::cout <<
"linear delta norm = " << newDelta.
norm() << std::endl;
308 newDelta.
print(
"delta");
311 gttic(compute_error);
313 gttoc(compute_error);
316 std::cout <<
"next error = " <<
error << std::endl;
342 std::cout <<
"Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
351 gttoc(optimizer_iteration);
354 std::cout <<
"using lambda = " <<
lambda << std::endl;
388 if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
394 std::cout << keyFormatter(
key) <<
" ";
396 std::cout <<
")" << std::endl;
398 std::cout <<
"{ nullptr }" << std::endl;
408 std::cout << keyFormatter(
key) <<
" ";
410 std::cout <<
")" << std::endl;
412 std::cout <<
"{ nullptr }" << std::endl;
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
void getSummarizedFactors(NonlinearFactorGraph &summarizedFactors, Values &separatorValues) override
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
std::shared_ptr< This > shared_ptr
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
Ordering ordering_
The current ordering used to calculate the linear deltas.
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoothing interface...
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
iterator insert(const std::pair< Key, Vector > &key_value)
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
const GaussianFactorGraph factors
void updateSmootherSummarization()
VariableIndex variableIndex_
The current variable index, which allows efficient factor lookup by variable.
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
LevenbergMarquardtParams parameters_
LM parameters.
void update(Key j, const Value &val)
void print(const std::string &s="Concurrent Batch Smoother:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
void removeFactors(const std::vector< size_t > &slots)
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
VectorValues delta_
The current set of linear deltas from the linearization point.
double error(const Values &values) const
const ValueType at(Key j) const
Values retract(const VectorValues &delta) const
Values theta_
Current linearization point of all variables in the smoother.
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
virtual Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< std::vector< size_t > > &removeFactorIndices={})
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
NonlinearFactorGraph factors_
The set of all factors currently in the smoother.
void replace(size_t index, sharedFactor factor)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool equals(const Values &other, double tol=1e-9) const
static void PrintLinearFactor(const GaussianFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
double lambdaUpperBound
The maximum lambda to try before assuming the optimization has failed (default: 1e5)
void synchronize(const NonlinearFactorGraph &smootherFactors, const Values &smootherValues, const NonlinearFactorGraph &summarizedFactors, const Values &separatorValues) override
double lambdaInitial
The initial Levenberg-Marquardt damping term (default: 1e-5)
bool equals(const Ordering &other, double tol=1e-9) const
noiseModel::Diagonal::shared_ptr model
const gtsam::Symbol key('X', 0)
std::vector< size_t > filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
VectorValues zeroVectors() const
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
bool equals(const VectorValues &x, double tol=1e-9) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
bool equals(const ConcurrentSmoother &rhs, double tol=1e-9) const override
std::vector< size_t > insertFactors(const NonlinearFactorGraph &factors)
void insert(Key j, const Value &val)
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
size_t maxIterations
The maximum iterations to stop iterating (default 100)
double errorTol
The maximum total error to stop iterating (default 0.0)
double lambdaFactor
The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
void reserve(size_t size)
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
value_type KeyValuePair
Typedef to pair<Key, Vector>
NonlinearFactorGraph graph
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
std::uint64_t Key
Integer nonlinear key type.
GaussianFactorGraph::Eliminate getEliminationFunction() const
Jet< T, N > sqrt(const Jet< T, N > &f)
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09