gtsam  4.1.0
gtsam
ConcurrentBatchFilter.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 
19 // \callgraph
20 #pragma once
21 
24 #include <queue>
25 
26 namespace gtsam {
27 
31 class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter {
32 
33 public:
34  typedef boost::shared_ptr<ConcurrentBatchFilter> shared_ptr;
36 
38  struct Result {
39  size_t iterations;
40  size_t lambdas;
42  size_t linearVariables;
43 
48  std::vector<size_t> newFactorsIndices;
49 
50  double error;
51 
53  Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {};
54 
56  size_t getIterations() const { return iterations; }
57  size_t getLambdas() const { return lambdas; }
58  size_t getNonlinearVariables() const { return nonlinearVariables; }
59  size_t getLinearVariables() const { return linearVariables; }
60  double getError() const { return error; }
61  };
62 
64  ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {};
65 
67  virtual ~ConcurrentBatchFilter() {};
68 
70  void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
71 
73  bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
74 
77  return factors_;
78  }
79 
81  const Values& getLinearizationPoint() const {
82  return theta_;
83  }
84 
86  const Ordering& getOrdering() const {
87  return ordering_;
88  }
89 
91  const VectorValues& getDelta() const {
92  return delta_;
93  }
94 
99  return theta_.retract(delta_);
100  }
101 
107  template<class VALUE>
108  VALUE calculateEstimate(Key key) const {
109  const Vector delta = delta_.at(key);
110  return theta_.at<VALUE>(key).retract(delta);
111  }
112 
126  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
127  const boost::optional<FastList<Key> >& keysToMove = boost::none, const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none);
128 
133  virtual void presync() override;
134 
142  void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
143 
152  void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
153 
159  void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
160 
165  virtual void postsync() override;
166 
167 protected:
168 
174  std::queue<size_t> availableSlots_;
176  std::vector<size_t> separatorSummarizationSlots_;
177 
178  // Storage for information from the Smoother
181 
182  // Storage for information to be sent to the smoother
186 
187 private:
188 
194  std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
195 
200  void removeFactors(const std::vector<size_t>& slots);
201 
203  void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
204 
210  void moveSeparator(const FastList<Key>& keysToMove);
211 
213  static void optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
214  VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters,
215  Result& result);
216 
218  static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
219  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
220 
222  static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
223  const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
224 
226  static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
227  const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
228 
230  static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
231  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
232 
234  static void PrintLinearFactorGraph(const GaussianFactorGraph& factors,
235  const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
236 
238  template<class Container>
239  static void PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
240 
241 }; // ConcurrentBatchFilter
242 
244 template<class Container>
245 void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
246  std::cout << indent << title;
247  for(Key key: keys) {
248  std::cout << " " << keyFormatter(key);
249  }
250  std::cout << std::endl;
251 }
252 
255 
257 template<>
258 struct traits<ConcurrentBatchFilter> : public Testable<ConcurrentBatchFilter> {
259 };
260 
261 } // \ namespace gtsam
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::ConcurrentBatchFilter::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute the current best estimate of a single variable.
Definition: ConcurrentBatchFilter.h:108
gtsam::ConcurrentBatchFilter::getOrdering
const Ordering & getOrdering() const
Access the current ordering.
Definition: ConcurrentBatchFilter.h:86
gtsam::ConcurrentBatchFilter::Result::lambdas
size_t lambdas
The number of different L-M lambda factors that were tried during optimization.
Definition: ConcurrentBatchFilter.h:40
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::ConcurrentBatchFilter::smootherFactors_
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
Definition: ConcurrentBatchFilter.h:184
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
gtsam::Ordering
Definition: Ordering.h:34
gtsam::ConcurrentBatchFilter::smootherValues_
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother.
Definition: ConcurrentBatchFilter.h:185
gtsam::ConcurrentBatchFilter::ordering_
Ordering ordering_
The current ordering used to calculate the linear deltas.
Definition: ConcurrentBatchFilter.h:172
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::ConcurrentBatchFilter::smootherShortcut_
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
Definition: ConcurrentBatchFilter.h:180
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ConcurrentBatchFilter::factors_
NonlinearFactorGraph factors_
The set of all factors currently in the filter.
Definition: ConcurrentBatchFilter.h:170
gtsam::ConcurrentBatchFilter::Result::newFactorsIndices
std::vector< size_t > newFactorsIndices
The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
Definition: ConcurrentBatchFilter.h:48
gtsam::ConcurrentBatchFilter::smootherSummarization_
NonlinearFactorGraph smootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization.
Definition: ConcurrentBatchFilter.h:179
gtsam::ConcurrentBatchFilter::Result
Meta information returned about the update.
Definition: ConcurrentBatchFilter.h:38
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ConcurrentBatchFilter::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ConcurrentBatchFilter.h:81
gtsam::ConcurrentBatchFilter::calculateEstimate
Values calculateEstimate() const
Compute the current best estimate of all variables and return a full Values structure.
Definition: ConcurrentBatchFilter.h:98
gtsam::FastList
Definition: FastList.h:38
gtsam::ConcurrentBatchFilter::theta_
Values theta_
Current linearization point of all variables in the filter.
Definition: ConcurrentBatchFilter.h:171
gtsam::ConcurrentBatchFilter::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: ConcurrentBatchFilter.h:56
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ConcurrentBatchFilter::delta_
VectorValues delta_
The current set of linear deltas from the linearization point.
Definition: ConcurrentBatchFilter.h:173
gtsam::ConcurrentBatchFilter::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentBatchFilter.h:42
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
ConcurrentFilteringAndSmoothing.h
Base classes for the 'filter' and 'smoother' portion of the Concurrent Filtering and Smoothing archit...
gtsam::ConcurrentBatchFilter::availableSlots_
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
Definition: ConcurrentBatchFilter.h:174
gtsam::ConcurrentBatchFilter::parameters_
LevenbergMarquardtParams parameters_
LM parameters.
Definition: ConcurrentBatchFilter.h:169
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition: Values.cpp:109
gtsam::ConcurrentBatchFilter::Result::error
double error
The final factor graph error.
Definition: ConcurrentBatchFilter.h:50
gtsam::ConcurrentBatchFilter
A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface.
Definition: ConcurrentBatchFilter.h:31
gtsam::ConcurrentBatchFilter::ConcurrentBatchFilter
ConcurrentBatchFilter(const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams())
Default constructor.
Definition: ConcurrentBatchFilter.h:64
gtsam::ConcurrentBatchFilter::getFactors
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: ConcurrentBatchFilter.h:76
gtsam::ConcurrentBatchFilterResult
ConcurrentBatchFilter::Result ConcurrentBatchFilterResult
Typedef for Matlab wrapping.
Definition: ConcurrentBatchFilter.h:254
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::ConcurrentBatchFilter::separatorSummarizationSlots_
std::vector< size_t > separatorSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
Definition: ConcurrentBatchFilter.h:176
gtsam::ConcurrentBatchFilter::filterSummarization_
NonlinearFactorGraph filterSummarization_
A temporary holding place for calculated filter summarization factors to be sent to the smoother.
Definition: ConcurrentBatchFilter.h:183
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition: LevenbergMarquardtParams.h:33
gtsam::ConcurrentBatchFilter::getDelta
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: ConcurrentBatchFilter.h:91
gtsam::ConcurrentBatchFilter::Result::Result
Result()
Constructor.
Definition: ConcurrentBatchFilter.h:53
gtsam::ConcurrentBatchFilter::separatorValues_
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
Definition: ConcurrentBatchFilter.h:175
gtsam::ConcurrentBatchFilter::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentBatchFilter.h:39
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ConcurrentBatchFilter::Base
ConcurrentFilter Base
typedef for base class
Definition: ConcurrentBatchFilter.h:35
gtsam::ConcurrentBatchFilter::~ConcurrentBatchFilter
virtual ~ConcurrentBatchFilter()
Default destructor.
Definition: ConcurrentBatchFilter.h:67
gtsam::ConcurrentFilter
The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture.
Definition: ConcurrentFilteringAndSmoothing.h:39
gtsam::ConcurrentBatchFilter::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentBatchFilter.h:41