gtsam 4.1.1
gtsam
BatchFixedLagSmoother.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#include <queue>
26
27namespace gtsam {
28
29class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
30
31public:
32
34 typedef boost::shared_ptr<BatchFixedLagSmoother> shared_ptr;
35
37 BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
38 FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { };
39
41 ~BatchFixedLagSmoother() override { };
42
44 void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
45
47 bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
48
50 Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
51 const Values& newTheta = Values(),
52 const KeyTimestampMap& timestamps = KeyTimestampMap(),
53 const FactorIndices& factorsToRemove = FactorIndices()) override;
54
59 Values calculateEstimate() const override {
60 return theta_.retract(delta_);
61 }
62
69 template<class VALUE>
70 VALUE calculateEstimate(Key key) const {
71 const Vector delta = delta_.at(key);
72 return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
73 }
74
77 return parameters_;
78 }
79
82 return parameters_;
83 }
84
87 return factors_;
88 }
89
92 return theta_;
93 }
94
96 const Ordering& getOrdering() const {
97 return ordering_;
98 }
99
101 const VectorValues& getDelta() const {
102 return delta_;
103 }
104
106 Matrix marginalCovariance(Key key) const;
107
111 static GaussianFactorGraph CalculateMarginalFactors(
112 const GaussianFactorGraph& graph, const KeyVector& keys,
113 const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
114
116 static NonlinearFactorGraph CalculateMarginalFactors(
117 const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
118 const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
119
120protected:
121
123 typedef std::map<Key, std::set<Key> > FactorIndex;
124
127
132
135
138
141
144
147
149 std::queue<size_t> availableSlots_;
150
153
155 void insertFactors(const NonlinearFactorGraph& newFactors);
156
158 void removeFactors(const std::set<size_t>& deleteFactors);
159
161 void eraseKeys(const KeyVector& keys);
162
164 void reorder(const KeyVector& marginalizeKeys = KeyVector());
165
168
170 void marginalize(const KeyVector& marginalizableKeys);
171
172private:
174 static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
175 static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
176 static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
177 static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
178 static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
179 static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
180}; // BatchFixedLagSmoother
181
182}
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Template to create a binary predicate.
Definition: Testable.h:111
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
Definition: Ordering.h:34
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
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:35
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
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:101
Definition: BatchFixedLagSmoother.h:29
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: BatchFixedLagSmoother.h:91
NonlinearFactorGraph factors_
The nonlinear factors.
Definition: BatchFixedLagSmoother.h:134
Values theta_
The current linearization point.
Definition: BatchFixedLagSmoother.h:137
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition: BatchFixedLagSmoother.h:70
Ordering ordering_
The current ordering.
Definition: BatchFixedLagSmoother.h:143
const LevenbergMarquardtParams & params() const
read the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:76
Values calculateEstimate() const override
Compute an estimate from the incomplete linear delta computed during the last update.
Definition: BatchFixedLagSmoother.h:59
boost::shared_ptr< BatchFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: BatchFixedLagSmoother.h:34
std::map< Key, std::set< Key > > FactorIndex
A typedef defining an Key-Factor mapping.
Definition: BatchFixedLagSmoother.h:123
bool enforceConsistency_
A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the linear...
Definition: BatchFixedLagSmoother.h:131
const Ordering & getOrdering() const
Access the current ordering.
Definition: BatchFixedLagSmoother.h:96
LevenbergMarquardtParams & params()
update the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:81
LevenbergMarquardtParams parameters_
The L-M optimization parameters.
Definition: BatchFixedLagSmoother.h:126
std::queue< size_t > availableSlots_
The set of available factor graph slots.
Definition: BatchFixedLagSmoother.h:149
BatchFixedLagSmoother(double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
default constructor
Definition: BatchFixedLagSmoother.h:37
~BatchFixedLagSmoother() override
destructor
Definition: BatchFixedLagSmoother.h:41
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: BatchFixedLagSmoother.h:86
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: BatchFixedLagSmoother.h:101
FactorIndex factorIndex_
A cross-reference structure to allow efficient factor lookups by key.
Definition: BatchFixedLagSmoother.h:152
VectorValues delta_
The current set of linear deltas.
Definition: BatchFixedLagSmoother.h:146
Values linearKeys_
The set of keys involved in current linear factors.
Definition: BatchFixedLagSmoother.h:140
Definition: FixedLagSmoother.h:33
Meta information returned about the update.
Definition: FixedLagSmoother.h:48