gtsam  4.1.0
gtsam
ConcurrentIncrementalFilter.h
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 
23 #include <gtsam/nonlinear/ISAM2.h>
24 
25 namespace gtsam {
26 
30 class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual ConcurrentFilter {
31 
32 public:
33 
34  typedef boost::shared_ptr<ConcurrentIncrementalFilter> shared_ptr;
36 
38  struct Result {
39  size_t iterations;
41  size_t linearVariables;
42  size_t variablesReeliminated;
43  size_t variablesRelinearized;
44 
50 
51  double error;
52 
54  Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {};
55 
57  size_t getIterations() const { return iterations; }
58  size_t getNonlinearVariables() const { return nonlinearVariables; }
59  size_t getLinearVariables() const { return linearVariables; }
60  double getError() const { return error; }
61  };
62 
64  ConcurrentIncrementalFilter(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};
65 
68 
70  void print(const std::string& s = "Concurrent Incremental Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
71 
73  bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
74 
77  return isam2_.getFactorsUnsafe();
78  }
79 
81  const ISAM2& getISAM2() const {
82  return isam2_;
83  }
84 
86  const Values& getLinearizationPoint() const {
87  return isam2_.getLinearizationPoint();
88  }
89 
91  const VectorValues& getDelta() const {
92  return isam2_.getDelta();
93  }
94 
99  return isam2_.calculateEstimate();
100  }
101 
107  template<class VALUE>
108  VALUE calculateEstimate(Key key) const {
109  return isam2_.calculateEstimate<VALUE>(key);
110  }
111 
125  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
126  const boost::optional<FastList<Key> >& keysToMove = boost::none,
127  const boost::optional< FactorIndices >& removeFactorIndices = boost::none);
128 
133  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  void postsync() override;
166 
167 protected:
168 
170 
171  // ???
175 
176  // Storage for information to be sent to the smoother
179 
180 private:
181 
183  static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
184 
186  static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
187 
189  // TODO: Make this a static function
190  void updateShortcut(const NonlinearFactorGraph& removedFactors);
191 
193  // TODO: Make this a static function
194  NonlinearFactorGraph calculateFilterSummarization() const;
195 
196 }; // ConcurrentBatchFilter
197 
200 
202 template<>
203 struct traits<ConcurrentIncrementalFilter> : public Testable<ConcurrentIncrementalFilter> {
204 };
205 
206 } //\ namespace gtsam
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::ConcurrentIncrementalFilter::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ConcurrentIncrementalFilter.h:86
gtsam::ConcurrentIncrementalFilter::currentSmootherSummarizationSlots_
FactorIndices currentSmootherSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
Definition: ConcurrentIncrementalFilter.h:173
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::ConcurrentIncrementalFilter::smootherFactors_
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother.
Definition: ConcurrentIncrementalFilter.h:177
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::ISAM2Params
Definition: ISAM2Params.h:135
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::ConcurrentIncrementalFilter::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute the current best estimate of a single variable.
Definition: ConcurrentIncrementalFilter.h:108
gtsam::ConcurrentIncrementalFilter
An iSAM2-based Batch Filter that implements the Concurrent Filtering and Smoother interface.
Definition: ConcurrentIncrementalFilter.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ConcurrentIncrementalFilter::ConcurrentIncrementalFilter
ConcurrentIncrementalFilter(const ISAM2Params &parameters=ISAM2Params())
Default constructor.
Definition: ConcurrentIncrementalFilter.h:64
gtsam::ConcurrentIncrementalFilter::getISAM2
const ISAM2 & getISAM2() const
Access the current linearization point.
Definition: ConcurrentIncrementalFilter.h:81
gtsam::ConcurrentIncrementalFilter::getFactors
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: ConcurrentIncrementalFilter.h:76
gtsam::ConcurrentIncrementalFilter::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentIncrementalFilter.h:39
gtsam::ConcurrentIncrementalFilter::~ConcurrentIncrementalFilter
virtual ~ConcurrentIncrementalFilter()
Default destructor.
Definition: ConcurrentIncrementalFilter.h:67
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ConcurrentIncrementalFilter::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: ConcurrentIncrementalFilter.h:57
gtsam::FastList
Definition: FastList.h:38
gtsam::ConcurrentIncrementalFilterResult
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult
Typedef for Matlab wrapping.
Definition: ConcurrentIncrementalFilter.h:199
gtsam::ConcurrentIncrementalFilter::previousSmootherSummarization_
NonlinearFactorGraph previousSmootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization.
Definition: ConcurrentIncrementalFilter.h:172
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ConcurrentIncrementalFilter::smootherValues_
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother.
Definition: ConcurrentIncrementalFilter.h:178
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::ConcurrentIncrementalFilter::Result::newFactorsIndices
FactorIndices newFactorsIndices
The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
Definition: ConcurrentIncrementalFilter.h:49
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::ConcurrentIncrementalFilter::smootherShortcut_
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
Definition: ConcurrentIncrementalFilter.h:174
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ConcurrentIncrementalFilter::Base
ConcurrentFilter Base
typedef for base class
Definition: ConcurrentIncrementalFilter.h:35
gtsam::ConcurrentIncrementalFilter::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentIncrementalFilter.h:40
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::ConcurrentIncrementalFilter::getDelta
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: ConcurrentIncrementalFilter.h:91
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::ConcurrentIncrementalFilter::Result
Meta information returned about the update.
Definition: ConcurrentIncrementalFilter.h:38
gtsam::ConcurrentIncrementalFilter::isam2_
ISAM2 isam2_
The iSAM2 inference engine.
Definition: ConcurrentIncrementalFilter.h:169
gtsam::ConcurrentIncrementalFilter::Result::error
double error
The final factor graph error.
Definition: ConcurrentIncrementalFilter.h:51
gtsam::ConcurrentIncrementalFilter::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentIncrementalFilter.h:41
gtsam::ConcurrentIncrementalFilter::calculateEstimate
Values calculateEstimate() const
Compute the current best estimate of all variables and return a full Values structure.
Definition: ConcurrentIncrementalFilter.h:98
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ConcurrentIncrementalFilter::Result::Result
Result()
Constructor.
Definition: ConcurrentIncrementalFilter.h:54
gtsam::ConcurrentFilter
The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture.
Definition: ConcurrentFilteringAndSmoothing.h:39