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 ConcurrentIncrementalFilter.cpp
14 * @brief An iSAM2-based Filter that implements the
15 * Concurrent Filtering and Smoothing interface.
16 * @author Stephen Williams
17 */
18#include <gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h>
19#include <gtsam/nonlinear/LinearContainerFactor.h>
20#include <gtsam/base/timing.h>
21#include <gtsam/base/debug.h>
22
23namespace gtsam {
24
25/* ************************************************************************* */
26void ConcurrentIncrementalFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
27 std::cout << s;
28 isam2_.print(s: "");
29}
30
31/* ************************************************************************* */
32bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol) const {
33 const ConcurrentIncrementalFilter* filter = dynamic_cast<const ConcurrentIncrementalFilter*>(&rhs);
34 return filter
35 && isam2_.equals(other: filter->isam2_)
36 && (currentSmootherSummarizationSlots_.size() == filter->currentSmootherSummarizationSlots_.size())
37 && std::equal(first1: currentSmootherSummarizationSlots_.begin(), last1: currentSmootherSummarizationSlots_.end(), first2: filter->currentSmootherSummarizationSlots_.begin())
38 && previousSmootherSummarization_.equals(other: filter->previousSmootherSummarization_)
39 && smootherShortcut_.equals(other: filter->smootherShortcut_)
40 && smootherFactors_.equals(other: filter->smootherFactors_)
41 && smootherValues_.equals(other: filter->smootherValues_);
42}
43
44/* ************************************************************************* */
45ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
46 const std::optional<FastList<Key> >& keysToMove, const std::optional< FactorIndices >& removeFactorIndices) {
47
48 gttic(update);
49
50// const bool debug = ISDEBUG("ConcurrentIncrementalFilter update");
51 const bool debug = false;
52
53 if(debug) std::cout << "ConcurrentIncrementalFilter::update Begin" << std::endl;
54
55 // Create the return result meta-data
56 Result result;
57
58 // We do not need to remove any factors at this time
59 FactorIndices removedFactors;
60
61 if(removeFactorIndices){
62 removedFactors.insert(position: removedFactors.end(), first: removeFactorIndices->begin(), last: removeFactorIndices->end());
63 }
64
65 // Generate ordering constraints that force the 'keys to move' to the end
66 std::optional<FastMap<Key,int> > orderingConstraints = {};
67 if(keysToMove && keysToMove->size() > 0) {
68 orderingConstraints = FastMap<Key,int>();
69 int group = 1;
70 // Set all existing variables to Group1
71 if(isam2_.getLinearizationPoint().size() > 0) {
72 for(const auto key: isam2_.getLinearizationPoint().keys()) {
73 orderingConstraints->operator[](k: key) = group;
74 }
75 ++group;
76 }
77 // Assign new variables to the root
78 for(const auto key: newTheta.keys()) {
79 orderingConstraints->operator[](k: key) = group;
80 }
81 // Set marginalizable variables to Group0
82 for(Key key: *keysToMove){
83 orderingConstraints->operator[](k: key) = 0;
84 }
85 }
86
87 // Create the set of linear keys that iSAM2 should hold constant
88 // iSAM2 takes care of this for us; no need to specify additional noRelin keys
89 std::optional<FastList<Key> > noRelinKeys = {};
90
91 // Mark additional keys between the 'keys to move' and the leaves
92 std::optional<FastList<Key> > additionalKeys = {};
93 if(keysToMove && keysToMove->size() > 0) {
94 std::set<Key> markedKeys;
95 for(Key key: *keysToMove) {
96 if(isam2_.getLinearizationPoint().exists(j: key)) {
97 ISAM2Clique::shared_ptr clique = isam2_[key];
98 GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
99 while(*key_iter != key) {
100 markedKeys.insert(x: *key_iter);
101 ++key_iter;
102 }
103 for(const ISAM2Clique::shared_ptr& child: clique->children) {
104 RecursiveMarkAffectedKeys(key, clique: child, additionalKeys&: markedKeys);
105 }
106 }
107 }
108 additionalKeys = FastList<Key>(markedKeys.begin(), markedKeys.end());
109 }
110
111 // Update the system using iSAM2
112 gttic(isam2);
113 ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removeFactorIndices: removedFactors, constrainedKeys: orderingConstraints, noRelinKeys, extraReelimKeys: additionalKeys);
114 gttoc(isam2);
115
116 if(keysToMove && keysToMove->size() > 0) {
117
118 gttic(cache_smoother_factors);
119 // Find the set of factors that will be removed
120 FactorIndices removedFactorSlots = FindAdjacentFactors(isam2: isam2_, keys: *keysToMove, factorsToIgnore: currentSmootherSummarizationSlots_);
121 // Cache these factors for later transmission to the smoother
122 NonlinearFactorGraph removedFactors;
123 for(size_t slot: removedFactorSlots) {
124 const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(i: slot);
125 if(factor) {
126 smootherFactors_.push_back(factor);
127 removedFactors.push_back(factor);
128 }
129 }
130 // Cache removed values for later transmission to the smoother
131 for(Key key: *keysToMove) {
132 smootherValues_.insert(j: key, val: isam2_.getLinearizationPoint().at(j: key));
133 }
134 gttoc(cache_smoother_factors);
135
136 gttic(marginalize);
137 FactorIndices marginalFactorsIndices;
138 FactorIndices deletedFactorsIndices;
139 isam2_.marginalizeLeaves(leafKeys: *keysToMove, optArgs&: marginalFactorsIndices, optArgs&: deletedFactorsIndices);
140 currentSmootherSummarizationSlots_.insert(position: currentSmootherSummarizationSlots_.end(), first: marginalFactorsIndices.begin(), last: marginalFactorsIndices.end());
141 for(size_t index: deletedFactorsIndices) {
142 currentSmootherSummarizationSlots_.erase(first: std::remove(first: currentSmootherSummarizationSlots_.begin(), last: currentSmootherSummarizationSlots_.end(), value: index), last: currentSmootherSummarizationSlots_.end());
143 }
144 gttoc(marginalize);
145
146 // Calculate a new shortcut
147 updateShortcut(removedFactors);
148 }
149
150 // Extract the ConcurrentIncrementalFilter::Result information
151 result.iterations = 1;
152 result.linearVariables = isam2_.getFixedVariables().size();
153 result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables;
154 result.newFactorsIndices = isam2Result.newFactorsIndices;
155 result.variablesReeliminated = isam2Result.variablesReeliminated;
156 result.variablesRelinearized = isam2Result.variablesRelinearized;
157// result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate());
158
159 if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl;
160
161 gttoc(update);
162
163 return result;
164}
165
166/* ************************************************************************* */
167void ConcurrentIncrementalFilter::presync() {
168
169 gttic(presync);
170
171 gttoc(presync);
172}
173
174/* ************************************************************************* */
175void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
176
177 gttic(synchronize);
178// const bool debug = ISDEBUG("ConcurrentIncrementalFilter synchronize");
179 const bool debug = false;
180
181 if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize Begin" << std::endl;
182
183 // Update the smoother summarization on the old separator
184 previousSmootherSummarization_ = smootherSummarization;
185
186 // Find the set of new separator keys
187 const KeySet& newSeparatorKeys = isam2_.getFixedVariables();
188
189 // Use the shortcut to calculate an updated marginal on the current separator
190 // Combine just the shortcut and the previousSmootherSummarization
191 NonlinearFactorGraph graph;
192 graph.push_back(container: previousSmootherSummarization_);
193 graph.push_back(container: smootherShortcut_);
194 Values values;
195 values.insert(values: smootherSummarizationValues);
196 for(Key key: newSeparatorKeys) {
197 if(!values.exists(j: key)) {
198 values.insert(j: key, val: isam2_.getLinearizationPoint().at(j: key));
199 }
200 }
201
202 // Force iSAM2 not to relinearize anything during this iteration
203 FastList<Key> noRelinKeys;
204 for(const auto key: isam2_.getLinearizationPoint().keys()) {
205 noRelinKeys.push_back(x: key);
206 }
207
208 // Calculate the summarized factor on just the new separator keys
209 NonlinearFactorGraph currentSmootherSummarization = internal::calculateMarginalFactors(graph, theta: values, remainingKeys: newSeparatorKeys,
210 eliminateFunction: isam2_.params().getEliminationFunction());
211
212 // Remove the old factors on the separator and insert the new ones
213 FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
214 ISAM2Result result = isam2_.update(newFactors: currentSmootherSummarization, newTheta: Values(), removeFactorIndices: removeFactors, constrainedKeys: {}, noRelinKeys, extraReelimKeys: {}, force_relinearize: false);
215 currentSmootherSummarizationSlots_ = result.newFactorsIndices;
216
217 // Set the previous smoother summarization to the current smoother summarization and clear the smoother shortcut
218 previousSmootherSummarization_ = currentSmootherSummarization;
219 smootherShortcut_.resize(size: 0);
220
221 if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize End" << std::endl;
222
223 gttoc(synchronize);
224}
225
226/* ************************************************************************* */
227void ConcurrentIncrementalFilter::getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) {
228
229 gttic(get_summarized_factors);
230
231 // Calculate the current filter summarization
232 filterSummarization = calculateFilterSummarization();
233
234 // Copy the current separator values into the output
235 for(Key key: isam2_.getFixedVariables()) {
236 filterSummarizationValues.insert(j: key, val: isam2_.getLinearizationPoint().at(j: key));
237 }
238
239 gttoc(get_summarized_factors);
240}
241
242/* ************************************************************************* */
243void ConcurrentIncrementalFilter::getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) {
244
245 gttic(get_smoother_factors);
246
247 // Copy the previous calculated smoother summarization factors into the output
248 smootherFactors.push_back(container: smootherFactors_);
249 smootherValues.insert(values: smootherValues_);
250
251 gttoc(get_smoother_factors);
252}
253
254/* ************************************************************************* */
255void ConcurrentIncrementalFilter::postsync() {
256
257 gttic(postsync);
258
259 // Clear out the factors and values that were just sent to the smoother
260 smootherFactors_.resize(size: 0);
261 smootherValues_.clear();
262
263 gttoc(postsync);
264}
265
266
267/* ************************************************************************* */
268void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
269
270 // Check if the separator keys of the current clique contain the specified key
271 if(std::find(first: clique->conditional()->beginParents(), last: clique->conditional()->endParents(), val: key) != clique->conditional()->endParents()) {
272
273 // Mark the frontal keys of the current clique
274 for(Key idx: clique->conditional()->frontals()) {
275 additionalKeys.insert(x: idx);
276 }
277
278 // Recursively mark all of the children
279 for(const ISAM2Clique::shared_ptr& child: clique->children) {
280 RecursiveMarkAffectedKeys(key, clique: child, additionalKeys);
281 }
282 }
283 // If the key was not found in the separator/parents, then none of its children can have it either
284
285}
286
287/* ************************************************************************* */
288FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore) {
289
290 // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
291 FactorIndices removedFactorSlots;
292 const VariableIndex& variableIndex = isam2.getVariableIndex();
293 for(Key key: keys) {
294 const auto& slots = variableIndex[key];
295 removedFactorSlots.insert(position: removedFactorSlots.end(), first: slots.begin(), last: slots.end());
296 }
297
298 // Sort and remove duplicates
299 std::sort(first: removedFactorSlots.begin(), last: removedFactorSlots.end());
300 removedFactorSlots.erase(first: std::unique(first: removedFactorSlots.begin(), last: removedFactorSlots.end()), last: removedFactorSlots.end());
301
302 // Remove any linear/marginal factor that made it into the set
303 for(size_t index: factorsToIgnore) {
304 removedFactorSlots.erase(first: std::remove(first: removedFactorSlots.begin(), last: removedFactorSlots.end(), value: index), last: removedFactorSlots.end());
305 }
306
307 return removedFactorSlots;
308}
309
310///* ************************************************************************* */
311// TODO: Make this a static function
312void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) {
313
314 // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
315 KeySet shortcutKeys;
316 for(size_t slot: currentSmootherSummarizationSlots_) {
317 const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(i: slot);
318 if(factor) {
319 shortcutKeys.insert(first: factor->begin(), last: factor->end());
320 }
321 }
322 for(Key key: previousSmootherSummarization_.keys()) {
323 shortcutKeys.insert(x: key);
324 }
325
326 // Combine the previous shortcut factor with all of the new factors being sent to the smoother
327 NonlinearFactorGraph graph;
328 graph.push_back(container: removedFactors);
329 graph.push_back(container: smootherShortcut_);
330 Values values;
331 values.insert(values: smootherValues_);
332 values.insert(values: isam2_.getLinearizationPoint());
333 // Calculate the summarized factor on the shortcut keys
334 smootherShortcut_ = internal::calculateMarginalFactors(graph, theta: values, remainingKeys: shortcutKeys,
335 eliminateFunction: isam2_.params().getEliminationFunction());
336}
337
338/* ************************************************************************* */
339// TODO: Make this a static function
340NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() const {
341
342 // The filter summarization factors are the resulting marginal factors on the separator
343 // variables that result from marginalizing out all of the other variables
344
345 // Find the set of current separator keys
346 const KeySet& separatorKeys = isam2_.getFixedVariables();
347
348 // Find all cliques that contain any separator variables
349 std::set<ISAM2Clique::shared_ptr> separatorCliques;
350 for(Key key: separatorKeys) {
351 ISAM2Clique::shared_ptr clique = isam2_[key];
352 separatorCliques.insert( x: clique );
353 }
354
355 // Create the set of clique keys LC:
356 KeyVector cliqueKeys;
357 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
358 for(Key key: clique->conditional()->frontals()) {
359 cliqueKeys.push_back(x: key);
360 }
361 }
362 std::sort(first: cliqueKeys.begin(), last: cliqueKeys.end());
363
364 // Gather all factors that involve only clique keys
365 std::set<size_t> cliqueFactorSlots;
366 for(Key key: cliqueKeys) {
367 for(size_t slot: isam2_.getVariableIndex()[key]) {
368 const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(i: slot);
369 if(factor) {
370 std::set<Key> factorKeys(factor->begin(), factor->end());
371 if(std::includes(first1: cliqueKeys.begin(), last1: cliqueKeys.end(), first2: factorKeys.begin(), last2: factorKeys.end())) {
372 cliqueFactorSlots.insert(x: slot);
373 }
374 }
375 }
376 }
377
378 // Remove any factor included in the current smoother summarization
379 for(size_t slot: currentSmootherSummarizationSlots_) {
380 cliqueFactorSlots.erase(x: slot);
381 }
382
383 // Create a factor graph from the identified factors
384 NonlinearFactorGraph graph;
385 for(size_t slot: cliqueFactorSlots) {
386 graph.push_back(factor: isam2_.getFactorsUnsafe().at(i: slot));
387 }
388
389 // Find the set of children of the separator cliques
390 std::set<ISAM2Clique::shared_ptr> childCliques;
391 // Add all of the children
392 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
393 childCliques.insert(first: clique->children.begin(), last: clique->children.end());
394 }
395 // Remove any separator cliques that were added because they were children of other separator cliques
396 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
397 childCliques.erase(x: clique);
398 }
399
400 // Augment the factor graph with cached factors from the children
401 for(const ISAM2Clique::shared_ptr& clique: childCliques) {
402 LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
403 graph.push_back( factor );
404 }
405
406 // Calculate the marginal factors on the separator
407 NonlinearFactorGraph filterSummarization = internal::calculateMarginalFactors(graph, theta: isam2_.getLinearizationPoint(), remainingKeys: separatorKeys,
408 eliminateFunction: isam2_.params().getEliminationFunction());
409
410 return filterSummarization;
411}
412
413/* ************************************************************************* */
414}/// namespace gtsam
415