NonlinearISAM.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 
21 
22 #include <iostream>
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 
29 /* ************************************************************************* */
30 void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
31  isam_.saveGraph(s, keyFormatter);
32 }
33 
34 /* ************************************************************************* */
35 void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) {
36 
37  if(newFactors.size() > 0) {
38 
39  // Reorder and relinearize every reorderInterval updates
40  if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
41  reorder_relinearize();
42  reorderCounter_ = 0;
43  }
44 
45  factors_.push_back(newFactors);
46 
47  // Linearize new factors and insert them
48  // TODO: optimize for whole config?
49  linPoint_.insert(initialValues);
50 
51  std::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_);
52 
53  // Update ISAM
54  isam_.update(*linearizedNewFactors, eliminationFunction_);
55  }
56 }
57 
58 /* ************************************************************************* */
59 void NonlinearISAM::reorder_relinearize() {
60 
61 // cout << "Reordering, relinearizing..." << endl;
62 
63  if(factors_.size() > 0) {
64  // Obtain the new linearization point
65  const Values newLinPoint = estimate();
66 
67  isam_.clear();
68 
69  // Just recreate the whole BayesTree
70  // TODO: allow for constrained ordering here
71  // TODO: decouple relinearization and reordering to avoid
72  isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_);
73 
74  // Update linearization point
75  linPoint_ = newLinPoint;
76  }
77 }
78 
79 /* ************************************************************************* */
80 Values NonlinearISAM::estimate() const {
81  if(isam_.size() > 0)
82  return linPoint_.retract(isam_.optimize());
83  else
84  return linPoint_;
85 }
86 
87 /* ************************************************************************* */
88 Matrix NonlinearISAM::marginalCovariance(Key key) const {
89  return isam_.marginalCovariance(key);
90 }
91 
92 /* ************************************************************************* */
93 void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) const {
94  cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
95  isam_.print("GaussianISAM:\n");
96  linPoint_.print("Linearization Point:\n", keyFormatter);
97  factors_.print("Nonlinear Graph:\n", keyFormatter);
98 }
99 
100 /* ************************************************************************* */
101 void NonlinearISAM::printStats() const {
102  isam_.getCliqueData().getStats().print();
103 }
104 
105 /* ************************************************************************* */
106 
107 }
const gtsam::Symbol key('X', 0)
def update(text)
Definition: relicense.py:46
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
size_t size() const
Definition: FactorGraph.h:334
RealScalar s
Linear Factor Graph where all factors are Gaussians.
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
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


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