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
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
std::shared_ptr< ConcurrentIncrementalFilter > shared_ptr
size_t nonlinearVariables
The number of variables that can be relinearized.
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult
Typedef for Matlab wrapping.
def update(text)
Definition: relicense.py:46
ISAM2 isam2_
The iSAM2 inference engine.
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
NonlinearFactorGraph previousSmootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization...
double error
The final factor graph error.
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
ConcurrentFilter Base
typedef for base class
ConcurrentIncrementalFilter(const ISAM2Params &parameters=ISAM2Params())
const NonlinearFactorGraph & getFactors() const
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
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
size_t iterations
The number of optimizer iterations performed.
static ConjugateGradientParameters parameters
traits
Definition: chartTesting.h:28
std::vector< float > Values
void synchronize(ConcurrentFilter &filter, ConcurrentSmoother &smoother)
FactorIndices currentSmootherSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
size_t linearVariables
The number of variables that must keep a constant linearization point.
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
const KeyVector keys
Base classes for the &#39;filter&#39; and &#39;smoother&#39; portion of the Concurrent Filtering and Smoothing archit...
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother...
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother...


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:03