gtsam 4.1.1
gtsam
IncrementalFixedLagSmoother.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
20// \callgraph
21#pragma once
22
25
26namespace gtsam {
27
33class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
34
35public:
36
38 typedef boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
39
41 IncrementalFixedLagSmoother(double smootherLag = 0.0,
42 const ISAM2Params& parameters = DefaultISAM2Params()) :
43 FixedLagSmoother(smootherLag), isam_(parameters) {
44 }
45
48 }
49
51 void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
52 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
53
55 bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
56
64 Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
65 const Values& newTheta = Values(), //
66 const KeyTimestampMap& timestamps = KeyTimestampMap(),
67 const FactorIndices& factorsToRemove = FactorIndices()) override;
68
73 Values calculateEstimate() const override {
74 return isam_.calculateEstimate();
75 }
76
83 template<class VALUE>
84 VALUE calculateEstimate(Key key) const {
85 return isam_.calculateEstimate<VALUE>(key);
86 }
87
89 const ISAM2Params& params() const {
90 return isam_.params();
91 }
92
95 return isam_.getFactorsUnsafe();
96 }
97
100 return isam_.getLinearizationPoint();
101 }
102
104 const VectorValues& getDelta() const {
105 return isam_.getDelta();
106 }
107
109 Matrix marginalCovariance(Key key) const {
110 return isam_.marginalCovariance(key);
111 }
112
114 const ISAM2Result& getISAM2Result() const{ return isamResult_; }
115
116protected:
117
120 ISAM2Params params;
121 params.findUnusedFactorSlots = true;
122 return params;
123 }
124
128
131
133 void eraseKeysBefore(double timestamp);
134
136 void createOrderingConstraints(const KeyVector& marginalizableKeys,
137 boost::optional<FastMap<Key, int> >& constrainedKeys) const;
138
139private:
141 static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
142 "Keys:");
143 static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
144 static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
145 const std::string& label = "Factor Graph:");
146 static void PrintSymbolicTree(const gtsam::ISAM2& isam,
147 const std::string& label = "Bayes Tree:");
148 static void PrintSymbolicTreeHelper(
149 const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
150 "");
151
152};
153// IncrementalFixedLagSmoother
154
155}
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Base class for a fixed-lag smoother.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:33
void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition: Key.cpp:82
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
Definition: FastMap.h:38
Template to create a binary predicate.
Definition: Testable.h:111
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
Definition: ISAM2.h:45
Definition: ISAM2Params.h:135
bool findUnusedFactorSlots
When you will be removing many factors, e.g.
Definition: ISAM2Params.h:224
Definition: ISAM2Result.h:41
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
Definition: FixedLagSmoother.h:33
This is a base class for the various HMF2 implementations.
Definition: IncrementalFixedLagSmoother.h:33
ISAM2 isam_
An iSAM2 object used to perform inference.
Definition: IncrementalFixedLagSmoother.h:127
~IncrementalFixedLagSmoother() override
destructor
Definition: IncrementalFixedLagSmoother.h:47
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: IncrementalFixedLagSmoother.h:104
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition: IncrementalFixedLagSmoother.h:84
Values calculateEstimate() const override
Compute an estimate from the incomplete linear delta computed during the last update.
Definition: IncrementalFixedLagSmoother.h:73
Matrix marginalCovariance(Key key) const
Calculate marginal covariance on given variable.
Definition: IncrementalFixedLagSmoother.h:109
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: IncrementalFixedLagSmoother.h:99
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: IncrementalFixedLagSmoother.h:94
ISAM2Result isamResult_
Store results of latest isam2 update.
Definition: IncrementalFixedLagSmoother.h:130
static ISAM2Params DefaultISAM2Params()
Create default parameters.
Definition: IncrementalFixedLagSmoother.h:119
const ISAM2Params & params() const
return the current set of iSAM2 parameters
Definition: IncrementalFixedLagSmoother.h:89
boost::shared_ptr< IncrementalFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: IncrementalFixedLagSmoother.h:38
const ISAM2Result & getISAM2Result() const
Get results of latest isam2 update.
Definition: IncrementalFixedLagSmoother.h:114
IncrementalFixedLagSmoother(double smootherLag=0.0, const ISAM2Params &parameters=DefaultISAM2Params())
default constructor
Definition: IncrementalFixedLagSmoother.h:41