ConcurrentIncrementalSmoother.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 ConcurrentIncrementalSmoother : public virtual ConcurrentSmoother {
31 
32 public:
33  typedef std::shared_ptr<ConcurrentIncrementalSmoother> shared_ptr;
35 
37  struct Result {
38  size_t iterations;
40  size_t linearVariables;
41  double error;
42 
44  Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}
45 
47  size_t getIterations() const { return iterations; }
48  size_t getNonlinearVariables() const { return nonlinearVariables; }
49  size_t getLinearVariables() const { return linearVariables; }
50  double getError() const { return error; }
51  };
52 
55 
58 
60  void print(const std::string& s = "Concurrent Incremental Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
61 
63  bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
64 
67  return isam2_.getFactorsUnsafe();
68  }
69 
71  const Values& getLinearizationPoint() const {
72  return isam2_.getLinearizationPoint();
73  }
74 
76  const VectorValues& getDelta() const {
77  return isam2_.getDelta();
78  }
79 
84  return isam2_.calculateEstimate();
85  }
86 
92  template<class VALUE>
94  return isam2_.calculateEstimate<VALUE>(key);
95  }
96 
111  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
112  const std::optional<FactorIndices>& removeFactorIndices = {});
113 
118  void presync() override;
119 
126  void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
127 
137  void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
138  const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
139 
144  void postsync() override;
145 
146 protected:
147 
149 
150  // Storage for information received from the filter during the last synchronization
157 
158  // Storage for information to be sent to the filter
160 
161 private:
162 
164  void updateSmootherSummarization();
165 
166 }; // ConcurrentBatchSmoother
167 
170 
172 template<>
173 struct traits<ConcurrentIncrementalSmoother> : public Testable<ConcurrentIncrementalSmoother> {
174 };
175 
176 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
size_t linearVariables
The number of variables that must keep a constant linearization point.
Values smootherValues_
New variables to be added to the smoother during the next update.
size_t iterations
The number of optimizer iterations performed.
def update(text)
Definition: relicense.py:46
FactorIndices filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
size_t nonlinearVariables
The number of variables that can be relinearized.
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
NonlinearFactorGraph smootherFactors_
New factors to be added to the smoother during the next update.
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult
Typedef for Matlab wrapping.
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
std::shared_ptr< ConcurrentIncrementalSmoother > shared_ptr
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
ConcurrentIncrementalSmoother(const ISAM2Params &parameters=ISAM2Params())
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
static ConjugateGradientParameters parameters
traits
Definition: chartTesting.h:28
NonlinearFactorGraph filterSummarizationFactors_
New filter summarization factors to replace the existing filter summarization during the next update...
std::vector< float > Values
Values separatorValues_
The linearization points of the separator variables. These should not be changed during optimization...
void synchronize(ConcurrentFilter &filter, ConcurrentSmoother &smoother)
const NonlinearFactorGraph & getFactors() const
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86
Base classes for the &#39;filter&#39; and &#39;smoother&#39; portion of the Concurrent Filtering and Smoothing archit...
bool synchronizationUpdatesAvailable_
Flag indicating the currently stored synchronization updates have not been applied yet...
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
ConcurrentSmoother Base
typedef for base class


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