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 ConcurrentFilteringAndSmoothing.cpp
14 * @brief Base classes for the 'filter' and 'smoother' portion of the Concurrent
15 * Filtering and Smoothing architecture, as well as an external synchronization
16 * function. These classes act as an interface only.
17 * @author Stephen Williams
18 */
19
20// \callgraph
21
22#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
23#include <gtsam/nonlinear/LinearContainerFactor.h>
24
25namespace gtsam {
26
27/* ************************************************************************* */
28void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) {
29
30 NonlinearFactorGraph smootherFactors, filterSumarization, smootherSummarization;
31 Values smootherValues, filterSeparatorValues, smootherSeparatorValues;
32
33 // Call the pre-sync functions of the filter and smoother
34 filter.presync();
35 smoother.presync();
36
37 // Get the updates from the smoother and apply them to the filter
38 smoother.getSummarizedFactors(summarizedFactors&: smootherSummarization, separatorValues&: smootherSeparatorValues);
39 filter.synchronize(summarizedFactors: smootherSummarization, separatorValues: smootherSeparatorValues);
40
41 // Get the updates from the filter and apply them to the smoother
42 filter.getSmootherFactors(smootherFactors, smootherValues);
43 filter.getSummarizedFactors(summarizedFactors&: filterSumarization, separatorValues&: filterSeparatorValues);
44 smoother.synchronize(smootherFactors, smootherValues, summarizedFactors: filterSumarization, rootValues: filterSeparatorValues);
45
46 // Call the post-sync functions of the filter and smoother
47 filter.postsync();
48 smoother.postsync();
49}
50
51namespace internal {
52
53/* ************************************************************************* */
54NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
55 const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
56
57
58 // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
59 KeySet rootKeys;
60 KeySet allKeys(graph.keys());
61 std::set_intersection(first1: allKeys.begin(), last1: allKeys.end(), first2: remainingKeys.begin(), last2: remainingKeys.end(), result: std::inserter(x&: rootKeys, i: rootKeys.end()));
62
63 // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
64 KeySet marginalizeKeys;
65 std::set_difference(first1: allKeys.begin(), last1: allKeys.end(), first2: remainingKeys.begin(), last2: remainingKeys.end(), result: std::inserter(x&: marginalizeKeys, i: marginalizeKeys.end()));
66
67 if(marginalizeKeys.size() == 0) {
68 // There are no keys to marginalize. Simply return the input factors
69 return graph;
70 } else {
71 // Create the linear factor graph
72 GaussianFactorGraph linearFactorGraph = *graph.linearize(linearizationPoint: theta);
73 // .first is the eliminated Bayes tree, while .second is the remaining factor graph
74 GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
75 variables: KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), function: eliminateFunction).second;
76
77 // Wrap in nonlinear container factors
78 NonlinearFactorGraph marginalFactors;
79 marginalFactors.reserve(size: marginalLinearFactors.size());
80 for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
81 marginalFactors.emplace_shared<LinearContainerFactor>(args: gaussianFactor, args: theta);
82 }
83
84 return marginalFactors;
85 }
86}
87
88/* ************************************************************************* */
89}/// namespace internal
90
91/* ************************************************************************* */
92}/// namespace gtsam
93