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
12/**
13 * @file ConcurrentBatchSmoother.h
14 * @brief A Levenberg-Marquardt Batch Smoother that implements the
15 * Concurrent Filtering and Smoothing interface.
16 * @author Stephen Williams
17 */
18
19// \callgraph
20#pragma once
21
22#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
23#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
24#include <queue>
25
26namespace gtsam {
27
28/**
29 * A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface.
30 */
31class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother {
32
33public:
34 typedef std::shared_ptr<ConcurrentBatchSmoother> shared_ptr;
35 typedef ConcurrentSmoother Base; ///< typedef for base class
36
37 /** Meta information returned about the update */
38 struct Result {
39 size_t iterations; ///< The number of optimizer iterations performed
40 size_t lambdas; ///< The number of different L-M lambda factors that were tried during optimization
41 size_t nonlinearVariables; ///< The number of variables that can be relinearized
42 size_t linearVariables; ///< The number of variables that must keep a constant linearization point
43 double error; ///< The final factor graph error
44
45 /// Constructor
46 Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}
47
48 /// Getter methods
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
56 /** Default constructor */
57 ConcurrentBatchSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}
58
59 /** Default destructor */
60 ~ConcurrentBatchSmoother() override {}
61
62 /** Implement a GTSAM standard 'print' function */
63 void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
64
65 /** Check if two Concurrent Smoothers are equal */
66 bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const override;
67
68 /** Access the current set of factors */
69 const NonlinearFactorGraph& getFactors() const {
70 return factors_;
71 }
72
73 /** Access the current linearization point */
74 const Values& getLinearizationPoint() const {
75 return theta_;
76 }
77
78 /** Access the current ordering */
79 const Ordering& getOrdering() const {
80 return ordering_;
81 }
82
83 /** Access the current set of deltas to the linearization point */
84 const VectorValues& getDelta() const {
85 return delta_;
86 }
87
88 /** Compute the current best estimate of all variables and return a full Values structure.
89 * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
90 */
91 Values calculateEstimate() const {
92 return theta_.retract(delta: delta_);
93 }
94
95 /** Compute the current best estimate of a single variable. This is generally faster than
96 * calling the no-argument version of calculateEstimate if only specific variables are needed.
97 * @param key
98 * @return
99 */
100 template<class VALUE>
101 VALUE calculateEstimate(Key key) const {
102 const Vector delta = delta_.at(j: key);
103 return theta_.at<VALUE>(key).retract(delta);
104 }
105
106 /**
107 * Add new factors and variables to the smoother.
108 *
109 * Add new measurements, and optionally new variables, to the smoother.
110 * This runs a full step of the ISAM2 algorithm, relinearizing and updating
111 * the solution as needed, according to the wildfire and relinearize
112 * thresholds.
113 *
114 * @param newFactors The new factors to be added to the smoother
115 * @param newTheta Initialization points for new variables to be added to the smoother
116 * You must include here all new variables occuring in newFactors (which were not already
117 * in the smoother). There must not be any variables here that do not occur in newFactors,
118 * and additionally, variables that were already in the system must not be included here.
119 */
120 virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
121 const std::optional< std::vector<size_t> >& removeFactorIndices = {});
122
123 /**
124 * Perform any required operations before the synchronization process starts.
125 * Called by 'synchronize'
126 */
127 void presync() override;
128
129 /**
130 * Populate the provided containers with factors that constitute the smoother branch summarization
131 * needed by the filter.
132 *
133 * @param summarizedFactors The summarized factors for the filter branch
134 */
135 void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) override;
136
137 /**
138 * Apply the new smoother factors sent by the filter, and the updated version of the filter
139 * branch summarized factors.
140 *
141 * @param smootherFactors A set of new factors added to the smoother from the filter
142 * @param smootherValues Linearization points for any new variables
143 * @param summarizedFactors An updated version of the filter branch summarized factors
144 * @param rootValues The linearization point of the root variables
145 */
146 void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
147 const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) override;
148
149 /**
150 * Perform any required operations after the synchronization process finishes.
151 * Called by 'synchronize'
152 */
153 void postsync() override;
154
155protected:
156
157 LevenbergMarquardtParams parameters_; ///< LM parameters
158 NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
159 Values theta_; ///< Current linearization point of all variables in the smoother
160 Ordering ordering_; ///< The current ordering used to calculate the linear deltas
161 VectorValues delta_; ///< The current set of linear deltas from the linearization point
162 VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
163 std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
164 Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
165 std::vector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
166
167 // Storage for information to be sent to the filter
168 NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization
169
170private:
171
172 /** Augment the graph with new factors
173 *
174 * @param factors The factors to add to the graph
175 * @return The slots in the graph where they were inserted
176 */
177 std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
178
179 /** Remove factors from the graph by slot index
180 *
181 * @param slots The slots in the factor graph that should be deleted
182 * */
183 void removeFactors(const std::vector<size_t>& slots);
184
185 /** Use colamd to update into an efficient ordering */
186 void reorder();
187
188 /** Use a modified version of L-M to update the linearization point and delta */
189 Result optimize();
190
191 /** Calculate the smoother marginal factors on the separator variables */
192 void updateSmootherSummarization();
193
194 /** Print just the nonlinear keys in a nonlinear factor */
195 static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
196 const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
197
198 /** Print just the nonlinear keys in a linear factor */
199 static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
200 const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
201
202}; // ConcurrentBatchSmoother
203
204/// Typedef for Matlab wrapping
205typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult;
206
207/// traits
208template<>
209struct traits<ConcurrentBatchSmoother> : public Testable<ConcurrentBatchSmoother> {
210};
211
212} //\ namespace gtsam
213