gtsam  4.0.0
gtsam
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_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
30 
31 public:
32 
34  typedef boost::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  virtual ~BatchFixedLagSmoother() { };
42 
44  virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
45 
47  virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
48 
50  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
51  const Values& newTheta = Values(),
52  const KeyTimestampMap& timestamps = KeyTimestampMap(),
53  const FastVector<size_t>& factorsToRemove = FastVector<size_t>());
54 
60  return theta_.retract(delta_);
61  }
62 
69  template<class VALUE>
70  VALUE calculateEstimate(Key key) const {
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 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
121  static NonlinearFactorGraph calculateMarginalFactors(
122  const NonlinearFactorGraph& graph, const Values& theta, const std::set<Key>& keys,
123  const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky) {
124  KeyVector keyVector(keys.begin(), keys.end());
125  return CalculateMarginalFactors(graph, theta, keyVector, eliminateFunction);
126  }
127 #endif
128 
129 protected:
130 
132  typedef std::map<Key, std::set<Key> > FactorIndex;
133 
136 
141 
144 
147 
150 
153 
156 
158  std::queue<size_t> availableSlots_;
159 
162 
164  void insertFactors(const NonlinearFactorGraph& newFactors);
165 
167  void removeFactors(const std::set<size_t>& deleteFactors);
168 
170  void eraseKeys(const KeyVector& keys);
171 
173  void reorder(const KeyVector& marginalizeKeys = KeyVector());
174 
176  Result optimize();
177 
179  void marginalize(const KeyVector& marginalizableKeys);
180 
181 private:
183  static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
184  static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
185  static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
186  static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
187  static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
188  static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
189 }; // BatchFixedLagSmoother
190 
191 }
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: BatchFixedLagSmoother.h:91
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
NonlinearFactorGraph factors_
The nonlinear factors.
Definition: BatchFixedLagSmoother.h:143
Meta information returned about the update.
Definition: FixedLagSmoother.h:48
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition: BatchFixedLagSmoother.h:70
Values theta_
The current linearization point.
Definition: BatchFixedLagSmoother.h:146
Ordering ordering_
The current ordering.
Definition: BatchFixedLagSmoother.h:152
boost::shared_ptr< BatchFixedLagSmoother > shared_ptr
Typedef for a shared pointer to an Incremental Fixed-Lag Smoother.
Definition: BatchFixedLagSmoother.h:34
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
Definition: FixedLagSmoother.h:33
Template to create a binary predicate.
Definition: Testable.h:110
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: BatchFixedLagSmoother.h:101
Base class for a fixed-lag smoother.
std::queue< size_t > availableSlots_
The set of available factor graph slots.
Definition: BatchFixedLagSmoother.h:158
LevenbergMarquardtParams & params()
update the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:81
const LevenbergMarquardtParams & params() const
read the current set of optimizer parameters
Definition: BatchFixedLagSmoother.h:76
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:33
virtual ~BatchFixedLagSmoother()
destructor
Definition: BatchFixedLagSmoother.h:41
Definition: BatchFixedLagSmoother.h:29
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:33
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
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
LevenbergMarquardtParams parameters_
The L-M optimization parameters.
Definition: BatchFixedLagSmoother.h:135
Values calculateEstimate() const
Compute an estimate from the incomplete linear delta computed during the last update.
Definition: BatchFixedLagSmoother.h:59
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:102
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:77
bool enforceConsistency_
A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the linear...
Definition: BatchFixedLagSmoother.h:140
Values linearKeys_
The set of keys involved in current linear factors.
Definition: BatchFixedLagSmoother.h:149
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FactorIndex factorIndex_
A cross-reference structure to allow efficient factor lookups by key.
Definition: BatchFixedLagSmoother.h:161
BatchFixedLagSmoother(double smootherLag=0.0, const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams(), bool enforceConsistency=true)
default constructor
Definition: BatchFixedLagSmoother.h:37
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
boost::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
const Ordering & getOrdering() const
Access the current ordering.
Definition: BatchFixedLagSmoother.h:96
VectorValues delta_
The current set of linear deltas.
Definition: BatchFixedLagSmoother.h:155
Definition: Ordering.h:34
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: BatchFixedLagSmoother.h:86
std::map< Key, std::set< Key > > FactorIndex
A typedef defining an Key-Factor mapping.
Definition: BatchFixedLagSmoother.h:132
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42