gtsam  4.1.0
gtsam
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_unstable/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_UNSTABLE_EXPORT FixedLagSmoother {
34 
35 public:
36 
38  typedef boost::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(const std::string& s = "FixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
73 
75  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
76 
78  double smootherLag() const {
79  return smootherLag_;
80  }
81 
83  double& smootherLag() {
84  return smootherLag_;
85  }
86 
88  const KeyTimestampMap& timestamps() const {
89  return keyTimestampMap_;
90  }
91 
93  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
94  const Values& newTheta = Values(),
95  const KeyTimestampMap& timestamps = KeyTimestampMap(),
96  const FactorIndices& factorsToRemove = FactorIndices()) = 0;
97 
102  virtual Values calculateEstimate() const = 0;
103 
104 
105 protected:
106 
108  double smootherLag_;
109 
111  TimestampKeyMap timestampKeyMap_;
112  KeyTimestampMap keyTimestampMap_;
113 
115  void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
116 
118  void eraseKeyTimestampMap(const KeyVector& keys);
119 
121  double getCurrentTimestamp() const;
122 
124  KeyVector findKeysBefore(double timestamp) const;
125 
127  KeyVector findKeysAfter(double timestamp) const;
128 
129 }; // FixedLagSmoother
130 
133 typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
135 
136 }
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::FixedLagSmoother::Result
Meta information returned about the update.
Definition: FixedLagSmoother.h:48
gtsam::FixedLagSmoother::Result::error
double error
The final factor graph error.
Definition: FixedLagSmoother.h:53
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::FixedLagSmoother::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: FixedLagSmoother.h:52
gtsam::FixedLagSmoother::smootherLag
double & smootherLag()
write to the current smoother lag
Definition: FixedLagSmoother.h:83
gtsam::FixedLagSmoother::shared_ptr
boost::shared_ptr< FixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: FixedLagSmoother.h:38
gtsam::FixedLagSmoother::smootherLag_
double smootherLag_
The length of the smoother lag.
Definition: FixedLagSmoother.h:108
gtsam::FixedLagSmoother::timestamps
const KeyTimestampMap & timestamps() const
Access the current set of timestamps associated with each variable.
Definition: FixedLagSmoother.h:88
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::FixedLagSmoother::FixedLagSmoother
FixedLagSmoother(double smootherLag=0.0)
default constructor
Definition: FixedLagSmoother.h:66
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::FixedLagSmootherKeyTimestampMap
FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap
Typedef for matlab wrapping.
Definition: FixedLagSmoother.h:132
gtsam::FixedLagSmoother::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: FixedLagSmoother.h:49
gtsam::FixedLagSmoother::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: FixedLagSmoother.h:51
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::FixedLagSmoother::update
virtual Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const KeyTimestampMap &timestamps=KeyTimestampMap(), const FactorIndices &factorsToRemove=FactorIndices())=0
Add new factors, updating the solution and relinearizing as needed.
Key.h
gtsam::FixedLagSmoother
Definition: FixedLagSmoother.h:33
gtsam::FixedLagSmoother::KeyTimestampMap
std::map< Key, double > KeyTimestampMap
Typedef for a Key-Timestamp map/database.
Definition: FixedLagSmoother.h:41
gtsam::FixedLagSmoother::~FixedLagSmoother
virtual ~FixedLagSmoother()
destructor
Definition: FixedLagSmoother.h:69
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::FixedLagSmoother::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: FixedLagSmoother.h:57
gtsam::FixedLagSmoother::timestampKeyMap_
TimestampKeyMap timestampKeyMap_
The current timestamp associated with each tracked key.
Definition: FixedLagSmoother.h:111
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FixedLagSmoother::calculateEstimate
virtual Values calculateEstimate() const =0
Compute an estimate from the incomplete linear delta computed during the last update.
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
Values.h
A non-templated config holding any types of Manifold-group elements.
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: FixedLagSmoother.h:50
gtsam::FixedLagSmoother::smootherLag
double smootherLag() const
read the current smoother lag
Definition: FixedLagSmoother.h:78