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 
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 }
relicense.update
def update(text)
Definition: relicense.py:46
gtsam::Result
Result from elimination.
Definition: HybridGaussianFactorGraph.cpp:61
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::FixedLagSmoother::Result::getError
double getError() const
Definition: nonlinear/FixedLagSmoother.h:61
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::FixedLagSmoother::timestamps
const KeyTimestampMap & timestamps() const
Definition: nonlinear/FixedLagSmoother.h:90
gtsam::FixedLagSmoother::Result::getIntermediateSteps
size_t getIntermediateSteps() const
Definition: nonlinear/FixedLagSmoother.h:58
gtsam::FixedLagSmoother::Result::Result
Result()
Definition: nonlinear/FixedLagSmoother.h:54
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::FixedLagSmoother::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: nonlinear/FixedLagSmoother.h:52
gtsam::FixedLagSmoother::TimestampKeyMap
std::multimap< double, Key > TimestampKeyMap
Definition: nonlinear/FixedLagSmoother.h:42
gtsam::FixedLagSmootherKeyTimestampMap
FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap
Typedef for matlab wrapping.
Definition: nonlinear/FixedLagSmoother.h:134
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::FixedLagSmootherKeyTimestampMapValue
FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue
Definition: nonlinear/FixedLagSmoother.h:135
Key.h
gtsam::FixedLagSmoother::timestampKeyMap_
TimestampKeyMap timestampKeyMap_
Definition: nonlinear/FixedLagSmoother.h:113
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::FixedLagSmoother
Definition: nonlinear/FixedLagSmoother.h:33
gtsam::FixedLagSmootherResult
FixedLagSmoother::Result FixedLagSmootherResult
Definition: nonlinear/FixedLagSmoother.h:136
gtsam::FixedLagSmoother::Result::getNonlinearVariables
size_t getNonlinearVariables() const
Definition: nonlinear/FixedLagSmoother.h:59
gtsam::FixedLagSmoother::smootherLag_
double smootherLag_
Definition: nonlinear/FixedLagSmoother.h:110
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
gtsam::FixedLagSmoother::Result
Definition: nonlinear/FixedLagSmoother.h:48
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::FixedLagSmoother::~FixedLagSmoother
virtual ~FixedLagSmoother()
Definition: nonlinear/FixedLagSmoother.h:69
gtsam::FixedLagSmoother::Result::getLinearVariables
size_t getLinearVariables() const
Definition: nonlinear/FixedLagSmoother.h:60
gtsam::FixedLagSmoother::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: nonlinear/FixedLagSmoother.h:49
gtsam::FixedLagSmoother::Result::intermediateSteps
size_t intermediateSteps
The number of intermediate steps performed within the optimization. For L-M, this is the number of la...
Definition: nonlinear/FixedLagSmoother.h:50
gtsam::FixedLagSmoother::smootherLag
double smootherLag() const
Definition: nonlinear/FixedLagSmoother.h:80
gtsam::FixedLagSmoother::Result::error
double error
The final factor graph error.
Definition: nonlinear/FixedLagSmoother.h:53
gtsam::equals
Definition: Testable.h:112
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam::FixedLagSmoother::KeyTimestampMap
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
Definition: nonlinear/FixedLagSmoother.h:41
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::FixedLagSmoother::smootherLag
double & smootherLag()
Definition: nonlinear/FixedLagSmoother.h:85
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::FixedLagSmoother::FixedLagSmoother
FixedLagSmoother(double smootherLag=0.0)
Definition: nonlinear/FixedLagSmoother.h:66
gtsam::FixedLagSmoother::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: nonlinear/FixedLagSmoother.h:51
gtsam::FixedLagSmoother::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: nonlinear/FixedLagSmoother.h:57
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FixedLagSmoother::keyTimestampMap_
KeyTimestampMap keyTimestampMap_
Definition: nonlinear/FixedLagSmoother.h:114
gtsam::FixedLagSmoother::shared_ptr
std::shared_ptr< FixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: nonlinear/FixedLagSmoother.h:38
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


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