gtsam  4.1.0
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 
27 namespace gtsam {
28 
29 class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
30 
31 public:
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  virtual ~BatchFixedLagSmoother() { };
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 
91  const Values& getLinearizationPoint() const {
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 
120 protected:
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 
167  Result optimize();
168 
170  void marginalize(const KeyVector& marginalizableKeys);
171 
172 private:
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 }
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::BatchFixedLagSmoother::getDelta
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: BatchFixedLagSmoother.h:101
gtsam::FixedLagSmoother::Result
Meta information returned about the update.
Definition: FixedLagSmoother.h:48
gtsam::BatchFixedLagSmoother::params
LevenbergMarquardtParams & params()
update the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:81
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
gtsam::Ordering
Definition: Ordering.h:34
gtsam::BatchFixedLagSmoother::~BatchFixedLagSmoother
virtual ~BatchFixedLagSmoother()
destructor
Definition: BatchFixedLagSmoother.h:41
gtsam::BatchFixedLagSmoother::factors_
NonlinearFactorGraph factors_
The nonlinear factors.
Definition: BatchFixedLagSmoother.h:134
gtsam::BatchFixedLagSmoother::theta_
Values theta_
The current linearization point.
Definition: BatchFixedLagSmoother.h:137
gtsam::BatchFixedLagSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: BatchFixedLagSmoother.h:91
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::BatchFixedLagSmoother
Definition: BatchFixedLagSmoother.h:29
gtsam::BatchFixedLagSmoother::calculateEstimate
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
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::BatchFixedLagSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: BatchFixedLagSmoother.h:86
gtsam::BatchFixedLagSmoother::FactorIndex
std::map< Key, std::set< Key > > FactorIndex
A typedef defining an Key-Factor mapping.
Definition: BatchFixedLagSmoother.h:123
gtsam::BatchFixedLagSmoother::ordering_
Ordering ordering_
The current ordering.
Definition: BatchFixedLagSmoother.h:143
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::BatchFixedLagSmoother::parameters_
LevenbergMarquardtParams parameters_
The L-M optimization parameters.
Definition: BatchFixedLagSmoother.h:126
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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::BatchFixedLagSmoother::calculateEstimate
Values calculateEstimate() const override
Compute an estimate from the incomplete linear delta computed during the last update.
Definition: BatchFixedLagSmoother.h:59
gtsam::BatchFixedLagSmoother::BatchFixedLagSmoother
BatchFixedLagSmoother(double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
default constructor
Definition: BatchFixedLagSmoother.h:37
gtsam::BatchFixedLagSmoother::enforceConsistency_
bool enforceConsistency_
A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the linear...
Definition: BatchFixedLagSmoother.h:131
gtsam::BatchFixedLagSmoother::availableSlots_
std::queue< size_t > availableSlots_
The set of available factor graph slots.
Definition: BatchFixedLagSmoother.h:149
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:109
gtsam::BatchFixedLagSmoother::factorIndex_
FactorIndex factorIndex_
A cross-reference structure to allow efficient factor lookups by key.
Definition: BatchFixedLagSmoother.h:152
gtsam::BatchFixedLagSmoother::delta_
VectorValues delta_
The current set of linear deltas.
Definition: BatchFixedLagSmoother.h:146
gtsam::BatchFixedLagSmoother::shared_ptr
boost::shared_ptr< BatchFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: BatchFixedLagSmoother.h:34
gtsam::FixedLagSmoother
Definition: FixedLagSmoother.h:33
gtsam::FastSet< Key >
gtsam::BatchFixedLagSmoother::params
const LevenbergMarquardtParams & params() const
read the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:76
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::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:33
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::BatchFixedLagSmoother::linearKeys_
Values linearKeys_
The set of keys involved in current linear factors.
Definition: BatchFixedLagSmoother.h:140
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::PrintKeySet
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
FixedLagSmoother.h
Base class for a fixed-lag smoother.
gtsam::BatchFixedLagSmoother::getOrdering
const Ordering & getOrdering() const
Access the current ordering.
Definition: BatchFixedLagSmoother.h:96