gtsam  4.1.0
gtsam
ISAM2.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 // \callgraph
20 
21 #pragma once
22 
29 
30 #include <vector>
31 
32 namespace gtsam {
33 
45 class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
46  protected:
49 
53 
62 
63  mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores
64  // the Gauss-Newton update
65  mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and
66  // is updated incrementally
67 
76  mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in
77  // the right way
78 
82 
85 
88 
90  mutable boost::optional<double> doglegDelta_;
91 
95 
97 
99  public:
100  using This = ISAM2;
104  using Cliques = Base::Cliques;
105 
107  explicit ISAM2(const ISAM2Params& params);
108 
111  ISAM2();
112 
114  virtual ~ISAM2() {}
115 
117  virtual bool equals(const ISAM2& other, double tol = 1e-9) const;
118 
151  virtual ISAM2Result update(
152  const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
153  const Values& newTheta = Values(),
154  const FactorIndices& removeFactorIndices = FactorIndices(),
155  const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
156  const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
157  const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
158  bool force_relinearize = false);
159 
178  virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
179  const Values& newTheta,
180  const ISAM2UpdateParams& updateParams);
181 
199  void marginalizeLeaves(
200  const FastList<Key>& leafKeys,
201  boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
202  boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
203 
205  const Values& getLinearizationPoint() const { return theta_; }
206 
208  bool valueExists(Key key) const { return theta_.exists(key); }
209 
215  Values calculateEstimate() const;
216 
223  template <class VALUE>
224  VALUE calculateEstimate(Key key) const {
225  const Vector& delta = getDelta()[key];
226  return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
227  }
228 
237  const Value& calculateEstimate(Key key) const;
238 
240  Matrix marginalCovariance(Key key) const;
241 
244 
248  Values calculateBestEstimate() const;
249 
251  const VectorValues& getDelta() const;
252 
254  double error(const VectorValues& x) const;
255 
258  return nonlinearFactors_;
259  }
260 
262  const VariableIndex& getVariableIndex() const { return variableIndex_; }
263 
265  const KeySet& getFixedVariables() const { return fixedVariables_; }
266 
267  const ISAM2Params& params() const { return params_; }
268 
270  void printStats() const { getCliqueData().getStats().print(); }
271 
279  VectorValues gradientAtZero() const;
280 
282 
283  protected:
285  void recalculate(const ISAM2UpdateParams& updateParams,
286  const KeySet& relinKeys, ISAM2Result* result);
287 
288  // Do a batch step - reorder and relinearize all variables
289  void recalculateBatch(const ISAM2UpdateParams& updateParams,
290  KeySet* affectedKeysSet, ISAM2Result* result);
291 
292  // retrieve all factors that ONLY contain the affected variables
293  // (note that the remaining stuff is summarized in the cached factors)
294  GaussianFactorGraph relinearizeAffectedFactors(
295  const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
296  const KeySet& relinKeys);
297 
298  void recalculateIncremental(const ISAM2UpdateParams& updateParams,
299  const KeySet& relinKeys,
300  const FastList<Key>& affectedKeys,
301  KeySet* affectedKeysSet, Cliques* orphans,
302  ISAM2Result* result);
303 
309  void addVariables(const Values& newTheta,
310  ISAM2Result::DetailedResults* detail = 0);
311 
315  void removeVariables(const KeySet& unusedKeys);
316 
317  void updateDelta(bool forceFullSolve = false) const;
318 }; // ISAM2
319 
321 template <>
322 struct traits<ISAM2> : public Testable<ISAM2> {};
323 
324 } // namespace gtsam
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::ISAM2::linearFactors_
GaussianFactorGraph linearFactors_
The current linear factors, which are only updated as needed.
Definition: ISAM2.h:84
gtsam::BayesTree< ISAM2Clique >::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition: BayesTree.h:72
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::ISAM2::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition: ISAM2.h:224
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::BayesTree
Definition: BayesTree.h:65
gtsam::Value
This is the base class for any type to be stored in Values.
Definition: Value.h:36
gtsam::ISAM2Params
Definition: ISAM2Params.h:135
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::ISAM2::valueExists
bool valueExists(Key key) const
Check whether variable with given key exists in linearization point.
Definition: ISAM2.h:208
gtsam::ISAM2::delta_
VectorValues delta_
The linear delta from the last linear solution, an update to the estimate in theta.
Definition: ISAM2.h:61
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ISAM2::update_count_
int update_count_
Counter incremented every update(), used to determine periodic relinearization.
Definition: ISAM2.h:96
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition: ISAM2Result.h:117
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:205
ISAM2Result.h
Class that stores detailed iSAM2 result.
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition: ISAM2Clique.h:37
gtsam::FastList
Definition: FastList.h:38
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ISAM2::params_
ISAM2Params params_
The current parameters.
Definition: ISAM2.h:87
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Access the set of nonlinear factors.
Definition: ISAM2.h:257
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::ISAM2::fixedVariables_
KeySet fixedVariables_
Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
Definition: ISAM2.h:94
ISAM2UpdateParams.h
Class that stores extra params for ISAM2::update()
gtsam::ISAM2::nonlinearFactors_
NonlinearFactorGraph nonlinearFactors_
All original nonlinear factors are stored here to use during relinearization.
Definition: ISAM2.h:81
gtsam::ISAM2::~ISAM2
virtual ~ISAM2()
default virtual destructor
Definition: ISAM2.h:114
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::ISAM2UpdateParams
Definition: ISAM2UpdateParams.h:32
ISAM2Params.h
Parameters for iSAM 2.
gtsam::FastSet< Key >
gtsam::ISAM2::variableIndex_
VariableIndex variableIndex_
VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
Definition: ISAM2.h:52
gtsam::ISAM2::getVariableIndex
const VariableIndex & getVariableIndex() const
Access the nonlinear variable index.
Definition: ISAM2.h:262
gtsam::ISAM2::theta_
Values theta_
The current linearization point.
Definition: ISAM2.h:48
gtsam::ISAM2::doglegDelta_
boost::optional< double > doglegDelta_
The current Dogleg Delta (trust region radius)
Definition: ISAM2.h:90
gtsam::FastMap
Definition: FastMap.h:38
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:104
gtsam::ISAM2::printStats
void printStats() const
prints out clique statistics
Definition: ISAM2.h:270
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ISAM2Result
Definition: ISAM2Result.h:41
gtsam::ISAM2::deltaReplacedMask_
KeySet deltaReplacedMask_
A cumulative mask for the variables that were replaced and have not yet been updated in the linear so...
Definition: ISAM2.h:76
gtsam::ISAM2::Cliques
Base::Cliques Cliques
List of Cliques.
Definition: ISAM2.h:104
gtsam::ISAM2::getFixedVariables
const KeySet & getFixedVariables() const
Access the nonlinear variable index.
Definition: ISAM2.h:265
ISAM2Clique.h
Specialized iSAM2 Clique.
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