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 ConcurrentBatchFilter.h
14 * @brief A Levenberg-Marquardt Batch Filter 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 Filter that implements the Concurrent Filtering and Smoother interface.
30 */
31class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter {
32
33public:
34 typedef std::shared_ptr<ConcurrentBatchFilter> shared_ptr;
35 typedef ConcurrentFilter 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
44 /** The indices of the newly-added factors, in 1-to-1 correspondence with the
45 * factors passed as \c newFactors update(). These indices may be
46 * used later to refer to the factors in order to remove them.
47 */
48 std::vector<size_t> newFactorsIndices;
49
50 double error; ///< The final factor graph error
51
52 /// Constructor
53 Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}
54
55 /// Getter methods
56 size_t getIterations() const { return iterations; }
57 size_t getLambdas() const { return lambdas; }
58 size_t getNonlinearVariables() const { return nonlinearVariables; }
59 size_t getLinearVariables() const { return linearVariables; }
60 double getError() const { return error; }
61 };
62
63 /** Default constructor */
64 ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}
65
66 /** Default destructor */
67 ~ConcurrentBatchFilter() override = default;
68
69 /** Implement a GTSAM standard 'print' function */
70 void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
71
72 /** Check if two Concurrent Filters are equal */
73 bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const override;
74
75 /** Access the current set of factors */
76 const NonlinearFactorGraph& getFactors() const {
77 return factors_;
78 }
79
80 /** Access the current linearization point */
81 const Values& getLinearizationPoint() const {
82 return theta_;
83 }
84
85 /** Access the current ordering */
86 const Ordering& getOrdering() const {
87 return ordering_;
88 }
89
90 /** Access the current set of deltas to the linearization point */
91 const VectorValues& getDelta() const {
92 return delta_;
93 }
94
95 /** Compute the current best estimate of all variables and return a full Values structure.
96 * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&).
97 */
98 Values calculateEstimate() const {
99 return theta_.retract(delta: delta_);
100 }
101
102 /** Compute the current best estimate of a single variable. This is generally faster than
103 * calling the no-argument version of calculateEstimate if only specific variables are needed.
104 * @param key
105 * @return
106 */
107 template<class VALUE>
108 VALUE calculateEstimate(Key key) const {
109 const Vector delta = delta_.at(j: key);
110 return theta_.at<VALUE>(key).retract(delta);
111 }
112
113 /**
114 * Add new factors and variables to the filter.
115 *
116 * Add new measurements, and optionally new variables, to the filter.
117 * This runs a full update step of the derived filter algorithm
118 *
119 * @param newFactors The new factors to be added to the smoother
120 * @param newTheta Initialization points for new variables to be added to the filter
121 * You must include here all new variables occurring in newFactors that were not already
122 * in the filter.
123 * @param keysToMove An optional set of keys to move from the filter to the smoother
124 * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph
125 */
126 virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
127 const std::optional<FastList<Key> >& keysToMove = {}, const std::optional< std::vector<size_t> >& removeFactorIndices = {});
128
129 /**
130 * Perform any required operations before the synchronization process starts.
131 * Called by 'synchronize'
132 */
133 void presync() override;
134
135 /**
136 * Populate the provided containers with factors that constitute the filter branch summarization
137 * needed by the smoother. Also, linearization points for the new root clique must be provided.
138 *
139 * @param summarizedFactors The summarized factors for the filter branch
140 * @param rootValues The linearization points of the root clique variables
141 */
142 void getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) override;
143
144 /**
145 * Populate the provided containers with factors being sent to the smoother from the filter. These
146 * may be original nonlinear factors, or factors encoding a summarization of the filter information.
147 * The specifics will be implementation-specific for a given filter.
148 *
149 * @param smootherFactors The new factors to be added to the smoother
150 * @param smootherValues The linearization points of any new variables
151 */
152 void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) override;
153
154 /**
155 * Apply the updated version of the smoother branch summarized factors.
156 *
157 * @param summarizedFactors An updated version of the smoother branch summarized factors
158 */
159 void synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) override;
160
161 /**
162 * Perform any required operations after the synchronization process finishes.
163 * Called by 'synchronize'
164 */
165 void postsync() override;
166
167protected:
168
169 LevenbergMarquardtParams parameters_; ///< LM parameters
170 NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
171 Values theta_; ///< Current linearization point of all variables in the filter
172 Ordering ordering_; ///< The current ordering used to calculate the linear deltas
173 VectorValues delta_; ///< The current set of linear deltas from the linearization point
174 std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
175 Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
176 std::vector<size_t> separatorSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
177
178 // Storage for information from the Smoother
179 NonlinearFactorGraph smootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization
180 NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
181
182 // Storage for information to be sent to the smoother
183 NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother
184 NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother
185 Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother
186
187private:
188
189 /** Augment the graph with new factors
190 *
191 * @param factors The factors to add to the graph
192 * @return The slots in the graph where they were inserted
193 */
194 std::vector<size_t> insertFactors(const NonlinearFactorGraph& factors);
195
196 /** Remove factors from the graph by slot index
197 *
198 * @param slots The slots in the factor graph that should be deleted
199 */
200 void removeFactors(const std::vector<size_t>& slots);
201
202 /** Use colamd to update into an efficient ordering */
203 void reorder(const std::optional<FastList<Key> >& keysToMove = {});
204
205 /** Marginalize out the set of requested variables from the filter, caching them for the smoother
206 * This effectively moves the separator.
207 *
208 * @param keysToMove The set of keys to move from the filter to the smoother
209 */
210 void moveSeparator(const FastList<Key>& keysToMove);
211
212 /** Use a modified version of L-M to update the linearization point and delta */
213 static void optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
214 VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters,
215 Result& result);
216
217 /** Print just the nonlinear keys in a nonlinear factor */
218 static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
219 const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
220
221 /** Print just the nonlinear keys in each factor for a whole Nonlinear Factor Graph */
222 static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
223 const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
224
225 /** Print just the nonlinear keys of specific factors in a Nonlinear Factor Graph */
226 static void PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
227 const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
228
229 /** Print just the nonlinear keys in a linear factor */
230 static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
231 const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
232
233 /** Print just the nonlinear keys in each linear factor for a whole Gaussian Factor Graph */
234 static void PrintLinearFactorGraph(const GaussianFactorGraph& factors,
235 const std::string& indent = "", const std::string& title = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
236
237 /** Print just the nonlinear keys contained inside a container */
238 template<class Container>
239 static void PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
240
241}; // ConcurrentBatchFilter
242
243/// Implementation of PrintKeys
244template<class Container>
245void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
246 std::cout << indent << title;
247 for(Key key: keys) {
248 std::cout << " " << keyFormatter(key);
249 }
250 std::cout << std::endl;
251}
252
253/// Typedef for Matlab wrapping
254typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult;
255
256/// traits
257template<>
258struct traits<ConcurrentBatchFilter> : public Testable<ConcurrentBatchFilter> {
259};
260
261} // \ namespace gtsam
262