gtsam  4.0.0
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
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)
8  * See LICENSE for the license information
10  * -------------------------------------------------------------------------- */
19 // \callgraph
20 #pragma once
24 #include <queue>
26 namespace gtsam {
31 class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
33 public:
34  typedef boost::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
38  struct Result {
39  size_t iterations;
40  size_t lambdas;
42  size_t linearVariables;
43  double error;
46  Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {};
49  size_t getIterations() const { return iterations; }
50  size_t getLambdas() const { return lambdas; }
51  size_t getNonlinearVariables() const { return nonlinearVariables; }
52  size_t getLinearVariables() const { return linearVariables; }
53  double getError() const { return error; }
54  };
57  ConcurrentBatchSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {};
63  virtual void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
66  virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const;
70  return factors_;
71  }
74  const Values& getLinearizationPoint() const {
75  return theta_;
76  }
79  const Ordering& getOrdering() const {
80  return ordering_;
81  }
84  const VectorValues& getDelta() const {
85  return delta_;
86  }
92  return theta_.retract(delta_);
93  }
100  template<class VALUE>
101  VALUE calculateEstimate(Key key) const {
102  const Vector delta =;
103  return<VALUE>(key).retract(delta);
104  }
120  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
121  const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none);
127  virtual void presync();
135  virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues);
146  virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
147  const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues);
153  virtual void postsync();
155 protected:
163  std::queue<size_t> availableSlots_;
165  std::vector<size_t> filterSummarizationSlots_;
167  // Storage for information to be sent to the filter
170 private:
177  std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
183  void removeFactors(const std::vector<size_t>& slots);
186  void reorder();
189  Result optimize();
192  void updateSmootherSummarization();
195  static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
196  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
199  static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
200  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
202 }; // ConcurrentBatchSmoother
208 template<>
209 struct traits<ConcurrentBatchSmoother> : public Testable<ConcurrentBatchSmoother> {
210 };
212 } //\ namespace gtsam
LevenbergMarquardtParams parameters_
LM parameters.
Definition: ConcurrentBatchSmoother.h:157
Values calculateEstimate() const
Compute the current best estimate of all variables and return a full Values structure.
Definition: ConcurrentBatchSmoother.h:91
ConcurrentBatchSmoother(const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams())
Default constructor.
Definition: ConcurrentBatchSmoother.h:57
VariableIndex variableIndex_
The current variable index, which allows efficient factor lookup by variable.
Definition: ConcurrentBatchSmoother.h:162
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
Definition: ConcurrentBatchSmoother.h:163
double error
The final factor graph error.
Definition: ConcurrentBatchSmoother.h:43
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
std::vector< size_t > filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
Definition: ConcurrentBatchSmoother.h:165
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
Definition: ConcurrentBatchSmoother.h:168
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
Template to create a binary predicate.
Definition: Testable.h:110
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: ConcurrentBatchSmoother.h:84
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult
Typedef for Matlab wrapping.
Definition: ConcurrentBatchSmoother.h:205
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
Definition: ConcurrentBatchSmoother.h:31
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
VALUE calculateEstimate(Key key) const
Compute the current best estimate of a single variable.
Definition: ConcurrentBatchSmoother.h:101
Values theta_
Current linearization point of all variables in the smoother.
Definition: ConcurrentBatchSmoother.h:159
Definition: ConcurrentBatchSmoother.h:46
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
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ConcurrentBatchSmoother.h:74
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
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentBatchSmoother.h:39
The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture.
Definition: ConcurrentFilteringAndSmoothing.h:99
ConcurrentSmoother Base
typedef for base class
Definition: ConcurrentBatchSmoother.h:35
Meta information returned about the update.
Definition: ConcurrentBatchSmoother.h:38
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
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
const Ordering & getOrdering() const
Access the current ordering.
Definition: ConcurrentBatchSmoother.h:79
Base classes for the 'filter' and 'smoother' portion of the Concurrent Filtering and Smoothing archit...
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: ConcurrentBatchSmoother.h:69
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
Definition: ConcurrentBatchSmoother.h:164
Ordering ordering_
The current ordering used to calculate the linear deltas.
Definition: ConcurrentBatchSmoother.h:160
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
virtual ~ConcurrentBatchSmoother()
Default destructor.
Definition: ConcurrentBatchSmoother.h:60
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentBatchSmoother.h:42
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentBatchSmoother.h:41
VectorValues delta_
The current set of linear deltas from the linearization point.
Definition: ConcurrentBatchSmoother.h:161
Definition: Ordering.h:34
size_t getIterations() const
Getter methods.
Definition: ConcurrentBatchSmoother.h:49
NonlinearFactorGraph factors_
The set of all factors currently in the smoother.
Definition: ConcurrentBatchSmoother.h:158
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
size_t lambdas
The number of different L-M lambda factors that were tried during optimization.
Definition: ConcurrentBatchSmoother.h:40