Loading...
Searching...
No Matches
ISAM2Clique.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <string>
namespace gtsam {
class GTSAM_EXPORT ISAM2Clique
: public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
public:
typedef ISAM2Clique This;
typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef GaussianConditional ConditionalType;
typedef ConditionalType::shared_ptr sharedConditional;
Base::FactorType::shared_ptr cachedFactor_;
Vector gradientContribution_;
#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
mutable FastMap<Key, VectorValues::iterator> solnPointers_;
#endif
ISAM2Clique() : Base() {}
virtual ~ISAM2Clique() = default;
ISAM2Clique(const ISAM2Clique& other)
: Base(other),
cachedFactor_(other.cachedFactor_),
gradientContribution_(other.gradientContribution_) {}
ISAM2Clique& operator=(const ISAM2Clique& other) {
Base::operator=(other);
cachedFactor_ = other.cachedFactor_;
gradientContribution_ = other.gradientContribution_;
return *this;
}
void setEliminationResult(
const FactorGraphType::EliminationResult& eliminationResult);
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
const Vector& gradientContribution() const { return gradientContribution_; }
void addGradientAtZero(VectorValues* g) const;
bool equals(const This& other, double tol = 1e-9) const;
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
void optimizeWildfire(const KeySet& replaced, double threshold,
KeySet* changed, VectorValues* delta,
size_t* count) const;
bool optimizeWildfireNode(const KeySet& replaced, double threshold,
KeySet* changed, VectorValues* delta,
size_t* count) const;
void nnz_internal(size_t* result) const;
size_t calculate_nnz() const;
void findAll(const KeySet& markedMask, KeySet* keys) const;
private:
bool isDirty(const KeySet& replaced, const KeySet& changed) const;
void fastBackSubstitute(VectorValues* delta) const;
/*
* Check whether the values changed above a threshold, or always true if the
* clique was replaced.
*/
bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
const VectorValues& delta, double threshold) const;
void markFrontalsAsChanged(KeySet* changed) const;
void restoreFromOriginals(const Vector& originalValues,
VectorValues* delta) const;
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
}
}; // \struct ISAM2Clique
size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
const KeySet& replaced, VectorValues* delta);
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
double threshold, const KeySet& replaced,
VectorValues* delta);
} // namespace gtsam
Key.h
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
GaussianConditional.h
Conditional Gaussian Base class.
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::optimizeWildfire
size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
Optimize the BayesTree, starting from the root.
Definition ISAM2Clique.cpp:226
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::BayesTreeCliqueBase< This, GaussianFactorGraph >::operator=
BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c)
Definition BayesTreeCliqueBase.h:90
gtsam::BayesTreeCliqueBase< ISAM2Clique, GaussianFactorGraph >::BayesTreeCliqueBase
BayesTreeCliqueBase()
Definition BayesTreeCliqueBase.h:74
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition ISAM2Clique.h:37
gtsam::ISAM2Clique::gradientContribution
const Vector & gradientContribution() const
Access the gradient contribution.
Definition ISAM2Clique.h:80
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique(const ISAM2Clique &other)
Copy constructor, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:58
gtsam::ISAM2Clique::operator=
ISAM2Clique & operator=(const ISAM2Clique &other)
Assignment operator, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:65
gtsam::ISAM2Clique::cachedFactor
Base::FactorType::shared_ptr & cachedFactor()
Access the cached factor.
Definition ISAM2Clique.h:77
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique()
Default constructor.
Definition ISAM2Clique.h:53
VectorValues
The Factor::error simply extracts the.