ISAM2Clique.cpp
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 
22 
23 #include <stack>
24 #include <utility>
25 
26 using namespace std;
27 
28 namespace gtsam {
29 
30 // Instantiate base class
31 template class BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph>;
32 
33 /* ************************************************************************* */
34 void ISAM2Clique::setEliminationResult(
35  const FactorGraphType::EliminationResult& eliminationResult) {
36  conditional_ = eliminationResult.first;
37  cachedFactor_ = eliminationResult.second;
38  // Compute gradient contribution
39  gradientContribution_.resize(conditional_->cols() - 1);
40  // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed
41  // reasons
42  gradientContribution_ << -conditional_->R().transpose() *
43  conditional_->d(),
44  -conditional_->S().transpose() * conditional_->d();
45 }
46 
47 /* ************************************************************************* */
48 bool ISAM2Clique::equals(const This& other, double tol) const {
49  return Base::equals(other) &&
50  ((!cachedFactor_ && !other.cachedFactor_) ||
51  (cachedFactor_ && other.cachedFactor_ &&
52  cachedFactor_->equals(*other.cachedFactor_, tol)));
53 }
54 
55 /* ************************************************************************* */
56 void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const {
57  Base::print(s, formatter);
58  if (cachedFactor_)
59  cachedFactor_->print(s + "Cached: ", formatter);
60  else
61  cout << s << "Cached empty" << endl;
62  if (gradientContribution_.rows() != 0)
63  gtsam::print(gradientContribution_, "Gradient contribution: ");
64 }
65 
66 /* ************************************************************************* */
67 bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const {
68  // if none of the variables in this clique (frontal and separator!) changed
69  // significantly, then by the running intersection property, none of the
70  // cliques in the children need to be processed
71 
72  // Are any clique variables part of the tree that has been redone?
73  bool dirty = replaced.exists(conditional_->frontals().front());
74 #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
75  for (Key frontal : conditional_->frontals()) {
76  assert(dirty == replaced.exists(frontal));
77  }
78 #endif
79 
80  // If not, then has one of the separator variables changed significantly?
81  if (!dirty) {
82  for (Key parent : conditional_->parents()) {
83  if (changed.exists(parent)) {
84  dirty = true;
85  break;
86  }
87  }
88  }
89  return dirty;
90 }
91 
92 /* ************************************************************************* */
97 void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const {
98 #ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
99  // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst,
100  // potentially refactor
101 
102  // Create solution part pointers if necessary and possible - necessary if
103  // solnPointers_ is empty, and possible if either we're a root, or we have
104  // a parent with valid solnPointers_.
105  ISAM2Clique::shared_ptr parent = parent_.lock();
106  if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) {
107  for (Key frontal : conditional_->frontals())
108  solnPointers_.emplace(frontal, delta->find(frontal));
109  for (Key parentKey : conditional_->parents()) {
110  assert(parent->solnPointers_.exists(parentKey));
111  solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey));
112  }
113  }
114 
115  // See if we can use solution part pointers - we can if they either
116  // already existed or were created above.
117  if (!solnPointers_.empty()) {
118  GaussianConditional& c = *conditional_;
119  // Solve matrix
120  Vector xS;
121  {
122  // Count dimensions of vector
123  DenseIndex dim = 0;
125  parentPointers.reserve(conditional_->nrParents());
126  for (Key parent : conditional_->parents()) {
127  parentPointers.push_back(solnPointers_.at(parent));
128  dim += parentPointers.back()->second.size();
129  }
130 
131  // Fill parent vector
132  xS.resize(dim);
133  DenseIndex vectorPos = 0;
134  for (const VectorValues::const_iterator& parentPointer : parentPointers) {
135  const Vector& parentVector = parentPointer->second;
136  xS.block(vectorPos, 0, parentVector.size(), 1) =
137  parentVector.block(0, 0, parentVector.size(), 1);
138  vectorPos += parentVector.size();
139  }
140  }
141 
142  // NOTE(gareth): We can no longer write: xS = b - S * xS
143  // This is because Eigen (as of 3.3) no longer evaluates S * xS into
144  // a temporary, and the operation trashes valus in xS.
145  // See: http://eigen.tuxfamily.org/index.php?title=3.3
146  const Vector rhs = c.getb() - c.S() * xS;
147  const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
148 
149  // Check for indeterminant solution
150  if (solution.hasNaN())
151  throw IndeterminantLinearSystemException(c.keys().front());
152 
153  // Insert solution into a VectorValues
154  DenseIndex vectorPosition = 0;
156  frontal != c.endFrontals(); ++frontal) {
157  solnPointers_.at(*frontal)->second =
158  solution.segment(vectorPosition, c.getDim(frontal));
159  vectorPosition += c.getDim(frontal);
160  }
161  } else {
162  // Just call plain solve because we couldn't use solution pointers.
163  delta->update(conditional_->solve(*delta));
164  }
165 #else
166  delta->update(conditional_->solve(*delta));
167 #endif
168 }
169 
170 /* ************************************************************************* */
171 bool ISAM2Clique::valuesChanged(const KeySet& replaced,
172  const Vector& originalValues,
173  const VectorValues& delta,
174  double threshold) const {
175  auto frontals = conditional_->frontals();
176  if (replaced.exists(frontals.front())) return true;
177  Vector diff = originalValues - delta.vector(frontals);
178  return diff.lpNorm<Eigen::Infinity>() >= threshold;
179 }
180 
181 /* ************************************************************************* */
183 void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const {
184  for (Key frontal : conditional_->frontals()) {
185  changed->insert(frontal);
186  }
187 }
188 
189 /* ************************************************************************* */
190 void ISAM2Clique::restoreFromOriginals(const Vector& originalValues,
191  VectorValues* delta) const {
192  size_t pos = 0;
193  for (Key frontal : conditional_->frontals()) {
194  auto v = delta->at(frontal);
195  v = originalValues.segment(pos, v.size());
196  pos += v.size();
197  }
198 }
199 
200 /* ************************************************************************* */
201 // Note: not being used right now in favor of non-recursive version below.
202 void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold,
203  KeySet* changed, VectorValues* delta,
204  size_t* count) const {
205  if (isDirty(replaced, *changed)) {
206  // Temporary copy of the original values, to check how much they change
207  auto originalValues = delta->vector(conditional_->frontals());
208 
209  // Back-substitute
210  fastBackSubstitute(delta);
211  count += conditional_->nrFrontals();
212 
213  if (valuesChanged(replaced, originalValues, *delta, threshold)) {
214  markFrontalsAsChanged(changed);
215  } else {
216  restoreFromOriginals(originalValues, delta);
217  }
218 
219  // Recurse to children
220  for (const auto& child : children) {
221  child->optimizeWildfire(replaced, threshold, changed, delta, count);
222  }
223  }
224 }
225 
226 size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
227  const KeySet& keys, VectorValues* delta) {
228  KeySet changed;
229  size_t count = 0;
230  // starting from the root, call optimize on each conditional
231  if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count);
232  return count;
233 }
234 
235 /* ************************************************************************* */
236 bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold,
237  KeySet* changed, VectorValues* delta,
238  size_t* count) const {
239  // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst,
240  // potentially refactor
241  bool dirty = isDirty(replaced, *changed);
242  if (dirty) {
243  // Temporary copy of the original values, to check how much they change
244  auto originalValues = delta->vector(conditional_->frontals());
245 
246  // Back-substitute
247  fastBackSubstitute(delta);
248  *count += conditional_->nrFrontals();
249 
250  if (valuesChanged(replaced, originalValues, *delta, threshold)) {
251  markFrontalsAsChanged(changed);
252  } else {
253  restoreFromOriginals(originalValues, delta);
254  }
255  }
256 
257  return dirty;
258 }
259 
261  double threshold, const KeySet& keys,
262  VectorValues* delta) {
263  KeySet changed;
264  size_t count = 0;
265 
266  if (root) {
267  std::stack<ISAM2Clique::shared_ptr> travStack;
268  travStack.push(root);
269  ISAM2Clique::shared_ptr currentNode = root;
270  while (!travStack.empty()) {
271  currentNode = travStack.top();
272  travStack.pop();
273  bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed,
274  delta, &count);
275  if (dirty) {
276  for (const auto& child : currentNode->children) {
277  travStack.push(child);
278  }
279  }
280  }
281  }
282 
283  return count;
284 }
285 
286 /* ************************************************************************* */
287 void ISAM2Clique::nnz_internal(size_t* result) const {
288  size_t dimR = conditional_->rows();
289  size_t dimSep = conditional_->S().cols();
290  *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR;
291  // traverse the children
292  for (const auto& child : children) {
293  child->nnz_internal(result);
294  }
295 }
296 
297 /* ************************************************************************* */
298 size_t ISAM2Clique::calculate_nnz() const {
299  size_t result = 0;
300  nnz_internal(&result);
301  return result;
302 }
303 
304 /* ************************************************************************* */
305 void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const {
306  static const bool debug = false;
307  // does the separator contain any of the variables?
308  bool found = false;
309  for (Key key : conditional_->parents()) {
310  if (markedMask.exists(key)) {
311  found = true;
312  break;
313  }
314  }
315  if (found) {
316  // then add this clique
317  keys->insert(conditional_->beginFrontals(), conditional_->endFrontals());
318  if (debug) print("Key(s) marked in clique ");
319  if (debug) cout << "so marking key " << conditional_->front() << endl;
320  }
321  for (const auto& child : children) {
322  child->findAll(markedMask, keys);
323  }
324 }
325 
326 /* ************************************************************************* */
327 void ISAM2Clique::addGradientAtZero(VectorValues* g) const {
328  // Loop through variables in each clique, adding contributions
329  DenseIndex position = 0;
330  for (auto it = conditional_->begin(); it != conditional_->end(); ++it) {
331  const DenseIndex dim = conditional_->getDim(it);
332  const Vector contribution = gradientContribution_.segment(position, dim);
333  auto [values_it, success] = g->tryInsert(*it, contribution);
334  if (!success) values_it->second += contribution;
335  position += dim;
336  }
337 
338  // Recursively add contributions from children
339  for (const auto& child : children) {
340  child->addGradientAtZero(g);
341  }
342 }
343 
344 /* ************************************************************************* */
345 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
iterator find(Key j)
Definition: VectorValues.h:245
FACTOR::const_iterator endFrontals() const
Definition: Conditional.h:182
Specialized iSAM2 Clique.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
bool exists(const VALUE &e) const
Definition: FastSet.h:98
Base class for cliques of a BayesTree.
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Vector & at(Key j)
Definition: VectorValues.h:139
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
void g(const string &key, int i)
Definition: testBTree.cpp:41
static constexpr bool debug
const KeyFormatter & formatter
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
Definition: VectorValues.h:209
size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
FACTOR::const_iterator beginFrontals() const
Definition: Conditional.h:179
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
Factor Graph Values.
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)
Array< int, Dynamic, 1 > v
std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > > EliminationResult
Templated algorithms that are used in multiple places in linear.
const constBVector getb() const
RealScalar s
DenseIndex getDim(const_iterator variable) const override
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
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
traits
Definition: chartTesting.h:28
Vector vector() const
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
VectorValues & update(const VectorValues &values)
const G double tol
Definition: Group.h:86
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
const KeyVector keys
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const int Infinity
Definition: Constants.h:36


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