ConcurrentIncrementalFilter.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 
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 std::shared_ptr<ConcurrentIncrementalFilter> shared_ptr;
36 
38  struct Result {
39  size_t iterations;
41  size_t linearVariables;
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 
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>
109  return isam2_.calculateEstimate<VALUE>(key);
110  }
111 
125  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
126  const std::optional<FastList<Key> >& keysToMove = {},
127  const std::optional< FactorIndices >& removeFactorIndices = {});
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::ConcurrentIncrementalFilter::getDelta
const VectorValues & getDelta() const
Definition: ConcurrentIncrementalFilter.h:91
gtsam::ISAM2Params
Definition: ISAM2Params.h:136
relicense.update
def update(text)
Definition: relicense.py:46
gtsam::ConcurrentIncrementalFilter::Result
Definition: ConcurrentIncrementalFilter.h:38
gtsam::Result
Result from elimination.
Definition: HybridGaussianFactorGraph.cpp:61
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ConcurrentIncrementalFilter::getISAM2
const ISAM2 & getISAM2() const
Definition: ConcurrentIncrementalFilter.h:81
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::ConcurrentIncrementalFilter::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentIncrementalFilter.h:40
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::synchronize
void synchronize(ConcurrentFilter &filter, ConcurrentSmoother &smoother)
Definition: ConcurrentFilteringAndSmoothing.cpp:28
gtsam::ConcurrentIncrementalFilter::Result::variablesRelinearized
size_t variablesRelinearized
Definition: ConcurrentIncrementalFilter.h:43
Values
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::ConcurrentIncrementalFilter::Result::error
double error
The final factor graph error.
Definition: ConcurrentIncrementalFilter.h:51
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::ConcurrentIncrementalFilter::Result::newFactorsIndices
FactorIndices newFactorsIndices
Definition: ConcurrentIncrementalFilter.h:49
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::ConcurrentIncrementalFilter::isam2_
ISAM2 isam2_
The iSAM2 inference engine.
Definition: ConcurrentIncrementalFilter.h:169
gtsam::ConcurrentFilter
Definition: ConcurrentFilteringAndSmoothing.h:39
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
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
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::ConcurrentIncrementalFilter::Base
ConcurrentFilter Base
typedef for base class
Definition: ConcurrentIncrementalFilter.h:35
gtsam::KeyFormatter
std::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
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::ConcurrentIncrementalFilter::calculateEstimate
Values calculateEstimate() const
Definition: ConcurrentIncrementalFilter.h:98
gtsam::ConcurrentIncrementalFilter::~ConcurrentIncrementalFilter
~ConcurrentIncrementalFilter() override
Definition: ConcurrentIncrementalFilter.h:67
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::ConcurrentIncrementalFilter::shared_ptr
std::shared_ptr< ConcurrentIncrementalFilter > shared_ptr
Definition: ConcurrentIncrementalFilter.h:34
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::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::ConcurrentIncrementalFilter::Result::Result
Result()
Constructor.
Definition: ConcurrentIncrementalFilter.h:54
gtsam::ConcurrentIncrementalFilter::calculateEstimate
VALUE calculateEstimate(Key key) const
Definition: ConcurrentIncrementalFilter.h:108
gtsam::FastList
Definition: FastList.h:43
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::ISAM2Clique::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
gtsam::ConcurrentIncrementalFilter::Result::getError
double getError() const
Definition: ConcurrentIncrementalFilter.h:60
gtsam::ConcurrentIncrementalFilter::Result::variablesReeliminated
size_t variablesReeliminated
Definition: ConcurrentIncrementalFilter.h:42
gtsam::ConcurrentIncrementalFilter::Result::getNonlinearVariables
size_t getNonlinearVariables() const
Definition: ConcurrentIncrementalFilter.h:58
gtsam::ConcurrentIncrementalFilter::getLinearizationPoint
const Values & getLinearizationPoint() const
Definition: ConcurrentIncrementalFilter.h:86
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::ConcurrentIncrementalFilter::ConcurrentIncrementalFilter
ConcurrentIncrementalFilter(const ISAM2Params &parameters=ISAM2Params())
Definition: ConcurrentIncrementalFilter.h:64
gtsam::Values
Definition: Values.h:65
gtsam::ConcurrentIncrementalFilter::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentIncrementalFilter.h:41
gtsam::ConcurrentIncrementalFilter::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentIncrementalFilter.h:39
ConcurrentFilteringAndSmoothing.h
Base classes for the 'filter' and 'smoother' portion of the Concurrent Filtering and Smoothing archit...
gtsam::ConcurrentIncrementalFilter::getFactors
const NonlinearFactorGraph & getFactors() const
Definition: ConcurrentIncrementalFilter.h:76
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::ConcurrentIncrementalFilter
Definition: ConcurrentIncrementalFilter.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::ConcurrentIncrementalFilter::Result::getLinearVariables
size_t getLinearVariables() const
Definition: ConcurrentIncrementalFilter.h:59
gtsam::ConcurrentIncrementalFilter::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: ConcurrentIncrementalFilter.h:57
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:09