nonlinear/FixedLagSmoother.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 
23 #include <gtsam/dllexport.h>
24 #include <gtsam/inference/Key.h>
26 #include <gtsam/nonlinear/Values.h>
27 
28 #include <map>
29 #include <vector>
30 
31 namespace gtsam {
32 
33 class GTSAM_EXPORT FixedLagSmoother {
34 
35 public:
36 
38  typedef std::shared_ptr<FixedLagSmoother> shared_ptr;
39 
41  typedef std::map<Key, double> KeyTimestampMap;
42  typedef std::multimap<double, Key> TimestampKeyMap;
43 
47  // TODO: Think of some more things to put here
48  struct Result {
49  size_t iterations;
52  size_t linearVariables;
53  double error;
54  Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}
55 
57  size_t getIterations() const { return iterations; }
58  size_t getIntermediateSteps() const { return intermediateSteps; }
59  size_t getNonlinearVariables() const { return nonlinearVariables; }
60  size_t getLinearVariables() const { return linearVariables; }
61  double getError() const { return error; }
62  void print() const;
63  };
64 
66  FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
67 
69  virtual ~FixedLagSmoother() { }
70 
72  virtual void print(
73  const std::string& s = "FixedLagSmoother:\n",
74  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
75 
77  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
78 
80  double smootherLag() const {
81  return smootherLag_;
82  }
83 
85  double& smootherLag() {
86  return smootherLag_;
87  }
88 
90  const KeyTimestampMap& timestamps() const {
91  return keyTimestampMap_;
92  }
93 
95  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
96  const Values& newTheta = Values(),
97  const KeyTimestampMap& timestamps = KeyTimestampMap(),
98  const FactorIndices& factorsToRemove = FactorIndices()) = 0;
99 
104  virtual Values calculateEstimate() const = 0;
105 
106 
107 protected:
108 
110  double smootherLag_;
111 
113  TimestampKeyMap timestampKeyMap_;
114  KeyTimestampMap keyTimestampMap_;
115 
117  void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
118 
120  void eraseKeyTimestampMap(const KeyVector& keys);
121 
123  double getCurrentTimestamp() const;
124 
126  KeyVector findKeysBefore(double timestamp) const;
127 
129  KeyVector findKeysAfter(double timestamp) const;
130 
131 }; // FixedLagSmoother
132 
135 typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
137 
138 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
size_t iterations
The number of optimizer iterations performed.
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
size_t linearVariables
The number of variables that must keep a constant linearization point.
def update(text)
Definition: relicense.py:46
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
double error
The final factor graph error.
FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue
size_t getIterations() const
Getter methods.
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
size_t intermediateSteps
The number of intermediate steps performed within the optimization. For L-M, this is the number of la...
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
std::multimap< double, Key > TimestampKeyMap
FixedLagSmoother::Result FixedLagSmootherResult
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
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
size_t nonlinearVariables
The number of variables that can be relinearized.
traits
Definition: chartTesting.h:28
FixedLagSmoother(double smootherLag=0.0)
std::vector< float > Values
const KeyTimestampMap & timestamps() const
FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap
Typedef for matlab wrapping.
static double error
Definition: testRot3.cpp:37
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::shared_ptr< FixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:13