gtsam  4.1.0
gtsam
ISAM2Clique.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 // \callgraph
19 
20 #pragma once
21 
23 #include <gtsam/inference/Key.h>
27 #include <string>
28 
29 namespace gtsam {
30 
36 class GTSAM_EXPORT ISAM2Clique
37  : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
38  public:
39  typedef ISAM2Clique This;
41  typedef boost::shared_ptr<This> shared_ptr;
42  typedef boost::weak_ptr<This> weak_ptr;
44  typedef ConditionalType::shared_ptr sharedConditional;
45 
46  Base::FactorType::shared_ptr cachedFactor_;
47  Vector gradientContribution_;
48 #ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
49  mutable FastMap<Key, VectorValues::iterator> solnPointers_;
50 #endif
51 
53  ISAM2Clique() : Base() {}
54  virtual ~ISAM2Clique() = default;
55 
58  ISAM2Clique(const ISAM2Clique& other)
59  : Base(other),
60  cachedFactor_(other.cachedFactor_),
61  gradientContribution_(other.gradientContribution_) {}
62 
66  Base::operator=(other);
67  cachedFactor_ = other.cachedFactor_;
68  gradientContribution_ = other.gradientContribution_;
69  return *this;
70  }
71 
73  void setEliminationResult(
74  const FactorGraphType::EliminationResult& eliminationResult);
75 
77  Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
78 
80  const Vector& gradientContribution() const { return gradientContribution_; }
81 
83  void addGradientAtZero(VectorValues* g) const;
84 
85  bool equals(const This& other, double tol = 1e-9) const;
86 
88  void print(const std::string& s = "",
89  const KeyFormatter& formatter = DefaultKeyFormatter) const override;
90 
91  void optimizeWildfire(const KeySet& replaced, double threshold,
92  KeySet* changed, VectorValues* delta,
93  size_t* count) const;
94 
95  bool optimizeWildfireNode(const KeySet& replaced, double threshold,
96  KeySet* changed, VectorValues* delta,
97  size_t* count) const;
98 
103  void nnz_internal(size_t* result) const;
104  size_t calculate_nnz() const;
105 
121  void findAll(const KeySet& markedMask, KeySet* keys) const;
122 
123  private:
128  bool isDirty(const KeySet& replaced, const KeySet& changed) const;
129 
134  void fastBackSubstitute(VectorValues* delta) const;
135 
136  /*
137  * Check whether the values changed above a threshold, or always true if the
138  * clique was replaced.
139  */
140  bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
141  const VectorValues& delta, double threshold) const;
142 
144  void markFrontalsAsChanged(KeySet* changed) const;
145 
147  void restoreFromOriginals(const Vector& originalValues,
148  VectorValues* delta) const;
149 
151  friend class boost::serialization::access;
152  template <class ARCHIVE>
153  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
154  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
155  ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
156  ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
157  }
158 }; // \struct ISAM2Clique
159 
171 size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
172  const KeySet& replaced, VectorValues* delta);
173 
174 size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
175  double threshold, const KeySet& replaced,
176  VectorValues* delta);
177 
178 } // namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::GaussianConditional
A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,...
Definition: GaussianConditional.h:39
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::cachedFactor
Base::FactorType::shared_ptr & cachedFactor()
Access the cached factor.
Definition: ISAM2Clique.h:77
gtsam::ISAM2Clique::gradientContribution
const Vector & gradientContribution() const
Access the gradient contribution.
Definition: ISAM2Clique.h:80
GaussianConditional.h
Conditional Gaussian Base class.
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition: BayesTreeCliqueBase.h:50
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition: ISAM2Clique.h:37
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
Key.h
gtsam::FastSet< Key >
gtsam::KeyFormatter
boost::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
Definition: FastMap.h:38
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
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::ISAM2Clique::ISAM2Clique
ISAM2Clique()
Default constructor.
Definition: ISAM2Clique.h:53
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