ConcurrentBatchSmoother.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 
24 #include <queue>
25 
26 namespace gtsam {
27 
31 class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
32 
33 public:
34  typedef std::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
36 
38  struct Result {
39  size_t iterations;
40  size_t lambdas;
42  size_t linearVariables;
43  double error;
44 
46  Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}
47 
49  size_t getIterations() const { return iterations; }
50  size_t getLambdas() const { return lambdas; }
51  size_t getNonlinearVariables() const { return nonlinearVariables; }
52  size_t getLinearVariables() const { return linearVariables; }
53  double getError() const { return error; }
54  };
55 
58 
61 
63  void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
64 
66  bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
67 
70  return factors_;
71  }
72 
74  const Values& getLinearizationPoint() const {
75  return theta_;
76  }
77 
79  const Ordering& getOrdering() const {
80  return ordering_;
81  }
82 
84  const VectorValues& getDelta() const {
85  return delta_;
86  }
87 
92  return theta_.retract(delta_);
93  }
94 
100  template<class VALUE>
102  const Vector delta = delta_.at(key);
103  return theta_.at<VALUE>(key).retract(delta);
104  }
105 
120  virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
121  const std::optional< std::vector<size_t> >& removeFactorIndices = {});
122 
127  void presync() override;
128 
135  void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
136 
146  void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
147  const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
148 
153  void postsync() override;
154 
155 protected:
156 
163  std::queue<size_t> availableSlots_;
165  std::vector<size_t> filterSummarizationSlots_;
166 
167  // Storage for information to be sent to the filter
169 
170 private:
171 
177  std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
178 
183  void removeFactors(const std::vector<size_t>& slots);
184 
186  void reorder();
187 
189  Result optimize();
190 
192  void updateSmootherSummarization();
193 
195  static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
196  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
197 
199  static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
200  const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
201 
202 }; // ConcurrentBatchSmoother
203 
206 
208 template<>
209 struct traits<ConcurrentBatchSmoother> : public Testable<ConcurrentBatchSmoother> {
210 };
211 
212 } //\ namespace gtsam
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
relicense.update
def update(text)
Definition: relicense.py:46
gtsam::Result
Result from elimination.
Definition: HybridGaussianFactorGraph.cpp:61
gtsam::ConcurrentBatchSmoother::ordering_
Ordering ordering_
The current ordering used to calculate the linear deltas.
Definition: ConcurrentBatchSmoother.h:160
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::ConcurrentBatchSmoother::getOrdering
const Ordering & getOrdering() const
Definition: ConcurrentBatchSmoother.h:79
gtsam::synchronize
void synchronize(ConcurrentFilter &filter, ConcurrentSmoother &smoother)
Definition: ConcurrentFilteringAndSmoothing.cpp:28
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Definition: triangulation.cpp:177
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::ConcurrentBatchSmoother::Result::getIterations
size_t getIterations() const
Getter methods.
Definition: ConcurrentBatchSmoother.h:49
Values
gtsam::ConcurrentBatchSmoother
Definition: ConcurrentBatchSmoother.h:31
gtsam::ConcurrentBatchSmoother::variableIndex_
VariableIndex variableIndex_
The current variable index, which allows efficient factor lookup by variable.
Definition: ConcurrentBatchSmoother.h:162
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::ConcurrentBatchSmoother::parameters_
LevenbergMarquardtParams parameters_
LM parameters.
Definition: ConcurrentBatchSmoother.h:157
gtsam::ConcurrentBatchSmoother::Result::Result
Result()
Constructor.
Definition: ConcurrentBatchSmoother.h:46
gtsam::ConcurrentBatchSmoother::Result::error
double error
The final factor graph error.
Definition: ConcurrentBatchSmoother.h:43
gtsam::ConcurrentBatchSmoother::ConcurrentBatchSmoother
ConcurrentBatchSmoother(const LevenbergMarquardtParams &parameters=LevenbergMarquardtParams())
Definition: ConcurrentBatchSmoother.h:57
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::ConcurrentBatchSmoother::Result::nonlinearVariables
size_t nonlinearVariables
The number of variables that can be relinearized.
Definition: ConcurrentBatchSmoother.h:41
gtsam::ConcurrentBatchSmoother::getLinearizationPoint
const Values & getLinearizationPoint() const
Definition: ConcurrentBatchSmoother.h:74
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::ConcurrentBatchSmoother::delta_
VectorValues delta_
The current set of linear deltas from the linearization point.
Definition: ConcurrentBatchSmoother.h:161
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::ConcurrentBatchSmoother::~ConcurrentBatchSmoother
~ConcurrentBatchSmoother() override
Definition: ConcurrentBatchSmoother.h:60
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::ConcurrentBatchSmoother::getFactors
const NonlinearFactorGraph & getFactors() const
Definition: ConcurrentBatchSmoother.h:69
gtsam::ConcurrentBatchSmoother::theta_
Values theta_
Current linearization point of all variables in the smoother.
Definition: ConcurrentBatchSmoother.h:159
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::ConcurrentBatchSmoother::Result::getLinearVariables
size_t getLinearVariables() const
Definition: ConcurrentBatchSmoother.h:52
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
gtsam::ConcurrentBatchSmoother::Result
Definition: ConcurrentBatchSmoother.h:38
gtsam::ConcurrentSmoother
Definition: ConcurrentFilteringAndSmoothing.h:101
gtsam::ConcurrentBatchSmoother::factors_
NonlinearFactorGraph factors_
The set of all factors currently in the smoother.
Definition: ConcurrentBatchSmoother.h:158
parameters
static ConjugateGradientParameters parameters
Definition: testIterative.cpp:33
gtsam::ConcurrentBatchSmoother::Result::linearVariables
size_t linearVariables
The number of variables that must keep a constant linearization point.
Definition: ConcurrentBatchSmoother.h:42
gtsam::ConcurrentBatchSmoother::calculateEstimate
Values calculateEstimate() const
Definition: ConcurrentBatchSmoother.h:91
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::ConcurrentBatchSmoother::getDelta
const VectorValues & getDelta() const
Definition: ConcurrentBatchSmoother.h:84
gtsam::VariableIndex
Definition: VariableIndex.h:41
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
gtsam::ConcurrentBatchSmoother::Result::getError
double getError() const
Definition: ConcurrentBatchSmoother.h:53
gtsam::ConcurrentBatchSmoother::filterSummarizationSlots_
std::vector< size_t > filterSummarizationSlots_
The slots in factor graph that correspond to the current filter summarization factors.
Definition: ConcurrentBatchSmoother.h:165
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam::ConcurrentBatchSmoother::Result::getNonlinearVariables
size_t getNonlinearVariables() const
Definition: ConcurrentBatchSmoother.h:51
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::ConcurrentBatchSmoother::Result::lambdas
size_t lambdas
The number of different L-M lambda factors that were tried during optimization.
Definition: ConcurrentBatchSmoother.h:40
gtsam::ConcurrentBatchSmoother::shared_ptr
std::shared_ptr< ConcurrentBatchSmoother > shared_ptr
Definition: ConcurrentBatchSmoother.h:34
gtsam::Values
Definition: Values.h:65
gtsam::ConcurrentBatchSmoother::Result::getLambdas
size_t getLambdas() const
Definition: ConcurrentBatchSmoother.h:50
ConcurrentFilteringAndSmoothing.h
Base classes for the 'filter' and 'smoother' portion of the Concurrent Filtering and Smoothing archit...
gtsam::ConcurrentBatchSmoother::smootherSummarization_
NonlinearFactorGraph smootherSummarization_
A temporary holding place for calculated smoother summarization.
Definition: ConcurrentBatchSmoother.h:168
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::ConcurrentBatchSmoother::separatorValues_
Values separatorValues_
The linearization points of the separator variables. These should not be updated during optimization.
Definition: ConcurrentBatchSmoother.h:164
gtsam::ConcurrentBatchSmoother::Base
ConcurrentSmoother Base
typedef for base class
Definition: ConcurrentBatchSmoother.h:35
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::ConcurrentBatchSmoother::calculateEstimate
VALUE calculateEstimate(Key key) const
Definition: ConcurrentBatchSmoother.h:101
gtsam::ConcurrentBatchSmoother::Result::iterations
size_t iterations
The number of optimizer iterations performed.
Definition: ConcurrentBatchSmoother.h:39
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::ConcurrentBatchSmoother::availableSlots_
std::queue< size_t > availableSlots_
The set of available factor graph slots caused by deleting factors.
Definition: ConcurrentBatchSmoother.h:163
gtsam::ConcurrentBatchSmootherResult
ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult
Typedef for Matlab wrapping.
Definition: ConcurrentBatchSmoother.h:205


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