Go to the documentation of this file.
28 const std::string& indent,
const KeyFormatter& keyFormatter) {
31 if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
37 std::cout << keyFormatter(
key) <<
" ";
39 std::cout <<
")" << std::endl;
41 std::cout <<
"{ nullptr }" << std::endl;
47 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
48 std::cout << indent << title << std::endl;
56 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
57 std::cout << indent << title << std::endl;
58 for(
size_t slot: slots) {
65 const std::string& indent,
const KeyFormatter& keyFormatter) {
78 std::cout << keyFormatter(
key) <<
" ";
80 std::cout <<
")" << std::endl;
82 std::cout <<
"{ nullptr }" << std::endl;
88 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
89 std::cout << indent << title << std::endl;
122 const std::optional<
FastList<Key> >& keysToMove,
const std::optional< std::vector<size_t> >& removeFactorIndices) {
127 const bool debug =
false;
129 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Begin" << std::endl;
134 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Augmenting System ..." << std::endl;
137 gttic(augment_system);
142 for (
const auto key : newTheta.
keys()) {
151 if(removeFactorIndices){
153 std::cout <<
"ConcurrentBatchFilter::update removeFactorIndices " << std::endl;
158 gttoc(augment_system);
160 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Reordering System ..." << std::endl;
167 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Optimizing System ..." << std::endl;
176 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Moving Separator ..." << std::endl;
178 gttic(move_separator);
179 if(keysToMove && keysToMove->size() > 0){
182 gttoc(move_separator);
184 if(
debug) std::cout <<
"ConcurrentBatchFilter::update End" << std::endl;
205 const bool debug =
false;
207 if(
debug) std::cout <<
"ConcurrentBatchFilter::synchronize Begin" << std::endl;
213 std::set<Key> newKeys = smootherSummarization.
keys();
214 assert(oldKeys.size() == newKeys.size());
215 assert(
std::equal(oldKeys.begin(), oldKeys.end(), newKeys.begin()));
226 if(
debug) {
PrintKeys(newSeparatorKeys,
"ConcurrentBatchFilter::synchronize ",
"Current Separator Keys:"); }
235 values.insert(smootherSummarizationValues);
236 for(
const auto key: newSeparatorKeys) {
270 if(
debug) std::cout <<
"ConcurrentBatchFilter::synchronize End" << std::endl;
278 gttic(get_summarized_factors);
284 gttoc(get_summarized_factors);
290 gttic(get_smoother_factors);
296 gttoc(get_smoother_factors);
314 gttic(insert_factors);
317 std::vector<size_t> slots;
331 slots.push_back(slot);
334 gttoc(insert_factors);
342 gttic(remove_factors);
345 for(
size_t slot: slots) {
354 gttoc(remove_factors);
361 if(keysToMove && keysToMove->size() > 0) {
375 const bool debug =
false;
377 if(
debug) std::cout <<
"ConcurrentBatchFilter::optimize Begin" << std::endl;
385 double lambdaFactor =
parameters.lambdaFactor;
386 double lambdaUpperBound =
parameters.lambdaUpperBound;
387 double lambdaLowerBound = 1.0e-10;
388 size_t maxIterations =
parameters.maxIterations;
389 double relativeErrorTol =
parameters.relativeErrorTol;
390 double absoluteErrorTol =
parameters.absoluteErrorTol;
398 if(
result.error <= errorTol) {
399 if(
debug) { std::cout <<
"Exiting, as error = " <<
result.error <<
" < " << errorTol << std::endl; }
403 std::cout <<
"linearValues: " << linearValues.
size() << std::endl;
404 std::cout <<
"Initial error: " <<
result.error << std::endl;
408 double previousError;
411 previousError =
result.error;
414 gttic(optimizer_iteration);
422 if(
debug) { std::cout <<
"trying lambda = " <<
lambda << std::endl; }
432 size_t dim = key_value.second.size();
433 Matrix A = Matrix::Identity(dim,dim);
447 evalpoint = theta.
retract(newDelta);
451 gttic(compute_error);
453 gttoc(compute_error);
456 std::cout <<
"linear delta norm = " << newDelta.
norm() << std::endl;
457 std::cout <<
"next error = " <<
error << std::endl;
469 if(linearValues.
size() > 0) {
470 theta.
update(linearValues);
471 for(
const auto key: linearValues.
keys()) {
478 if(
lambda < lambdaLowerBound) {
479 lambda = lambdaLowerBound;
485 if(
lambda >= lambdaUpperBound) {
487 std::cout <<
"Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
496 gttoc(optimizer_iteration);
498 if(
debug) { std::cout <<
"using lambda = " <<
lambda << std::endl; }
501 }
while(
result.iterations < maxIterations &&
504 if(
debug) { std::cout <<
"newError: " <<
result.error << std::endl; }
506 if(
debug) std::cout <<
"ConcurrentBatchFilter::optimize End" << std::endl;
525 const bool debug =
false;
527 if(
debug) std::cout <<
"ConcurrentBatchFilter::moveSeparator Begin" << std::endl;
533 std::vector<size_t> removedFactorSlots;
535 for(
Key key: keysToMove) {
536 const auto& slots = variableIndex[
key];
537 removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
541 std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
542 removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
545 removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
550 std::cout <<
"ConcurrentBatchFilter::moveSeparator Removed Factor Slots: ";
551 for(
size_t slot: removedFactorSlots) {
552 std::cout << slot <<
" ";
554 std::cout << std::endl;
559 for(
size_t slot: removedFactorSlots) {
573 KeySet newSeparatorKeys = removedFactors.
keys();
575 for(
Key key: keysToMove) {
576 newSeparatorKeys.erase(
key);
582 KeySet shortcutKeys = newSeparatorKeys;
584 shortcutKeys.insert(
key);
593 graph.push_back(removedFactors);
639 for(
Key key: keysToMove) {
644 for(
Key key: keysToMove) {
650 if(
debug) std::cout <<
"ConcurrentBatchFilter::moveSeparator End" << std::endl;
void removeFactors(const std::vector< size_t > &slots)
std::shared_ptr< This > shared_ptr
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.
iterator insert(const std::pair< Key, Vector > &key_value)
const GaussianFactorGraph factors
void getSmootherFactors(NonlinearFactorGraph &smootherFactors, Values &smootherValues) override
A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoothing interface.
NonlinearFactorGraph factors_
The set of all factors currently in the filter.
static void optimize(const NonlinearFactorGraph &factors, Values &theta, const Ordering &ordering, VectorValues &delta, const Values &linearValues, const LevenbergMarquardtParams ¶meters, Result &result)
void merge(const FastSet &other)
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
void update(Key j, const Value &val)
static void PrintKeys(const Container &keys, const std::string &indent, const std::string &title, const KeyFormatter &keyFormatter=DefaultKeyFormatter)
Implementation of PrintKeys.
static void PrintLinearFactor(const GaussianFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
NonlinearFactorGraph smootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization.
static const double sigma
void print(const std::string &s="Concurrent Batch Filter:\n", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
LevenbergMarquardtParams parameters_
LM parameters.
void moveSeparator(const FastList< Key > &keysToMove)
const sharedFactor at(size_t i) const
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother.
static void PrintLinearFactorGraph(const GaussianFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
const ValueType at(Key j) const
Values retract(const VectorValues &delta) const
virtual void resize(size_t size)
void getSummarizedFactors(NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override
bool equals(const ConcurrentFilter &rhs, double tol=1e-9) const override
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
static void PrintNonlinearFactorGraph(const NonlinearFactorGraph &factors, const std::string &indent="", const std::string &title="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
static Ordering Colamd(const FACTOR_GRAPH &graph)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
static ConjugateGradientParameters parameters
void replace(size_t index, sharedFactor factor)
void reorder(const std::optional< FastList< Key > > &keysToMove={})
static constexpr bool debug
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
bool equals(const Ordering &other, double tol=1e-9) const
noiseModel::Diagonal::shared_ptr model
static enum @1096 ordering
const gtsam::Symbol key('X', 0)
VectorValues zeroVectors() const
void synchronize(const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override
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.
void insert(Key j, const Value &val)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
std::vector< size_t > insertFactors(const NonlinearFactorGraph &factors)
void reserve(size_t size)
VectorValues delta_
The current set of linear deltas from the linearization point.
Ordering ordering_
The current ordering used to calculate the linear deltas.
Values theta_
Current linearization point of all variables in the filter.
virtual Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< std::vector< size_t > > &removeFactorIndices={})
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
value_type KeyValuePair
Typedef to pair<Key, Vector>
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
GaussianFactorGraph::Eliminate getEliminationFunction() const
NonlinearFactorGraph filterSummarization_
A temporary holding place for calculated filter summarization factors to be sent to the smoother.
bool equal(const T &obj1, const T &obj2, double tol)
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr &factor, const std::string &indent="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
Jet< T, N > sqrt(const Jet< T, N > &f)
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
std::vector< size_t > separatorSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09