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.cpp
14 * @brief A Levenberg-Marquardt Batch Filter that implements the
15 * Concurrent Filtering and Smoothing interface.
16 * @author Stephen Williams
17 */
18#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
19#include <gtsam/nonlinear/LinearContainerFactor.h>
20#include <gtsam/linear/GaussianJunctionTree.h>
21#include <gtsam/base/timing.h>
22#include <gtsam/base/debug.h>
23
24#include <cassert>
25
26namespace gtsam {
27
28/* ************************************************************************* */
29void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
30 const std::string& indent, const KeyFormatter& keyFormatter) {
31 std::cout << indent;
32 if(factor) {
33 if(std::dynamic_pointer_cast<LinearContainerFactor>(r: factor)) {
34 std::cout << "l( ";
35 } else {
36 std::cout << "f( ";
37 }
38 for(Key key: *factor) {
39 std::cout << keyFormatter(key) << " ";
40 }
41 std::cout << ")" << std::endl;
42 } else {
43 std::cout << "{ nullptr }" << std::endl;
44 }
45}
46
47/* ************************************************************************* */
48void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors,
49 const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
50 std::cout << indent << title << std::endl;
51 for(const NonlinearFactor::shared_ptr& factor: factors) {
52 PrintNonlinearFactor(factor, indent: indent + " ", keyFormatter);
53 }
54}
55
56/* ************************************************************************* */
57void ConcurrentBatchFilter::PrintNonlinearFactorGraph(const NonlinearFactorGraph& factors, const std::vector<size_t>& slots,
58 const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
59 std::cout << indent << title << std::endl;
60 for(size_t slot: slots) {
61 PrintNonlinearFactor(factor: factors.at(i: slot), indent: indent + " ", keyFormatter);
62 }
63}
64
65/* ************************************************************************* */
66void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor,
67 const std::string& indent, const KeyFormatter& keyFormatter) {
68 std::cout << indent;
69 if(factor) {
70 JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast<JacobianFactor>(r: factor);
71 HessianFactor::shared_ptr hf = std::dynamic_pointer_cast<HessianFactor>(r: factor);
72 if(jf) {
73 std::cout << "j( ";
74 } else if(hf) {
75 std::cout << "h( ";
76 } else {
77 std::cout << "g( ";
78 }
79 for(Key key: *factor) {
80 std::cout << keyFormatter(key) << " ";
81 }
82 std::cout << ")" << std::endl;
83 } else {
84 std::cout << "{ nullptr }" << std::endl;
85 }
86}
87
88/* ************************************************************************* */
89void ConcurrentBatchFilter::PrintLinearFactorGraph(const GaussianFactorGraph& factors,
90 const std::string& indent, const std::string& title, const KeyFormatter& keyFormatter) {
91 std::cout << indent << title << std::endl;
92 for(const GaussianFactor::shared_ptr& factor: factors) {
93 PrintLinearFactor(factor, indent: indent + " ", keyFormatter);
94 }
95}
96
97/* ************************************************************************* */
98void ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
99 std::cout << s;
100 PrintNonlinearFactorGraph(factors: factors_, indent: " ", title: "Factors:");
101 PrintKeys(keys: theta_.keys(), indent: " ", title: "Values:");
102 PrintNonlinearFactorGraph(factors: smootherFactors_, indent: " ", title: "Cached Smoother Factors:");
103 PrintKeys(keys: smootherValues_.keys(), indent: " ", title: "Cached Smoother Values:");
104}
105
106/* ************************************************************************* */
107bool ConcurrentBatchFilter::equals(const ConcurrentFilter& rhs, double tol) const {
108 const ConcurrentBatchFilter* filter = dynamic_cast<const ConcurrentBatchFilter*>(&rhs);
109 return filter
110 && factors_.equals(other: filter->factors_)
111 && theta_.equals(other: filter->theta_)
112 && ordering_.equals(other: filter->ordering_)
113 && delta_.equals(x: filter->delta_)
114 && separatorValues_.equals(other: filter->separatorValues_)
115 && smootherSummarization_.equals(other: filter->smootherSummarization_)
116 && smootherShortcut_.equals(other: filter->smootherShortcut_)
117 && filterSummarization_.equals(other: filter->filterSummarization_)
118 && smootherFactors_.equals(other: filter->smootherFactors_)
119 && smootherValues_.equals(other: filter->smootherValues_);
120}
121
122/* ************************************************************************* */
123ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
124 const std::optional<FastList<Key> >& keysToMove, const std::optional< std::vector<size_t> >& removeFactorIndices) {
125
126 gttic(update);
127
128// const bool debug = ISDEBUG("ConcurrentBatchFilter update");
129 const bool debug = false;
130
131 if(debug) std::cout << "ConcurrentBatchFilter::update Begin" << std::endl;
132
133 // Create the return result meta-data
134 Result result;
135
136 if(debug) std::cout << "ConcurrentBatchFilter::update Augmenting System ..." << std::endl;
137
138 // Update all of the internal variables with the new information
139 gttic(augment_system);
140
141 // Add the new variables to theta
142 theta_.insert(values: newTheta);
143 // Add new variables to the end of the ordering
144 for (const auto key : newTheta.keys()) {
145 ordering_.push_back(x: key);
146 }
147 // Augment Delta
148 delta_.insert(values: newTheta.zeroVectors());
149
150 // Add the new factors to the graph, updating the variable index
151 result.newFactorsIndices = insertFactors(factors: newFactors);
152
153 if(removeFactorIndices){
154 if(debug){
155 std::cout << "ConcurrentBatchFilter::update removeFactorIndices " << std::endl;
156 }
157 removeFactors(slots: *removeFactorIndices);
158 }
159
160 gttoc(augment_system);
161
162 if(debug) std::cout << "ConcurrentBatchFilter::update Reordering System ..." << std::endl;
163
164 // Reorder the system to ensure efficient optimization (and marginalization) performance
165 gttic(reorder);
166 reorder(keysToMove);
167 gttoc(reorder);
168
169 if(debug) std::cout << "ConcurrentBatchFilter::update Optimizing System ..." << std::endl;
170
171 // Optimize the factors using a modified version of L-M
172 gttic(optimize);
173 if(factors_.size() > 0) {
174 optimize(factors: factors_, theta&: theta_, ordering: ordering_, delta&: delta_, linearValues: separatorValues_, parameters: parameters_, result);
175 }
176 gttoc(optimize);
177
178 if(debug) std::cout << "ConcurrentBatchFilter::update Moving Separator ..." << std::endl;
179
180 gttic(move_separator);
181 if(keysToMove && keysToMove->size() > 0){
182 moveSeparator(keysToMove: *keysToMove);
183 }
184 gttoc(move_separator);
185
186 if(debug) std::cout << "ConcurrentBatchFilter::update End" << std::endl;
187
188 gttoc(update);
189
190 return result;
191}
192
193/* ************************************************************************* */
194void ConcurrentBatchFilter::presync() {
195
196 gttic(presync);
197
198 gttoc(presync);
199}
200
201/* ************************************************************************* */
202void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
203
204 gttic(synchronize);
205
206// const bool debug = ISDEBUG("ConcurrentBatchFilter synchronize");
207 const bool debug = false;
208
209 if(debug) std::cout << "ConcurrentBatchFilter::synchronize Begin" << std::endl;
210
211 if(debug) { PrintNonlinearFactorGraph(factors: smootherSummarization_, indent: "ConcurrentBatchFilter::synchronize ", title: "Previous Smoother Summarization:", keyFormatter: DefaultKeyFormatter); }
212
213#ifndef NDEBUG
214 std::set<Key> oldKeys = smootherSummarization_.keys();
215 std::set<Key> newKeys = smootherSummarization.keys();
216 assert(oldKeys.size() == newKeys.size());
217 assert(std::equal(oldKeys.begin(), oldKeys.end(), newKeys.begin()));
218#endif
219
220 // Update the smoother summarization on the old separator
221 smootherSummarization_ = smootherSummarization;
222
223 if(debug) { PrintNonlinearFactorGraph(factors: smootherSummarization_, indent: "ConcurrentBatchFilter::synchronize ", title: "Updated Smoother Summarization:", keyFormatter: DefaultKeyFormatter); }
224
225 // Find the set of new separator keys
226 const KeySet newSeparatorKeys = separatorValues_.keySet();
227
228 if(debug) { PrintKeys(keys: newSeparatorKeys, indent: "ConcurrentBatchFilter::synchronize ", title: "Current Separator Keys:"); }
229
230 // Use the shortcut to calculate an updated marginal on the current separator
231 {
232 // Combine just the shortcut and the previousSmootherSummarization
233 NonlinearFactorGraph graph;
234 graph.push_back(container: smootherSummarization_);
235 graph.push_back(container: smootherShortcut_);
236 Values values;
237 values.insert(values: smootherSummarizationValues);
238 for(const auto key: newSeparatorKeys) {
239 if(!values.exists(j: key)) {
240 values.insert(j: key, val: separatorValues_.at(j: key));
241 }
242 }
243 // Calculate the summarized factor on just the new separator keys
244 smootherSummarization_ = internal::calculateMarginalFactors(graph, theta: values, remainingKeys: newSeparatorKeys, eliminateFunction: parameters_.getEliminationFunction());
245
246 // Remove the old factors on the separator and insert new
247 removeFactors(slots: separatorSummarizationSlots_);
248 separatorSummarizationSlots_ = insertFactors(factors: smootherSummarization_);
249
250 // Clear the smoother shortcut
251 smootherShortcut_.resize(size: 0);
252 }
253
254 if(debug) { PrintNonlinearFactorGraph(factors: factors_, slots: separatorSummarizationSlots_, indent: "ConcurrentBatchFilter::synchronize ", title: "Current Separator Summarization:", keyFormatter: DefaultKeyFormatter); }
255
256 // Calculate the marginal on the new separator from the filter factors
257 // Note: This could also be done during each filter update so it would simply be available
258 {
259 // Calculate the summarized factor on just the new separator keys (from the filter side)
260 // Copy all of the factors from the filter, not including the smoother summarization
261 NonlinearFactorGraph factors;
262 for(size_t slot = 0; slot < factors_.size(); ++slot) {
263 if(factors_.at(i: slot) && std::find(first: separatorSummarizationSlots_.begin(), last: separatorSummarizationSlots_.end(), val: slot) == separatorSummarizationSlots_.end()) {
264 factors.push_back(factor: factors_.at(i: slot));
265 }
266 }
267 filterSummarization_ = internal::calculateMarginalFactors(graph: factors, theta: theta_, remainingKeys: newSeparatorKeys, eliminateFunction: parameters_.getEliminationFunction());
268 }
269
270 if(debug) { PrintNonlinearFactorGraph(factors: filterSummarization_, indent: "ConcurrentBatchFilter::synchronize ", title: "Updated Filter Summarization:", keyFormatter: DefaultKeyFormatter); }
271
272 if(debug) std::cout << "ConcurrentBatchFilter::synchronize End" << std::endl;
273
274 gttoc(synchronize);
275}
276
277/* ************************************************************************* */
278void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& filterSummarization, Values& filterSummarizationValues) {
279
280 gttic(get_summarized_factors);
281
282 // Copy the previous calculated smoother summarization factors into the output
283 filterSummarization.push_back(container: filterSummarization_);
284 filterSummarizationValues.insert(values: separatorValues_);
285
286 gttoc(get_summarized_factors);
287}
288
289/* ************************************************************************* */
290void ConcurrentBatchFilter::getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) {
291
292 gttic(get_smoother_factors);
293
294 // Copy the previous calculated smoother summarization factors into the output
295 smootherFactors.push_back(container: smootherFactors_);
296 smootherValues.insert(values: smootherValues_);
297
298 gttoc(get_smoother_factors);
299}
300
301/* ************************************************************************* */
302void ConcurrentBatchFilter::postsync() {
303
304 gttic(postsync);
305
306 // Clear out the factors and values that were just sent to the smoother
307 smootherFactors_.resize(size: 0);
308 smootherValues_.clear();
309
310 gttoc(postsync);
311}
312
313/* ************************************************************************* */
314std::vector<size_t> ConcurrentBatchFilter::insertFactors(const NonlinearFactorGraph& factors) {
315
316 gttic(insert_factors);
317
318 // create the output vector
319 std::vector<size_t> slots;
320 slots.reserve(n: factors.size());
321
322 // Insert the factor into an existing hole in the factor graph, if possible
323 for(const NonlinearFactor::shared_ptr& factor: factors) {
324 size_t slot;
325 if(availableSlots_.size() > 0) {
326 slot = availableSlots_.front();
327 availableSlots_.pop();
328 factors_.replace(index: slot, factor);
329 } else {
330 slot = factors_.size();
331 factors_.push_back(factor);
332 }
333 slots.push_back(x: slot);
334 }
335
336 gttoc(insert_factors);
337
338 return slots;
339}
340
341/* ************************************************************************* */
342void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
343
344 gttic(remove_factors);
345
346 // For each factor slot to delete...
347 for(size_t slot: slots) {
348
349 // Remove the factor from the graph
350 factors_.remove(i: slot);
351
352 // Mark the factor slot as available
353 availableSlots_.push(x: slot);
354 }
355
356 gttoc(remove_factors);
357}
358
359/* ************************************************************************* */
360void ConcurrentBatchFilter::reorder(const std::optional<FastList<Key> >& keysToMove) {
361
362 // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
363 if(keysToMove && keysToMove->size() > 0) {
364 ordering_ = Ordering::ColamdConstrainedFirst(graph: factors_, constrainFirst: KeyVector(keysToMove->begin(), keysToMove->end()));
365 }else{
366 ordering_ = Ordering::Colamd(graph: factors_);
367 }
368
369}
370
371/* ************************************************************************* */
372void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
373 VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters,
374 ConcurrentBatchFilter::Result& result) {
375
376 // const bool debug = ISDEBUG("ConcurrentBatchFilter optimize");
377 const bool debug = false;
378
379 if(debug) std::cout << "ConcurrentBatchFilter::optimize Begin" << std::endl;
380
381 // Create output result structure
382 result.nonlinearVariables = theta.size() - linearValues.size();
383 result.linearVariables = linearValues.size();
384
385 // Set optimization parameters
386 double lambda = parameters.lambdaInitial;
387 double lambdaFactor = parameters.lambdaFactor;
388 double lambdaUpperBound = parameters.lambdaUpperBound;
389 double lambdaLowerBound = 1.0e-10;
390 size_t maxIterations = parameters.maxIterations;
391 double relativeErrorTol = parameters.relativeErrorTol;
392 double absoluteErrorTol = parameters.absoluteErrorTol;
393 double errorTol = parameters.errorTol;
394
395 // Create a Values that holds the current evaluation point
396 Values evalpoint = theta.retract(delta);
397 result.error = factors.error(values: evalpoint);
398
399 // check if we're already close enough
400 if(result.error <= errorTol) {
401 if(debug) { std::cout << "Exiting, as error = " << result.error << " < " << errorTol << std::endl; }
402 }
403
404 if(debug) {
405 std::cout << "linearValues: " << linearValues.size() << std::endl;
406 std::cout << "Initial error: " << result.error << std::endl;
407 }
408
409 // Use a custom optimization loop so the linearization points can be controlled
410 double previousError;
411 VectorValues newDelta;
412 do {
413 previousError = result.error;
414
415 // Do next iteration
416 gttic(optimizer_iteration);
417
418 // Linearize graph around the linearization point
419 GaussianFactorGraph linearFactorGraph = *factors.linearize(linearizationPoint: theta);
420
421 // Keep increasing lambda until we make make progress
422 while(true) {
423
424 if(debug) { std::cout << "trying lambda = " << lambda << std::endl; }
425
426 // Add prior factors at the current solution
427 gttic(damp);
428 GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
429 dampedFactorGraph.reserve(size: linearFactorGraph.size() + delta.size());
430 double sigma = 1.0 / std::sqrt(x: lambda);
431
432 // for each of the variables, add a prior at the current solution
433 for(const VectorValues::KeyValuePair& key_value: delta) {
434 size_t dim = key_value.second.size();
435 Matrix A = Matrix::Identity(rows: dim,cols: dim);
436 Vector b = key_value.second;
437 SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
438 GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model));
439 dampedFactorGraph.push_back(factor: prior);
440 }
441
442 gttoc(damp);
443 result.lambdas++;
444
445 gttic(solve);
446 // Solve Damped Gaussian Factor Graph
447 newDelta = dampedFactorGraph.optimize(ordering, function: parameters.getEliminationFunction());
448 // update the evalpoint with the new delta
449 evalpoint = theta.retract(delta: newDelta);
450 gttoc(solve);
451
452 // Evaluate the new nonlinear error
453 gttic(compute_error);
454 double error = factors.error(values: evalpoint);
455 gttoc(compute_error);
456
457 if(debug) {
458 std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
459 std::cout << "next error = " << error << std::endl;
460 }
461
462 if(error < result.error) {
463 // Keep this change
464 // Update the error value
465 result.error = error;
466 // Update the linearization point
467 theta = evalpoint;
468 // Reset the deltas to zeros
469 delta.setZero();
470 // Put the linearization points and deltas back for specific variables
471 if(linearValues.size() > 0) {
472 theta.update(values: linearValues);
473 for(const auto key: linearValues.keys()) {
474 delta.at(j: key) = newDelta.at(j: key);
475 }
476 }
477
478 // Decrease lambda for next time
479 lambda /= lambdaFactor;
480 if(lambda < lambdaLowerBound) {
481 lambda = lambdaLowerBound;
482 }
483 // End this lambda search iteration
484 break;
485 } else {
486 // Reject this change
487 if(lambda >= lambdaUpperBound) {
488 // The maximum lambda has been used. Print a warning and end the search.
489 std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl;
490 break;
491 } else {
492 // Increase lambda and continue searching
493 lambda *= lambdaFactor;
494 }
495 }
496 } // end while
497
498 gttoc(optimizer_iteration);
499
500 if(debug) { std::cout << "using lambda = " << lambda << std::endl; }
501
502 result.iterations++;
503 } while(result.iterations < maxIterations &&
504 !checkConvergence(relativeErrorTreshold: relativeErrorTol, absoluteErrorTreshold: absoluteErrorTol, errorThreshold: errorTol, currentError: previousError, newError: result.error, verbosity: NonlinearOptimizerParams::SILENT));
505
506 if(debug) { std::cout << "newError: " << result.error << std::endl; }
507
508 if(debug) std::cout << "ConcurrentBatchFilter::optimize End" << std::endl;
509}
510
511/* ************************************************************************* */
512void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
513 // In order to move the separator, we need to calculate the marginal information on the new
514 // separator from all of the factors on the smoother side (both the factors actually in the
515 // smoother and the ones to be transitioned to the smoother but stored in the filter).
516 // This is exactly the same operation that is performed by a fixed-lag smoother, calculating
517 // a marginal factor from the variables outside the smoother window.
518 //
519 // However, for the concurrent system, we would like to calculate this marginal in a particular
520 // way, such that an intermediate term is produced that provides a "shortcut" between the old
521 // separator (as defined in the smoother) and the new separator. This will allow us to quickly
522 // update the new separator with changes at the old separator (from the smoother)
523
524 // TODO: This is currently not very efficient: multiple calls to graph.keys(), etc.
525
526// const bool debug = ISDEBUG("ConcurrentBatchFilter moveSeparator");
527 const bool debug = false;
528
529 if(debug) std::cout << "ConcurrentBatchFilter::moveSeparator Begin" << std::endl;
530
531 if(debug) { PrintKeys(keys: keysToMove, indent: "ConcurrentBatchFilter::moveSeparator ", title: "Keys To Move:", keyFormatter: DefaultKeyFormatter); }
532
533
534 // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove)
535 std::vector<size_t> removedFactorSlots;
536 VariableIndex variableIndex(factors_);
537 for(Key key: keysToMove) {
538 const auto& slots = variableIndex[key];
539 removedFactorSlots.insert(position: removedFactorSlots.end(), first: slots.begin(), last: slots.end());
540 }
541
542 // Sort and remove duplicates
543 std::sort(first: removedFactorSlots.begin(), last: removedFactorSlots.end());
544 removedFactorSlots.erase(first: std::unique(first: removedFactorSlots.begin(), last: removedFactorSlots.end()), last: removedFactorSlots.end());
545 // Remove any linear/marginal factor that made it into the set
546 for(size_t index: separatorSummarizationSlots_) {
547 removedFactorSlots.erase(first: std::remove(first: removedFactorSlots.begin(), last: removedFactorSlots.end(), value: index), last: removedFactorSlots.end());
548 }
549
550 // TODO: Make this compact
551 if(debug) {
552 std::cout << "ConcurrentBatchFilter::moveSeparator Removed Factor Slots: ";
553 for(size_t slot: removedFactorSlots) {
554 std::cout << slot << " ";
555 }
556 std::cout << std::endl;
557 }
558
559 // Add these factors to a factor graph
560 NonlinearFactorGraph removedFactors;
561 for(size_t slot: removedFactorSlots) {
562 if(factors_.at(i: slot)) {
563 removedFactors.push_back(factor: factors_.at(i: slot));
564 }
565 }
566
567 if(debug) {
568 PrintNonlinearFactorGraph(factors: removedFactors, indent: "ConcurrentBatchFilter::synchronize ", title: "Removed Factors:", keyFormatter: DefaultKeyFormatter);
569 PrintNonlinearFactorGraph(factors: smootherShortcut_, indent: "ConcurrentBatchFilter::synchronize ", title: "Old Shortcut:", keyFormatter: DefaultKeyFormatter);
570 PrintKeys(keys: smootherShortcut_.keys(), indent: "ConcurrentBatchFilter::moveSeparator ", title: "Old Shortcut Keys:", keyFormatter: DefaultKeyFormatter);
571 PrintKeys(keys: separatorValues_.keys(), indent: "ConcurrentBatchFilter::moveSeparator ", title: "Previous Separator Keys:", keyFormatter: DefaultKeyFormatter);
572 }
573
574 // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
575 KeySet newSeparatorKeys = removedFactors.keys();
576 newSeparatorKeys.merge(other: separatorValues_.keySet());
577 for(Key key: keysToMove) {
578 newSeparatorKeys.erase(x: key);
579 }
580
581 if(debug) { PrintKeys(keys: newSeparatorKeys, indent: "ConcurrentBatchFilter::synchronize ", title: "New Separator Keys:", keyFormatter: DefaultKeyFormatter); }
582
583 // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
584 KeySet shortcutKeys = newSeparatorKeys;
585 for(Key key: smootherSummarization_.keys()) {
586 shortcutKeys.insert(x: key);
587 }
588
589 if(debug) { PrintKeys(keys: shortcutKeys, indent: "ConcurrentBatchFilter::moveSeparator ", title: "Old Shortcut Keys:", keyFormatter: DefaultKeyFormatter); }
590
591 // Calculate a new shortcut
592 {
593 // Combine the previous shortcut factor with all of the new factors being sent to the smoother
594 NonlinearFactorGraph graph;
595 graph.push_back(container: removedFactors);
596 graph.push_back(container: smootherShortcut_);
597 Values values;
598 values.insert(values: smootherValues_);
599 values.insert(values: theta_);
600 // Calculate the summarized factor on the shortcut keys
601 smootherShortcut_ = internal::calculateMarginalFactors(graph, theta: values, remainingKeys: shortcutKeys, eliminateFunction: parameters_.getEliminationFunction());
602 }
603
604 if(debug) {
605 PrintNonlinearFactorGraph(factors: smootherShortcut_, indent: "ConcurrentBatchFilter::synchronize ", title: "New Shortcut:", keyFormatter: DefaultKeyFormatter);
606 PrintNonlinearFactorGraph(factors: smootherSummarization_, indent: "ConcurrentBatchFilter::synchronize ", title: "Smoother Summarization:", keyFormatter: DefaultKeyFormatter);
607 PrintNonlinearFactorGraph(factors: factors_, slots: separatorSummarizationSlots_, indent: "ConcurrentBatchFilter::synchronize ", title: "Current Separator Summarization:", keyFormatter: DefaultKeyFormatter);
608 }
609
610 // Calculate the new smoother summarization on the new separator using the shortcut
611 NonlinearFactorGraph separatorSummarization;
612 {
613 // Combine just the shortcut and the previousSmootherSummarization
614 NonlinearFactorGraph graph;
615 graph.push_back(container: smootherSummarization_);
616 graph.push_back(container: smootherShortcut_);
617 Values values;
618 values.insert(values: smootherValues_);
619 values.insert(values: theta_);
620 // Calculate the summarized factor on just the new separator
621 separatorSummarization = internal::calculateMarginalFactors(graph, theta: values, remainingKeys: newSeparatorKeys, eliminateFunction: parameters_.getEliminationFunction());
622 }
623
624 // Remove the previous marginal factors and insert the new marginal factors
625 removeFactors(slots: separatorSummarizationSlots_);
626 separatorSummarizationSlots_ = insertFactors(factors: separatorSummarization);
627
628 if(debug) { PrintNonlinearFactorGraph(factors: factors_, slots: separatorSummarizationSlots_, indent: "ConcurrentBatchFilter::synchronize ", title: "New Separator Summarization:", keyFormatter: DefaultKeyFormatter); }
629
630 // Update the separatorValues object (should only contain the new separator keys)
631 separatorValues_.clear();
632 for(Key key: separatorSummarization.keys()) {
633 separatorValues_.insert(j: key, val: theta_.at(j: key));
634 }
635
636 // Remove the marginalized factors and add them to the smoother cache
637 smootherFactors_.push_back(container: removedFactors);
638 removeFactors(slots: removedFactorSlots);
639
640 // Add the linearization point of the moved variables to the smoother cache
641 for(Key key: keysToMove) {
642 smootherValues_.insert(j: key, val: theta_.at(j: key));
643 }
644
645 // Remove marginalized keys from values (and separator)
646 for(Key key: keysToMove) {
647 theta_.erase(j: key);
648 ordering_.erase(position: std::find(first: ordering_.begin(), last: ordering_.end(), val: key));
649 delta_.erase(var: key);
650 }
651
652 if(debug) std::cout << "ConcurrentBatchFilter::moveSeparator End" << std::endl;
653}
654
655/* ************************************************************************* */
656}/// namespace gtsam
657