gtsam 4.2.0
gtsam
HybridNonlinearISAM.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
22
23namespace gtsam {
27class GTSAM_EXPORT HybridNonlinearISAM {
28 protected:
31
34
37
40
43 int reorderCounter_;
44
47
48 public:
51
60 int reorderInterval = 1,
61 const HybridGaussianFactorGraph::Eliminate& eliminationFunction =
62 HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
63 : reorderInterval_(reorderInterval),
64 reorderCounter_(0),
65 eliminationFunction_(eliminationFunction) {}
66
70
72 Values estimate();
73
74 // /** find the marginal covariance for a single variable */
75 // Matrix marginalCovariance(Key key) const;
76
77 // access
78
80 const HybridGaussianISAM& bayesTree() const { return isam_; }
81
87 void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); }
88
90 const Values& getLinearizationPoint() const { return linPoint_; }
91
93 const DiscreteValues& assignment() const { return assignment_; }
94
97 return factors_;
98 }
99
101 int reorderInterval() const { return reorderInterval_; }
102 int reorderCounter() const { return reorderCounter_; }
103
105 void print(const std::string& s = "",
106 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
107
109 void printStats() const;
110
112 void saveGraph(const std::string& s,
113 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
114
118
120 void update(const HybridNonlinearFactorGraph& newFactors,
121 const Values& initialValues,
122 const boost::optional<size_t>& maxNrLeaves = boost::none,
123 const boost::optional<Ordering>& ordering = boost::none);
124
126 void reorder_relinearize();
127
129};
130
131} // namespace gtsam
Nonlinear hybrid factor graph that uses type erasure.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
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
A map from keys to values.
Definition: DiscreteValues.h:34
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition: HybridBayesTree.cpp:175
Definition: HybridGaussianISAM.h:34
Definition: HybridNonlinearFactorGraph.h:33
Wrapper class to manage ISAM in a nonlinear context.
Definition: HybridNonlinearISAM.h:27
const HybridNonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition: HybridNonlinearISAM.h:96
DiscreteValues assignment_
The discrete assignment.
Definition: HybridNonlinearISAM.h:36
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition: HybridNonlinearISAM.h:90
HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition: HybridNonlinearISAM.h:59
HybridNonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition: HybridNonlinearISAM.h:39
const DiscreteValues & assignment() const
Return the current discrete assignment.
Definition: HybridNonlinearISAM.h:93
int reorderCounter() const
TODO: comment.
Definition: HybridNonlinearISAM.h:102
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition: HybridNonlinearISAM.h:87
int reorderInterval_
The reordering interval and counter.
Definition: HybridNonlinearISAM.h:42
int reorderInterval() const
get counters
Definition: HybridNonlinearISAM.h:101
Values linPoint_
The current linearization point.
Definition: HybridNonlinearISAM.h:33
gtsam::HybridGaussianISAM isam_
The internal iSAM object.
Definition: HybridNonlinearISAM.h:30
HybridGaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition: HybridNonlinearISAM.h:46
const HybridGaussianISAM & bayesTree() const
access the underlying bayes tree
Definition: HybridNonlinearISAM.h:80
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65