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 std::shared_ptr<This> shared_ptr;
42  typedef std::weak_ptr<This> weak_ptr;
45 
48 #ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
49  mutable FastMap<Key, VectorValues::iterator> solnPointers_;
50 #endif
51 
53  ISAM2Clique() : Base() {}
54 
58  : Base(other),
59  cachedFactor_(other.cachedFactor_),
60  gradientContribution_(other.gradientContribution_) {}
61 
65  Base::operator=(other);
66  cachedFactor_ = other.cachedFactor_;
67  gradientContribution_ = other.gradientContribution_;
68  return *this;
69  }
70 
72  void setEliminationResult(
73  const FactorGraphType::EliminationResult& eliminationResult);
74 
76  Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
77 
79  const Vector& gradientContribution() const { return gradientContribution_; }
80 
82  void addGradientAtZero(VectorValues* g) const;
83 
84  bool equals(const This& other, double tol = 1e-9) const;
85 
87  void print(const std::string& s = "",
88  const KeyFormatter& formatter = DefaultKeyFormatter) const override;
89 
90  void optimizeWildfire(const KeySet& replaced, double threshold,
91  KeySet* changed, VectorValues* delta,
92  size_t* count) const;
93 
94  bool optimizeWildfireNode(const KeySet& replaced, double threshold,
95  KeySet* changed, VectorValues* delta,
96  size_t* count) const;
97 
102  void nnz_internal(size_t* result) const;
103  size_t calculate_nnz() const;
104 
120  void findAll(const KeySet& markedMask, KeySet* keys) const;
121 
122  private:
127  bool isDirty(const KeySet& replaced, const KeySet& changed) const;
128 
133  void fastBackSubstitute(VectorValues* delta) const;
134 
135  /*
136  * Check whether the values changed above a threshold, or always true if the
137  * clique was replaced.
138  */
139  bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
140  const VectorValues& delta, double threshold) const;
141 
143  void markFrontalsAsChanged(KeySet* changed) const;
144 
146  void restoreFromOriginals(const Vector& originalValues,
147  VectorValues* delta) const;
148 
149 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
150 
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 #endif
159 }; // \struct ISAM2Clique
160 
172 size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
173  const KeySet& replaced, VectorValues* delta);
174 
176  double threshold, const KeySet& replaced,
177  VectorValues* delta);
178 
179 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::string serialize(const T &input)
serializes to a string
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
void g(const string &key, int i)
Definition: testBTree.cpp:41
const KeyFormatter & formatter
size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
Base::FactorType::shared_ptr cachedFactor_
Definition: ISAM2Clique.h:46
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
GaussianConditional ConditionalType
Definition: ISAM2Clique.h:43
ISAM2Clique()
Default constructor.
Definition: ISAM2Clique.h:53
std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > > EliminationResult
Vector gradientContribution_
Definition: ISAM2Clique.h:47
std::shared_ptr< This > shared_ptr
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
ISAM2Clique(const ISAM2Clique &other)
Definition: ISAM2Clique.h:57
Conditional Gaussian Base class.
Linear Factor Graph where all factors are Gaussians.
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
Base::FactorType::shared_ptr & cachedFactor()
Definition: ISAM2Clique.h:76
traits
Definition: chartTesting.h:28
std::weak_ptr< This > weak_ptr
Definition: ISAM2Clique.h:42
BayesTreeCliqueBase< This, GaussianFactorGraph > Base
Definition: ISAM2Clique.h:40
Chordal Bayes Net, the result of eliminating a factor graph.
ISAM2Clique & operator=(const ISAM2Clique &other)
Definition: ISAM2Clique.h:64
const G double tol
Definition: Group.h:86
const KeyVector keys
ConditionalType::shared_ptr sharedConditional
Definition: ISAM2Clique.h:44
Base class for cliques of a BayesTree.
ISAM2Clique This
Definition: ISAM2Clique.h:39
const Vector & gradientContribution() const
Access the gradient contribution.
Definition: ISAM2Clique.h:79


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:26