Go to the documentation of this file.
30 const std::string& indent,
const KeyFormatter& keyFormatter) {
33 if(std::dynamic_pointer_cast<LinearContainerFactor>(
factor)) {
39 std::cout << keyFormatter(
key) <<
" ";
41 std::cout <<
")" << std::endl;
43 std::cout <<
"{ nullptr }" << std::endl;
49 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
50 std::cout << indent << title << std::endl;
58 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
59 std::cout << indent << title << std::endl;
60 for(
size_t slot: slots) {
67 const std::string& indent,
const KeyFormatter& keyFormatter) {
80 std::cout << keyFormatter(
key) <<
" ";
82 std::cout <<
")" << std::endl;
84 std::cout <<
"{ nullptr }" << std::endl;
90 const std::string& indent,
const std::string& title,
const KeyFormatter& keyFormatter) {
91 std::cout << indent << title << std::endl;
124 const std::optional<
FastList<Key> >& keysToMove,
const std::optional< std::vector<size_t> >& removeFactorIndices) {
129 const bool debug =
false;
131 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Begin" << std::endl;
136 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Augmenting System ..." << std::endl;
139 gttic(augment_system);
144 for (
const auto key : newTheta.
keys()) {
153 if(removeFactorIndices){
155 std::cout <<
"ConcurrentBatchFilter::update removeFactorIndices " << std::endl;
160 gttoc(augment_system);
162 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Reordering System ..." << std::endl;
169 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Optimizing System ..." << std::endl;
178 if(
debug) std::cout <<
"ConcurrentBatchFilter::update Moving Separator ..." << std::endl;
180 gttic(move_separator);
181 if(keysToMove && keysToMove->size() > 0){
184 gttoc(move_separator);
186 if(
debug) std::cout <<
"ConcurrentBatchFilter::update End" << std::endl;
207 const bool debug =
false;
209 if(
debug) std::cout <<
"ConcurrentBatchFilter::synchronize Begin" << std::endl;
215 std::set<Key> newKeys = smootherSummarization.
keys();
216 assert(oldKeys.size() == newKeys.size());
217 assert(
std::equal(oldKeys.begin(), oldKeys.end(), newKeys.begin()));
228 if(
debug) {
PrintKeys(newSeparatorKeys,
"ConcurrentBatchFilter::synchronize ",
"Current Separator Keys:"); }
237 values.insert(smootherSummarizationValues);
238 for(
const auto key: newSeparatorKeys) {
272 if(
debug) std::cout <<
"ConcurrentBatchFilter::synchronize End" << std::endl;
280 gttic(get_summarized_factors);
286 gttoc(get_summarized_factors);
292 gttic(get_smoother_factors);
298 gttoc(get_smoother_factors);
316 gttic(insert_factors);
319 std::vector<size_t> slots;
333 slots.push_back(slot);
336 gttoc(insert_factors);
344 gttic(remove_factors);
347 for(
size_t slot: slots) {
356 gttoc(remove_factors);
363 if(keysToMove && keysToMove->size() > 0) {
377 const bool debug =
false;
379 if(
debug) std::cout <<
"ConcurrentBatchFilter::optimize Begin" << std::endl;
387 double lambdaFactor =
parameters.lambdaFactor;
388 double lambdaUpperBound =
parameters.lambdaUpperBound;
389 double lambdaLowerBound = 1.0e-10;
390 size_t maxIterations =
parameters.maxIterations;
391 double relativeErrorTol =
parameters.relativeErrorTol;
392 double absoluteErrorTol =
parameters.absoluteErrorTol;
400 if(
result.error <= errorTol) {
401 if(
debug) { std::cout <<
"Exiting, as error = " <<
result.error <<
" < " << errorTol << std::endl; }
405 std::cout <<
"linearValues: " << linearValues.
size() << std::endl;
406 std::cout <<
"Initial error: " <<
result.error << std::endl;
410 double previousError;
413 previousError =
result.error;
416 gttic(optimizer_iteration);
424 if(
debug) { std::cout <<
"trying lambda = " <<
lambda << std::endl; }
434 size_t dim = key_value.second.size();
435 Matrix A = Matrix::Identity(dim,dim);
449 evalpoint = theta.
retract(newDelta);
453 gttic(compute_error);
455 gttoc(compute_error);
458 std::cout <<
"linear delta norm = " << newDelta.
norm() << std::endl;
459 std::cout <<
"next error = " <<
error << std::endl;
471 if(linearValues.
size() > 0) {
472 theta.
update(linearValues);
473 for(
const auto key: linearValues.
keys()) {
480 if(
lambda < lambdaLowerBound) {
481 lambda = lambdaLowerBound;
487 if(
lambda >= lambdaUpperBound) {
489 std::cout <<
"Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
498 gttoc(optimizer_iteration);
500 if(
debug) { std::cout <<
"using lambda = " <<
lambda << std::endl; }
503 }
while(
result.iterations < maxIterations &&
506 if(
debug) { std::cout <<
"newError: " <<
result.error << std::endl; }
508 if(
debug) std::cout <<
"ConcurrentBatchFilter::optimize End" << std::endl;
527 const bool debug =
false;
529 if(
debug) std::cout <<
"ConcurrentBatchFilter::moveSeparator Begin" << std::endl;
535 std::vector<size_t> removedFactorSlots;
537 for(
Key key: keysToMove) {
538 const auto& slots = variableIndex[
key];
539 removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
543 std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
544 removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
547 removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
552 std::cout <<
"ConcurrentBatchFilter::moveSeparator Removed Factor Slots: ";
553 for(
size_t slot: removedFactorSlots) {
554 std::cout << slot <<
" ";
556 std::cout << std::endl;
561 for(
size_t slot: removedFactorSlots) {
575 KeySet newSeparatorKeys = removedFactors.
keys();
577 for(
Key key: keysToMove) {
578 newSeparatorKeys.erase(
key);
584 KeySet shortcutKeys = newSeparatorKeys;
586 shortcutKeys.insert(
key);
595 graph.push_back(removedFactors);
641 for(
Key key: keysToMove) {
646 for(
Key key: keysToMove) {
652 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.
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
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 Sun Dec 22 2024 04:11:17