gtsam  3.2.1
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 
18 #pragma once
19 
20 #include <gtsam/linear/GaussianBayesTree.h>
21 #include <gtsam/nonlinear/ISAM2.h>
22 
23 namespace gtsam {
24 
25 struct GTSAM_EXPORT ISAM2::Impl {
26 
27  struct GTSAM_EXPORT PartialSolveResult {
28  ISAM2::sharedClique bayesTree;
29  };
30 
31  struct GTSAM_EXPORT ReorderingMode {
32  size_t nFullSystemVars;
33  enum { /*AS_ADDED,*/ COLAMD } algorithm;
34  enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
35  boost::optional<FastMap<Key,int> > constrainedKeys;
36  };
37 
47  static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
48  VectorValues& deltaNewton, VectorValues& RgProd,
49  const KeyFormatter& keyFormatter = DefaultKeyFormatter);
50 
54  static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
55  NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices);
56 
60  static void RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
61  Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
62  VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
63  FastSet<Key>& fixedVariables);
64 
74  static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta,
75  const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
76 
88  static FastSet<Key> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
89  const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
90 
106  static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask);
107 
121  static void ExpmapMasked(Values& values, const VectorValues& delta,
122  const FastSet<Key>& mask,
123  boost::optional<VectorValues&> invalidateIfDebug = boost::none,
124  const KeyFormatter& keyFormatter = DefaultKeyFormatter);
125 
129  static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
130  const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
131 
136  static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
137  const VectorValues& gradAtZero, VectorValues& RgProd);
138 
142  static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
143  const VectorValues& RgProd);
144 
145 };
146 
147 }
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:75
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:478
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:69
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph...
Definition: VariableIndex.h:42
gtsam::ISAM2::ReorderingMode
Definition: ISAM2-impl.h:31
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:89
gtsam::ISAM2
Definition: ISAM2-impl.h:25
gtsam::ISAM2::PartialSolveResult
Definition: ISAM2-impl.h:27
gtsam::delta
Vector delta(size_t n, size_t i, double value)
Create basis vector of dimension n, with a constant in spot i.
Definition: Vector.cpp:53
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: types.h:62
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition: ISAM2.h:105