gtsam  4.1.0
gtsam
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 boost::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 
54  ConcurrentIncrementalSmoother(const ISAM2Params& parameters = ISAM2Params()) : isam2_(parameters) {};
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>
93  VALUE calculateEstimate(Key key) const {
94  return isam2_.calculateEstimate<VALUE>(key);
95  }
96 
111  Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
112  const boost::optional<FactorIndices>& removeFactorIndices = boost::none);
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
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::ConcurrentIncrementalSmoother::Base
ConcurrentSmoother Base
typedef for base class
Definition: ConcurrentIncrementalSmoother.h:34
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::ConcurrentIncrementalSmoother::isam2_
ISAM2 isam2_
iSAM2 inference engine
Definition: ConcurrentIncrementalSmoother.h:148
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
gtsam::ConcurrentIncrementalSmoother::getDelta
const VectorValues & getDelta() const
Access the current set of deltas to the linearization point.
Definition: ConcurrentIncrementalSmoother.h:76
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ConcurrentIncrementalSmoother::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: ConcurrentIncrementalSmoother.h:47
gtsam::ConcurrentIncrementalSmoother::Result::error
double error
The final factor graph error.
Definition: ConcurrentIncrementalSmoother.h:41
gtsam::ConcurrentIncrementalSmoother::Result
Meta information returned about the update.
Definition: ConcurrentIncrementalSmoother.h:37
gtsam::ConcurrentIncrementalSmoother::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute the current best estimate of a single variable.
Definition: ConcurrentIncrementalSmoother.h:93
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::ConcurrentIncrementalSmoother::filterSummarizationSlots_
FactorIndices filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
Definition: ConcurrentIncrementalSmoother.h:155
gtsam::ConcurrentSmoother
The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture.
Definition: ConcurrentFilteringAndSmoothing.h:99
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ConcurrentIncrementalSmoother::calculateEstimate
Values calculateEstimate() const
Compute the current best estimate of all variables and return a full Values structure.
Definition: ConcurrentIncrementalSmoother.h:83
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
gtsam::ConcurrentIncrementalSmoother::Result::Result
Result()
Constructor.
Definition: ConcurrentIncrementalSmoother.h:44
ConcurrentFilteringAndSmoothing.h
Base classes for the 'filter' and 'smoother' portion of the Concurrent Filtering and Smoothing archit...
gtsam::ISAM2
Definition: ISAM2.h:45
gtsam::ConcurrentIncrementalSmoother::smootherValues_
Values smootherValues_
New variables to be added to the smoother during the next update.
Definition: ConcurrentIncrementalSmoother.h:152
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:32
gtsam::ConcurrentIncrementalSmoother::filterSummarizationFactors_
NonlinearFactorGraph filterSummarizationFactors_
New filter summarization factors to replace the existing filter summarization during the next update.
Definition: ConcurrentIncrementalSmoother.h:153
gtsam::ConcurrentIncrementalSmoother::smootherFactors_
NonlinearFactorGraph smootherFactors_
New factors to be added to the smoother during the next update.
Definition: ConcurrentIncrementalSmoother.h:151
gtsam::ConcurrentIncrementalSmoother::smootherSummarization_
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
Definition: ConcurrentIncrementalSmoother.h:159
gtsam::ConcurrentIncrementalSmoother::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentIncrementalSmoother.h:38
gtsam::ConcurrentIncrementalSmoother::separatorValues_
Values separatorValues_
The linearization points of the separator variables. These should not be changed during optimization.
Definition: ConcurrentIncrementalSmoother.h:154
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::ConcurrentIncrementalSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ConcurrentIncrementalSmoother.h:71
gtsam::ConcurrentIncrementalSmoother::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentIncrementalSmoother.h:39
gtsam::ConcurrentIncrementalSmoother::ConcurrentIncrementalSmoother
ConcurrentIncrementalSmoother(const ISAM2Params &parameters=ISAM2Params())
Default constructor.
Definition: ConcurrentIncrementalSmoother.h:54
gtsam::ConcurrentIncrementalSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Access the current set of factors.
Definition: ConcurrentIncrementalSmoother.h:66
gtsam::ConcurrentIncrementalSmootherResult
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult
Typedef for Matlab wrapping.
Definition: ConcurrentIncrementalSmoother.h:169
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ConcurrentIncrementalSmoother::synchronizationUpdatesAvailable_
bool synchronizationUpdatesAvailable_
Flag indicating the currently stored synchronization updates have not been applied yet.
Definition: ConcurrentIncrementalSmoother.h:156
gtsam::ConcurrentIncrementalSmoother::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentIncrementalSmoother.h:40
gtsam::ConcurrentIncrementalSmoother
A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
Definition: ConcurrentIncrementalSmoother.h:30
gtsam::ConcurrentIncrementalSmoother::~ConcurrentIncrementalSmoother
virtual ~ConcurrentIncrementalSmoother()
Default destructor.
Definition: ConcurrentIncrementalSmoother.h:57