gtsam  4.1.0
gtsam
NonlinearISAM.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 
21 #include <gtsam/linear/GaussianISAM.h>
22 
23 namespace gtsam {
27 class GTSAM_EXPORT NonlinearISAM {
28 protected:
29 
32 
35 
38 
41  int reorderCounter_;
42 
45 
46 public:
47 
50 
58  NonlinearISAM(int reorderInterval = 1,
59  const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
60  reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
61 
65 
67  Values estimate() const;
68 
70  Matrix marginalCovariance(Key key) const;
71 
72  // access
73 
75  const GaussianISAM& bayesTree() const { return isam_; }
76 
78  const Values& getLinearizationPoint() const { return linPoint_; }
79 
81  const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
82 
84  int reorderInterval() const { return reorderInterval_; }
85  int reorderCounter() const { return reorderCounter_; }
86 
88  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
89 
91  void printStats() const;
92 
94  void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
95 
99 
101  void update(const NonlinearFactorGraph& newFactors, const Values& initialValues);
102 
104  void reorder_relinearize();
105 
107 
108 };
109 
110 } // \namespace gtsam
gtsam::NonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition: NonlinearISAM.h:34
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::NonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition: NonlinearISAM.h:85
gtsam::GaussianISAM
Definition: GaussianISAM.h:28
gtsam::NonlinearISAM
Wrapper class to manage ISAM in a nonlinear context.
Definition: NonlinearISAM.h:27
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition: NonlinearISAM.h:78
gtsam::NonlinearISAM::eliminationFunction_
GaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition: NonlinearISAM.h:44
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
boost::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NonlinearISAM::isam_
gtsam::GaussianISAM isam_
The internal iSAM object.
Definition: NonlinearISAM.h:31
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NonlinearISAM::bayesTree
const GaussianISAM & bayesTree() const
access the underlying bayes tree
Definition: NonlinearISAM.h:75
gtsam::NonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition: NonlinearISAM.h:84
gtsam::NonlinearISAM::factors_
NonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition: NonlinearISAM.h:37
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: Key.h:35
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::NonlinearISAM::NonlinearISAM
NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate &eliminationFunction=GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition: NonlinearISAM.h:58
gtsam::NonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition: NonlinearISAM.h:40
gtsam::NonlinearISAM::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition: NonlinearISAM.h:81