gtsam  4.1.0
gtsam
IncrementalFixedLagSmoother.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 
24 #include <gtsam/nonlinear/ISAM2.h>
25 
26 namespace gtsam {
27 
33 class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
34 
35 public:
36 
38  typedef boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
39 
41  IncrementalFixedLagSmoother(double smootherLag = 0.0,
42  const ISAM2Params& parameters = DefaultISAM2Params()) :
43  FixedLagSmoother(smootherLag), isam_(parameters) {
44  }
45 
48  }
49 
51  virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
52  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
53 
55  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
56 
64  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
65  const Values& newTheta = Values(), //
66  const KeyTimestampMap& timestamps = KeyTimestampMap(),
67  const FactorIndices& factorsToRemove = FactorIndices()) override;
68 
73  Values calculateEstimate() const override {
74  return isam_.calculateEstimate();
75  }
76 
83  template<class VALUE>
84  VALUE calculateEstimate(Key key) const {
85  return isam_.calculateEstimate<VALUE>(key);
86  }
87 
89  const ISAM2Params& params() const {
90  return isam_.params();
91  }
92 
95  return isam_.getFactorsUnsafe();
96  }
97 
99  const Values& getLinearizationPoint() const {
100  return isam_.getLinearizationPoint();
101  }
102 
104  const VectorValues& getDelta() const {
105  return isam_.getDelta();
106  }
107 
109  Matrix marginalCovariance(Key key) const {
110  return isam_.marginalCovariance(key);
111  }
112 
114  const ISAM2Result& getISAM2Result() const{ return isamResult_; }
115 
116 protected:
117 
120  ISAM2Params params;
121  params.findUnusedFactorSlots = true;
122  return params;
123  }
124 
128 
131 
133  void eraseKeysBefore(double timestamp);
134 
136  void createOrderingConstraints(const KeyVector& marginalizableKeys,
137  boost::optional<FastMap<Key, int> >& constrainedKeys) const;
138 
139 private:
141  static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
142  "Keys:");
143  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
144  static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
145  const std::string& label = "Factor Graph:");
146  static void PrintSymbolicTree(const gtsam::ISAM2& isam,
147  const std::string& label = "Bayes Tree:");
148  static void PrintSymbolicTreeHelper(
149  const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
150  "");
151 
152 };
153 // IncrementalFixedLagSmoother
154 
155 }
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::IncrementalFixedLagSmoother::marginalCovariance
Matrix marginalCovariance(Key key) const
Calculate marginal covariance on given variable.
Definition: IncrementalFixedLagSmoother.h:109
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::IncrementalFixedLagSmoother::DefaultISAM2Params
static ISAM2Params DefaultISAM2Params()
Create default parameters.
Definition: IncrementalFixedLagSmoother.h:119
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::ISAM2Params
Definition: ISAM2Params.h:135
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::IncrementalFixedLagSmoother::getISAM2Result
const ISAM2Result & getISAM2Result() const
Get results of latest isam2 update.
Definition: IncrementalFixedLagSmoother.h:114
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::IncrementalFixedLagSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: IncrementalFixedLagSmoother.h:94
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::IncrementalFixedLagSmoother::getDelta
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: IncrementalFixedLagSmoother.h:104
gtsam::ISAM2Params::findUnusedFactorSlots
bool findUnusedFactorSlots
When you will be removing many factors, e.g.
Definition: ISAM2Params.h:224
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::ISAM2
Definition: ISAM2.h:45
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::IncrementalFixedLagSmoother::calculateEstimate
Values calculateEstimate() const override
Compute an estimate from the incomplete linear delta computed during the last update.
Definition: IncrementalFixedLagSmoother.h:73
gtsam::IncrementalFixedLagSmoother::isam_
ISAM2 isam_
An iSAM2 object used to perform inference.
Definition: IncrementalFixedLagSmoother.h:127
gtsam::IncrementalFixedLagSmoother::isamResult_
ISAM2Result isamResult_
Store results of latest isam2 update.
Definition: IncrementalFixedLagSmoother.h:130
gtsam::FixedLagSmoother
Definition: FixedLagSmoother.h:33
gtsam::IncrementalFixedLagSmoother::IncrementalFixedLagSmoother
IncrementalFixedLagSmoother(double smootherLag=0.0, const ISAM2Params &parameters=DefaultISAM2Params())
default constructor
Definition: IncrementalFixedLagSmoother.h:41
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::IncrementalFixedLagSmoother::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition: IncrementalFixedLagSmoother.h:84
gtsam::IncrementalFixedLagSmoother
This is a base class for the various HMF2 implementations.
Definition: IncrementalFixedLagSmoother.h:33
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FastMap
Definition: FastMap.h:38
gtsam::IncrementalFixedLagSmoother::shared_ptr
boost::shared_ptr< IncrementalFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: IncrementalFixedLagSmoother.h:38
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ISAM2Result
Definition: ISAM2Result.h:41
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:82
gtsam::IncrementalFixedLagSmoother::params
const ISAM2Params & params() const
return the current set of iSAM2 parameters
Definition: IncrementalFixedLagSmoother.h:89
FixedLagSmoother.h
Base class for a fixed-lag smoother.
gtsam::IncrementalFixedLagSmoother::~IncrementalFixedLagSmoother
virtual ~IncrementalFixedLagSmoother()
destructor
Definition: IncrementalFixedLagSmoother.h:47
gtsam::IncrementalFixedLagSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: IncrementalFixedLagSmoother.h:99