nonlinear/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_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
30 
31 public:
32 
34  typedef std::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 
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>
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 
152  FactorIndex factorIndex_;
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 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition: Key.cpp:85
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
def update(text)
Definition: relicense.py:46
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
BatchFixedLagSmoother(double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
const NonlinearFactorGraph & getFactors() const
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
const LevenbergMarquardtParams & params() const
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
Eigen::VectorXd Vector
Definition: Vector.h:38
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
std::shared_ptr< BatchFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
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
static ConjugateGradientParameters parameters
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
std::vector< float > Values
std::shared_ptr< This > shared_ptr
const G double tol
Definition: Group.h:86
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::map< Key, std::set< Key > > FactorIndex
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:57