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 
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::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
relicense.update
def update(text)
Definition: relicense.py:46
gtsam::Result
Result from elimination.
Definition: HybridGaussianFactorGraph.cpp:61
gtsam::BatchFixedLagSmoother::parameters_
LevenbergMarquardtParams parameters_
Definition: nonlinear/BatchFixedLagSmoother.h:126
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::BatchFixedLagSmoother::delta_
VectorValues delta_
Definition: nonlinear/BatchFixedLagSmoother.h:146
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Definition: triangulation.cpp:177
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
gtsam::BatchFixedLagSmoother::params
const LevenbergMarquardtParams & params() const
Definition: nonlinear/BatchFixedLagSmoother.h:76
gtsam::BatchFixedLagSmoother::availableSlots_
std::queue< size_t > availableSlots_
Definition: nonlinear/BatchFixedLagSmoother.h:149
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::FastSet< Key >
gtsam::BatchFixedLagSmoother::calculateEstimate
VALUE calculateEstimate(Key key) const
Definition: nonlinear/BatchFixedLagSmoother.h:70
gtsam::BatchFixedLagSmoother::theta_
Values theta_
Definition: nonlinear/BatchFixedLagSmoother.h:137
gtsam::BatchFixedLagSmoother::params
LevenbergMarquardtParams & params()
Definition: nonlinear/BatchFixedLagSmoother.h:81
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::BatchFixedLagSmoother::factorIndex_
FactorIndex factorIndex_
Definition: nonlinear/BatchFixedLagSmoother.h:152
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::EliminatePreferCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:540
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::BatchFixedLagSmoother::getOrdering
const Ordering & getOrdering() const
Definition: nonlinear/BatchFixedLagSmoother.h:96
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
gtsam::FixedLagSmoother
Definition: nonlinear/FixedLagSmoother.h:33
gtsam::BatchFixedLagSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Definition: nonlinear/BatchFixedLagSmoother.h:91
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::KeyFormatter
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
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::FixedLagSmoother::Result
Definition: nonlinear/FixedLagSmoother.h:48
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::BatchFixedLagSmoother
Definition: nonlinear/BatchFixedLagSmoother.h:29
gtsam::BatchFixedLagSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Definition: nonlinear/BatchFixedLagSmoother.h:86
gtsam::BatchFixedLagSmoother::enforceConsistency_
bool enforceConsistency_
Definition: nonlinear/BatchFixedLagSmoother.h:131
gtsam::BatchFixedLagSmoother::ordering_
Ordering ordering_
Definition: nonlinear/BatchFixedLagSmoother.h:143
gtsam::BatchFixedLagSmoother::linearValues_
Values linearValues_
Definition: nonlinear/BatchFixedLagSmoother.h:140
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
FixedLagSmoother.h
Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: SFMdata.h:40
gtsam::BatchFixedLagSmoother::~BatchFixedLagSmoother
~BatchFixedLagSmoother() override
Definition: nonlinear/BatchFixedLagSmoother.h:41
gtsam::traits
Definition: Group.h:36
gtsam::Values
Definition: Values.h:65
gtsam::BatchFixedLagSmoother::FactorIndex
std::map< Key, std::set< Key > > FactorIndex
Definition: nonlinear/BatchFixedLagSmoother.h:123
gtsam::BatchFixedLagSmoother::getDelta
const VectorValues & getDelta() const
Definition: nonlinear/BatchFixedLagSmoother.h:101
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:88
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::BatchFixedLagSmoother::calculateEstimate
Values calculateEstimate() const override
Definition: nonlinear/BatchFixedLagSmoother.h:59
gtsam::BatchFixedLagSmoother::shared_ptr
std::shared_ptr< BatchFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: nonlinear/BatchFixedLagSmoother.h:34
gtsam::BatchFixedLagSmoother::factors_
NonlinearFactorGraph factors_
Definition: nonlinear/BatchFixedLagSmoother.h:134
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
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:88
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::BatchFixedLagSmoother::BatchFixedLagSmoother
BatchFixedLagSmoother(double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
Definition: nonlinear/BatchFixedLagSmoother.h:37
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:00