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 std::optional<double> doglegDelta_;
91 
95 
97 
99  public:
100  using This = ISAM2;
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 std::optional<FastMap<Key, int> >& constrainedKeys = {},
156  const std::optional<FastList<Key> >& noRelinKeys = {},
157  const std::optional<FastList<Key> >& extraReelimKeys = {},
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  FactorIndices* marginalFactorsIndices = nullptr,
202  FactorIndices* deletedFactorsIndices = nullptr);
203 
208  template <class... OptArgs>
209  void marginalizeLeaves(const FastList<Key>& leafKeys,
210  OptArgs&&... optArgs) {
211  // dereference the optional arguments and pass
212  // it to the pointer version
213  marginalizeLeaves(leafKeys, (&optArgs)...);
214  }
215 
217  const Values& getLinearizationPoint() const { return theta_; }
218 
220  bool valueExists(Key key) const { return theta_.exists(key); }
221 
227  Values calculateEstimate() const;
228 
235  template <class VALUE>
237  const Vector& delta = getDelta()[key];
238  return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
239  }
240 
249  const Value& calculateEstimate(Key key) const;
250 
252  Matrix marginalCovariance(Key key) const;
253 
256 
260  Values calculateBestEstimate() const;
261 
263  const VectorValues& getDelta() const;
264 
266  double error(const VectorValues& x) const;
267 
270  return nonlinearFactors_;
271  }
272 
274  const VariableIndex& getVariableIndex() const { return variableIndex_; }
275 
277  const KeySet& getFixedVariables() const { return fixedVariables_; }
278 
279  const ISAM2Params& params() const { return params_; }
280 
282  void printStats() const { getCliqueData().getStats().print(); }
283 
291  VectorValues gradientAtZero() const;
292 
294 
295  protected:
297  void recalculate(const ISAM2UpdateParams& updateParams,
298  const KeySet& relinKeys, ISAM2Result* result);
299 
300  // Do a batch step - reorder and relinearize all variables
301  void recalculateBatch(const ISAM2UpdateParams& updateParams,
302  KeySet* affectedKeysSet, ISAM2Result* result);
303 
304  // retrieve all factors that ONLY contain the affected variables
305  // (note that the remaining stuff is summarized in the cached factors)
306  GaussianFactorGraph relinearizeAffectedFactors(
307  const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
308  const KeySet& relinKeys);
309 
321  void recalculateIncremental(const ISAM2UpdateParams& updateParams,
322  const KeySet& relinKeys,
323  const FastList<Key>& affectedKeys,
324  KeySet* affectedKeysSet, Cliques* orphans,
325  ISAM2Result* result);
326 
332  void addVariables(const Values& newTheta,
334 
338  void removeVariables(const KeySet& unusedKeys);
339 
340  void updateDelta(bool forceFullSolve = false) const;
341 
342  private:
343 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
344 
345  friend class boost::serialization::access;
346  template<class ARCHIVE>
347  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
348  ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
349  ar & BOOST_SERIALIZATION_NVP(theta_);
350  ar & BOOST_SERIALIZATION_NVP(variableIndex_);
351  ar & BOOST_SERIALIZATION_NVP(delta_);
352  ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
353  ar & BOOST_SERIALIZATION_NVP(RgProd_);
354  ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
355  ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
356  ar & BOOST_SERIALIZATION_NVP(linearFactors_);
357  ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
358  ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
359  ar & BOOST_SERIALIZATION_NVP(update_count_);
360  }
361 #endif
362 
363 }; // ISAM2
364 
366 template <>
367 struct traits<ISAM2> : public Testable<ISAM2> {};
368 
369 } // namespace gtsam
const gtsam::Symbol key('X', 0)
void marginalizeLeaves(const FastList< Key > &leafKeys, OptArgs &&... optArgs)
Definition: ISAM2.h:209
VALUE calculateEstimate(Key key) const
Definition: ISAM2.h:236
def update(text)
Definition: relicense.py:46
Factor Graph consisting of non-linear factors.
std::string serialize(const T &input)
serializes to a string
VectorValues RgProd_
Definition: ISAM2.h:65
const ValueType at(Key j) const
Definition: Values-inl.h:204
Specialized iSAM2 Clique.
bool valueExists(Key key) const
Check whether variable with given key exists in linearization point.
Definition: ISAM2.h:220
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
VariableIndex variableIndex_
Definition: ISAM2.h:52
std::optional< double > doglegDelta_
Definition: ISAM2.h:90
ISAM2Params params_
Definition: ISAM2.h:87
std::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition: BayesTree.h:74
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
static const SmartProjectionParams params
KeySet fixedVariables_
Definition: ISAM2.h:94
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
Eigen::VectorXd Vector
Definition: Vector.h:38
NonlinearFactorGraph nonlinearFactors_
Definition: ISAM2.h:81
Values result
void printStats() const
Definition: ISAM2.h:282
Class that stores detailed iSAM2 result.
virtual ~ISAM2()
Definition: ISAM2.h:114
GaussianFactorGraph linearFactors_
Definition: ISAM2.h:84
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Values theta_
Definition: ISAM2.h:48
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition: ISAM2.h:103
int update_count_
Definition: ISAM2.h:96
KeySet deltaReplacedMask_
Definition: ISAM2.h:76
traits
Definition: chartTesting.h:28
std::vector< float > Values
const KeySet & getFixedVariables() const
Definition: ISAM2.h:277
Class that stores extra params for ISAM2::update()
VectorValues delta_
Definition: ISAM2.h:61
const VariableIndex & getVariableIndex() const
Definition: ISAM2.h:274
static double error
Definition: testRot3.cpp:37
VectorValues deltaNewton_
Definition: ISAM2.h:63
const G double tol
Definition: Group.h:86
bool exists(Key j) const
Definition: Values.cpp:93
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const ISAM2Params & params() const
Definition: ISAM2.h:279
Parameters for iSAM 2.


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