gtsam  4.1.0
gtsam
ISAM2-impl.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 
19 #pragma once
20 
21 #include <gtsam/nonlinear/ISAM2.h>
23 
24 #include <gtsam/base/debug.h>
25 #include <gtsam/inference/JunctionTree-inst.h> // We need the inst file because we'll make a special JT templated on ISAM2
26 #include <gtsam/inference/Symbol.h>
30 
31 #include <boost/range/adaptors.hpp>
32 #include <boost/range/algorithm/copy.hpp>
33 namespace br {
34 using namespace boost::range;
35 using namespace boost::adaptors;
36 } // namespace br
37 
38 #include <algorithm>
39 #include <limits>
40 #include <string>
41 #include <utility>
42 
43 namespace gtsam {
44 
45 /* ************************************************************************* */
46 // Special BayesTree class that uses ISAM2 cliques - this is the result of
47 // reeliminating ISAM2 subtrees.
48 class ISAM2BayesTree : public ISAM2::Base {
49  public:
50  typedef ISAM2::Base Base;
51  typedef ISAM2BayesTree This;
52  typedef boost::shared_ptr<This> shared_ptr;
53 
54  ISAM2BayesTree() {}
55 };
56 
57 /* ************************************************************************* */
58 // Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
59 // reeliminating ISAM2 subtrees.
61  : public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
62  public:
64  typedef ISAM2JunctionTree This;
65  typedef boost::shared_ptr<This> shared_ptr;
66 
67  explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
68  : Base(eliminationTree) {}
69 };
70 
71 /* ************************************************************************* */
72 struct GTSAM_EXPORT DeltaImpl {
73  struct GTSAM_EXPORT PartialSolveResult {
74  ISAM2::sharedClique bayesTree;
75  };
76 
77  struct GTSAM_EXPORT ReorderingMode {
78  size_t nFullSystemVars;
79  enum { /*AS_ADDED,*/ COLAMD } algorithm;
80  enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
81  boost::optional<FastMap<Key, int> > constrainedKeys;
82  };
83 
87  static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
88  const KeySet& replacedKeys,
89  double wildfireThreshold,
90  VectorValues* delta);
91 
96  static size_t UpdateRgProd(const ISAM2::Roots& roots,
97  const KeySet& replacedKeys,
98  const VectorValues& gradAtZero,
99  VectorValues* RgProd);
100 
104  static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
105  const VectorValues& RgProd);
106 };
107 
108 /* ************************************************************************* */
114 struct GTSAM_EXPORT UpdateImpl {
115  const ISAM2Params& params_;
116  const ISAM2UpdateParams& updateParams_;
117  UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams)
118  : params_(params), updateParams_(updateParams) {}
119 
120  // Provide some debugging information at the start of update
121  static void LogStartingUpdate(const NonlinearFactorGraph& newFactors,
122  const ISAM2& isam2) {
123  gttic(pushBackFactors);
124  const bool debug = ISDEBUG("ISAM2 update");
125  const bool verbose = ISDEBUG("ISAM2 update verbose");
126 
127  if (verbose) {
128  std::cout << "ISAM2::update\n";
129  isam2.print("ISAM2: ");
130  }
131 
132  if (debug || verbose) {
133  newFactors.print("The new factors are: ");
134  }
135  }
136 
137  // Check relinearization if we're at the nth step, or we are using a looser
138  // loop relinerization threshold.
139  bool relinarizationNeeded(size_t update_count) const {
140  return updateParams_.force_relinearize ||
141  (params_.enableRelinearization &&
142  update_count % params_.relinearizeSkip == 0);
143  }
144 
145  // Add any new factors \Factors:=\Factors\cup\Factors'.
146  void pushBackFactors(const NonlinearFactorGraph& newFactors,
147  NonlinearFactorGraph* nonlinearFactors,
148  GaussianFactorGraph* linearFactors,
149  VariableIndex* variableIndex,
150  FactorIndices* newFactorsIndices,
151  KeySet* keysWithRemovedFactors) const {
152  gttic(pushBackFactors);
153 
154  // Perform the first part of the bookkeeping updates for adding new factors.
155  // Adds them to the complete list of nonlinear factors, and populates the
156  // list of new factor indices, both optionally finding and reusing empty
157  // factor slots.
158  *newFactorsIndices = nonlinearFactors->add_factors(
159  newFactors, params_.findUnusedFactorSlots);
160 
161  // Remove the removed factors
162  NonlinearFactorGraph removedFactors;
163  removedFactors.reserve(updateParams_.removeFactorIndices.size());
164  for (const auto index : updateParams_.removeFactorIndices) {
165  removedFactors.push_back(nonlinearFactors->at(index));
166  nonlinearFactors->remove(index);
167  if (params_.cacheLinearizedFactors) linearFactors->remove(index);
168  }
169 
170  // Remove removed factors from the variable index so we do not attempt to
171  // relinearize them
172  variableIndex->remove(updateParams_.removeFactorIndices.begin(),
173  updateParams_.removeFactorIndices.end(),
174  removedFactors);
175  *keysWithRemovedFactors = removedFactors.keys();
176  }
177 
178  // Get keys from removed factors and new factors, and compute unused keys,
179  // i.e., keys that are empty now and do not appear in the new factors.
180  void computeUnusedKeys(const NonlinearFactorGraph& newFactors,
181  const VariableIndex& variableIndex,
182  const KeySet& keysWithRemovedFactors,
183  KeySet* unusedKeys) const {
184  gttic(computeUnusedKeys);
185  KeySet removedAndEmpty;
186  for (Key key : keysWithRemovedFactors) {
187  if (variableIndex.empty(key))
188  removedAndEmpty.insert(removedAndEmpty.end(), key);
189  }
190  KeySet newFactorSymbKeys = newFactors.keys();
191  std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
192  newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
193  std::inserter(*unusedKeys, unusedKeys->end()));
194  }
195 
196  // Calculate nonlinear error
197  void error(const NonlinearFactorGraph& nonlinearFactors,
198  const Values& estimate, boost::optional<double>* result) const {
199  gttic(error);
200  result->reset(nonlinearFactors.error(estimate));
201  }
202 
203  // Mark linear update
204  void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
205  const NonlinearFactorGraph& nonlinearFactors,
206  const KeySet& keysWithRemovedFactors,
207  KeySet* markedKeys) const {
208  gttic(gatherInvolvedKeys);
209  *markedKeys = newFactors.keys(); // Get keys from new factors
210  // Also mark keys involved in removed factors
211  markedKeys->insert(keysWithRemovedFactors.begin(),
212  keysWithRemovedFactors.end());
213 
214  // Also mark any provided extra re-eliminate keys
215  if (updateParams_.extraReelimKeys) {
216  for (Key key : *updateParams_.extraReelimKeys) {
217  markedKeys->insert(key);
218  }
219  }
220 
221  // Also, keys that were not observed in existing factors, but whose affected
222  // keys have been extended now (e.g. smart factors)
223  if (updateParams_.newAffectedKeys) {
224  for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
225  const auto factorIdx = factorAddedKeys.first;
226  const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys();
227  markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
228  }
229  }
230  }
231 
232  // Update detail, unused, and observed keys from markedKeys
233  void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
234  gttic(updateKeys);
235  // Observed keys for detailed results
236  if (result->detail && params_.enableDetailedResults) {
237  for (Key key : markedKeys) {
238  result->detail->variableStatus[key].isObserved = true;
239  }
240  }
241 
242  for (Key index : markedKeys) {
243  // Only add if not unused
244  if (result->unusedKeys.find(index) == result->unusedKeys.end())
245  // Make a copy of these, as we'll soon add to them
246  result->observedKeys.push_back(index);
247  }
248  }
249 
250  static void CheckRelinearizationRecursiveMap(
251  const FastMap<char, Vector>& thresholds, const VectorValues& delta,
252  const ISAM2::sharedClique& clique, KeySet* relinKeys) {
253  // Check the current clique for relinearization
254  bool relinearize = false;
255  for (Key var : *clique->conditional()) {
256  // Find the threshold for this variable type
257  const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
258 
259  const Vector& deltaVar = delta[var];
260 
261  // Verify the threshold vector matches the actual variable size
262  if (threshold.rows() != deltaVar.rows())
263  throw std::invalid_argument(
264  "Relinearization threshold vector dimensionality for '" +
265  std::string(1, Symbol(var).chr()) +
266  "' passed into iSAM2 parameters does not match actual variable "
267  "dimensionality.");
268 
269  // Check for relinearization
270  if ((deltaVar.array().abs() > threshold.array()).any()) {
271  relinKeys->insert(var);
272  relinearize = true;
273  }
274  }
275 
276  // If this node was relinearized, also check its children
277  if (relinearize) {
278  for (const ISAM2::sharedClique& child : clique->children) {
279  CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
280  }
281  }
282  }
283 
284  static void CheckRelinearizationRecursiveDouble(
285  double threshold, const VectorValues& delta,
286  const ISAM2::sharedClique& clique, KeySet* relinKeys) {
287  // Check the current clique for relinearization
288  bool relinearize = false;
289  for (Key var : *clique->conditional()) {
290  double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
291  if (maxDelta >= threshold) {
292  relinKeys->insert(var);
293  relinearize = true;
294  }
295  }
296 
297  // If this node was relinearized, also check its children
298  if (relinearize) {
299  for (const ISAM2::sharedClique& child : clique->children) {
300  CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
301  }
302  }
303  }
304 
319  const ISAM2::Roots& roots, const VectorValues& delta,
320  const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
321  KeySet relinKeys;
322  for (const ISAM2::sharedClique& root : roots) {
323  if (relinearizeThreshold.type() == typeid(double))
324  CheckRelinearizationRecursiveDouble(
325  boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
326  else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
327  CheckRelinearizationRecursiveMap(
328  boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
329  root, &relinKeys);
330  }
331  return relinKeys;
332  }
333 
346  const VectorValues& delta,
347  const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
348  KeySet relinKeys;
349 
350  if (const double* threshold = boost::get<double>(&relinearizeThreshold)) {
351  for (const VectorValues::KeyValuePair& key_delta : delta) {
352  double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
353  if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
354  }
355  } else if (const FastMap<char, Vector>* thresholds =
356  boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) {
357  for (const VectorValues::KeyValuePair& key_delta : delta) {
358  const Vector& threshold =
359  thresholds->find(Symbol(key_delta.first).chr())->second;
360  if (threshold.rows() != key_delta.second.rows())
361  throw std::invalid_argument(
362  "Relinearization threshold vector dimensionality for '" +
363  std::string(1, Symbol(key_delta.first).chr()) +
364  "' passed into iSAM2 parameters does not match actual variable "
365  "dimensionality.");
366  if ((key_delta.second.array().abs() > threshold.array()).any())
367  relinKeys.insert(key_delta.first);
368  }
369  }
370 
371  return relinKeys;
372  }
373 
374  // Mark keys in \Delta above threshold \beta:
375  KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
376  const VectorValues& delta,
377  const KeySet& fixedVariables,
378  KeySet* markedKeys) const {
379  gttic(gatherRelinearizeKeys);
380  // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
381  KeySet relinKeys =
382  params_.enablePartialRelinearizationCheck
383  ? CheckRelinearizationPartial(roots, delta,
384  params_.relinearizeThreshold)
385  : CheckRelinearizationFull(delta, params_.relinearizeThreshold);
386  if (updateParams_.forceFullSolve)
387  relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging
388 
389  // Remove from relinKeys any keys whose linearization points are fixed
390  for (Key key : fixedVariables) {
391  relinKeys.erase(key);
392  }
393  if (updateParams_.noRelinKeys) {
394  for (Key key : *updateParams_.noRelinKeys) {
395  relinKeys.erase(key);
396  }
397  }
398 
399  // Add the variables being relinearized to the marked keys
400  markedKeys->insert(relinKeys.begin(), relinKeys.end());
401  return relinKeys;
402  }
403 
404  // Record relinerization threshold keys in detailed results
405  void recordRelinearizeDetail(const KeySet& relinKeys,
406  ISAM2Result::DetailedResults* detail) const {
407  if (detail && params_.enableDetailedResults) {
408  for (Key key : relinKeys) {
409  detail->variableStatus[key].isAboveRelinThreshold = true;
410  detail->variableStatus[key].isRelinearized = true;
411  }
412  }
413  }
414 
415  // Mark all cliques that involve marked variables \Theta_{J} and all
416  // their ancestors.
417  void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys,
418  KeySet* markedKeys,
419  ISAM2Result::DetailedResults* detail) const {
420  gttic(findFluid);
421  for (const auto& root : roots)
422  // add other cliques that have the marked ones in the separator
423  root->findAll(relinKeys, markedKeys);
424 
425  // Relinearization-involved keys for detailed results
426  if (detail && params_.enableDetailedResults) {
427  KeySet involvedRelinKeys;
428  for (const auto& root : roots)
429  root->findAll(relinKeys, &involvedRelinKeys);
430  for (Key key : involvedRelinKeys) {
431  if (!detail->variableStatus[key].isAboveRelinThreshold) {
432  detail->variableStatus[key].isRelinearizeInvolved = true;
433  detail->variableStatus[key].isRelinearized = true;
434  }
435  }
436  }
437  }
438 
444  static void ExpmapMasked(const VectorValues& delta, const KeySet& mask,
445  Values* theta) {
446  gttic(ExpmapMasked);
447  assert(theta->size() == delta.size());
448  Values::iterator key_value;
450 #ifdef GTSAM_USE_TBB
451  for (key_value = theta->begin(); key_value != theta->end(); ++key_value) {
452  key_delta = delta.find(key_value->key);
453 #else
454  for (key_value = theta->begin(), key_delta = delta.begin();
455  key_value != theta->end(); ++key_value, ++key_delta) {
456  assert(key_value->key == key_delta->first);
457 #endif
458  Key var = key_value->key;
459  assert(static_cast<size_t>(delta[var].size()) == key_value->value.dim());
460  assert(delta[var].allFinite());
461  if (mask.exists(var)) {
462  Value* retracted = key_value->value.retract_(delta[var]);
463  key_value->value = *retracted;
464  retracted->deallocate_();
465  }
466  }
467  }
468 
469  // Linearize new factors
470  void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
471  const Values& theta, size_t numNonlinearFactors,
472  const FactorIndices& newFactorsIndices,
473  GaussianFactorGraph* linearFactors) const {
474  gttic(linearizeNewFactors);
475  auto linearized = newFactors.linearize(theta);
476  if (params_.findUnusedFactorSlots) {
477  linearFactors->resize(numNonlinearFactors);
478  for (size_t i = 0; i < newFactors.size(); ++i)
479  (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i];
480  } else {
481  linearFactors->push_back(*linearized);
482  }
483  assert(linearFactors->size() == numNonlinearFactors);
484  }
485 
486  void augmentVariableIndex(const NonlinearFactorGraph& newFactors,
487  const FactorIndices& newFactorsIndices,
488  VariableIndex* variableIndex) const {
489  gttic(augmentVariableIndex);
490  // Augment the variable index with the new factors
491  if (params_.findUnusedFactorSlots)
492  variableIndex->augment(newFactors, newFactorsIndices);
493  else
494  variableIndex->augment(newFactors);
495 
496  // Augment it with existing factors which now affect to more variables:
497  if (updateParams_.newAffectedKeys) {
498  for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
499  const auto factorIdx = factorAddedKeys.first;
500  variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second);
501  }
502  }
503  }
504 
505  static void LogRecalculateKeys(const ISAM2Result& result) {
506  const bool debug = ISDEBUG("ISAM2 recalculate");
507 
508  if (debug) {
509  std::cout << "markedKeys: ";
510  for (const Key key : result.markedKeys) {
511  std::cout << key << " ";
512  }
513  std::cout << std::endl;
514  std::cout << "observedKeys: ";
515  for (const Key key : result.observedKeys) {
516  std::cout << key << " ";
517  }
518  std::cout << std::endl;
519  }
520  }
521 
522  static FactorIndexSet GetAffectedFactors(const KeyList& keys,
523  const VariableIndex& variableIndex) {
524  gttic(GetAffectedFactors);
525  FactorIndexSet indices;
526  for (const Key key : keys) {
527  const FactorIndices& factors(variableIndex[key]);
528  indices.insert(factors.begin(), factors.end());
529  }
530  return indices;
531  }
532 
533  // find intermediate (linearized) factors from cache that are passed into
534  // the affected area
535  static GaussianFactorGraph GetCachedBoundaryFactors(
536  const ISAM2::Cliques& orphans) {
537  GaussianFactorGraph cachedBoundary;
538 
539  for (const auto& orphan : orphans) {
540  // retrieve the cached factor and add to boundary
541  cachedBoundary.push_back(orphan->cachedFactor());
542  }
543 
544  return cachedBoundary;
545  }
546 };
547 
548 } // namespace gtsam
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::ISAM2UpdateParams::newAffectedKeys
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition: ISAM2UpdateParams.h:66
gtsam::BayesTree< ISAM2Clique >
gtsam::Value
This is the base class for any type to be stored in Values.
Definition: Value.h:36
gtsam::UpdateImpl
Implementation functions for update method All of the methods below have clear inputs and outputs,...
Definition: ISAM2-impl.h:114
VariableIndex.h
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::ISAM2UpdateParams::force_relinearize
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition: ISAM2UpdateParams.h:54
gtsam::ISAM2Params
Definition: ISAM2Params.h:135
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::VectorValues::size
size_t size() const
Number of variables stored.
Definition: VectorValues.h:125
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition: FactorGraph.h:311
gtsam::Values::size
size_t size() const
The number of variables in this config.
Definition: Values.h:236
gtsam::ISAM2BayesTree
Definition: ISAM2-impl.h:48
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:162
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:102
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:302
gtsam::UpdateImpl::ExpmapMasked
static void ExpmapMasked(const VectorValues &delta, const KeySet &mask, Values *theta)
Apply expmap to the given values, but only for indices appearing in mask.
Definition: ISAM2-impl.h:444
gtsam::ISAM2UpdateParams::removeFactorIndices
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition: ISAM2UpdateParams.h:36
ISAM2Result.h
Class that stores detailed iSAM2 result.
GaussianEliminationTree.h
gtsam::BayesTree< ISAM2Clique >::Roots
FastVector< sharedClique > Roots
Root cliques.
Definition: BayesTree.h:93
gtsam::ISAM2UpdateParams::extraReelimKeys
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition: ISAM2UpdateParams.h:49
gtsam::VariableIndex::empty
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition: VariableIndex.h:98
gtsam::DeltaImpl
Definition: ISAM2-impl.h:72
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ISAM2UpdateParams::noRelinKeys
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition: ISAM2UpdateParams.h:44
gtsam::FastList
Definition: FastList.h:38
gtsam::UpdateImpl::CheckRelinearizationPartial
static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition: ISAM2-impl.h:318
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
JunctionTree-inst.h
The junction tree, template bodies.
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ISAM2UpdateParams::forceFullSolve
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition: ISAM2UpdateParams.h:71
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
unnormalized error, in the most common case
Definition: NonlinearFactorGraph.cpp:260
gtsam::DeltaImpl::ReorderingMode
Definition: ISAM2-impl.h:77
gtsam::ISAM2JunctionTree
Definition: ISAM2-impl.h:61
gtsam::ISAM2UpdateParams
Definition: ISAM2UpdateParams.h:32
gtsam::FastSet< Key >
gtsam::VectorValues::begin
iterator begin()
Iterator over variables.
Definition: VectorValues.h:226
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition: FactorGraph-inst.h:100
gtsam::VariableIndex::augmentExistingFactor
void augmentExistingFactor(const FactorIndex factorIndex, const KeySet &newKeys)
Augment the variable index after an existing factor now affects to more variable Keys.
Definition: VariableIndex.cpp:59
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
gtsam::DeltaImpl::PartialSolveResult
Definition: ISAM2-impl.h:73
debug.h
Global debugging flags.
gtsam::FactorGraph::resize
void resize(size_t size)
Directly resize the number of factors in the graph.
Definition: FactorGraph.h:354
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:76
gtsam::FastMap
Definition: FastMap.h:38
gtsam::FastSet::exists
bool exists(const VALUE &e) const
Handy 'exists' function.
Definition: FastSet.h:98
gtsam::GaussianEliminationTree
Definition: GaussianEliminationTree.h:29
Symbol.h
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ISAM2Result
Definition: ISAM2Result.h:41
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition: ISAM2Params.h:140
gtsam::Values::iterator
boost::transform_iterator< boost::function1< KeyValuePair, const KeyValuePtrPair & >, KeyValueMap::iterator > iterator
Mutable forward iterator, with value type KeyValuePair.
Definition: Values.h:120
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: NonlinearFactorGraph.cpp:52
gtsam::Symbol::chr
unsigned char chr() const
Retrieve key character.
Definition: Symbol.h:71
gtsam::JunctionTree
Definition: JunctionTree.h:50
gtsam::Value::retract_
virtual Value * retract_(const Vector &delta) const =0
Increment the value, by mapping from the vector delta in the tangent space of the current value back ...
gtsam::NonlinearFactorGraph::linearize
boost::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
Definition: NonlinearFactorGraph.cpp:329
gtsam::UpdateImpl::CheckRelinearizationFull
static KeySet CheckRelinearizationFull(const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition: ISAM2-impl.h:345
gtsam::VectorValues::find
iterator find(Key j)
Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
Definition: VectorValues.h:235
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition: VariableIndex-inl.h:54
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: BayesTree-inst.h:195
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition: FactorGraph.h:158
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition: FactorGraph.h:358
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
gtsam::VariableIndex::augment
void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
Augment the variable index with new factors.
Definition: VariableIndex-inl.h:27
gtsam::Symbol
Character and index key used to refer to variables.
Definition: Symbol.h:33