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 ConcurrentIncrementalSmoother.cpp
14 * @brief An iSAM2-based Smoother that implements the
15 * Concurrent Filtering and Smoothing interface.
16 * @author Stephen Williams
17 */
18
19#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
20#include <gtsam/nonlinear/LinearContainerFactor.h>
21#include <gtsam/base/timing.h>
22#include <gtsam/base/debug.h>
23
24namespace gtsam {
25
26/* ************************************************************************* */
27void ConcurrentIncrementalSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const {
28 std::cout << s;
29 isam2_.print(s: "");
30}
31
32/* ************************************************************************* */
33bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double tol) const {
34 const ConcurrentIncrementalSmoother* smoother = dynamic_cast<const ConcurrentIncrementalSmoother*>(&rhs);
35 return smoother
36 && isam2_.equals(other: smoother->isam2_)
37 && smootherFactors_.equals(other: smoother->smootherFactors_)
38 && smootherValues_.equals(other: smoother->smootherValues_)
39 && filterSummarizationFactors_.equals(other: smoother->filterSummarizationFactors_)
40 && separatorValues_.equals(other: smoother->separatorValues_)
41 && (filterSummarizationSlots_.size() == smoother->filterSummarizationSlots_.size())
42 && std::equal(first1: filterSummarizationSlots_.begin(), last1: filterSummarizationSlots_.end(), first2: smoother->filterSummarizationSlots_.begin())
43 && smootherSummarization_.equals(other: smoother->smootherSummarization_);
44}
45
46/* ************************************************************************* */
47ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
48 const std::optional<FactorIndices>& removeFactorIndices) {
49
50 gttic(update);
51
52 // Create the return result meta-data
53 Result result;
54
55 FastVector<size_t> removedFactors;
56
57 if(removeFactorIndices){
58 // Be very careful to this line
59 std::cout << "ConcurrentIncrementalSmoother::update removeFactorIndices - not implemented yet" << std::endl;
60 filterSummarizationSlots_.insert(position: filterSummarizationSlots_.end(), first: removeFactorIndices->begin(), last: removeFactorIndices->end());
61 // before it was:
62 // removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
63 }
64
65 // Constrain the separator keys to remain in the root
66 // Also, mark the separator keys as fixed linearization points
67 FastMap<Key,int> constrainedKeys;
68 FastList<Key> noRelinKeys;
69 for (const auto key : separatorValues_.keys()) {
70 constrainedKeys[key] = 1;
71 noRelinKeys.push_back(x: key);
72 }
73
74 // Use iSAM2 to perform an update
75 ISAM2Result isam2Result;
76 if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) {
77 if(synchronizationUpdatesAvailable_) {
78 // Augment any new factors/values with the cached data from the last synchronization
79 NonlinearFactorGraph graph(newFactors);
80 graph.push_back(container: smootherFactors_);
81 graph.push_back(container: filterSummarizationFactors_);
82 Values values(newTheta);
83 // Unfortunately, we must be careful here, as some of the smoother values
84 // and/or separator values may have been added previously
85 for(const auto key: smootherValues_.keys()) {
86 if(!isam2_.getLinearizationPoint().exists(j: key)) {
87 values.insert(j: key, val: smootherValues_.at(j: key));
88 }
89 }
90 for(const auto key: separatorValues_.keys()) {
91 if(!isam2_.getLinearizationPoint().exists(j: key)) {
92 values.insert(j: key, val: separatorValues_.at(j: key));
93 }
94 }
95
96 // Update the system using iSAM2
97 isam2Result = isam2_.update(newFactors: graph, newTheta: values, removeFactorIndices: filterSummarizationSlots_, constrainedKeys, noRelinKeys);
98
99 // Clear out the cache and update the filter summarization slots
100 smootherFactors_.resize(size: 0);
101 smootherValues_.clear();
102 filterSummarizationSlots_.clear();
103 filterSummarizationSlots_.insert(position: filterSummarizationSlots_.end(),
104 first: isam2Result.newFactorsIndices.end()-filterSummarizationFactors_.size(), last: isam2Result.newFactorsIndices.end());
105 filterSummarizationFactors_.resize(size: 0);
106 synchronizationUpdatesAvailable_ = false;
107 } else {
108 // Update the system using iSAM2
109 isam2Result = isam2_.update(newFactors, newTheta, removeFactorIndices: FactorIndices(), constrainedKeys, noRelinKeys);
110 }
111 }
112
113 // Extract the ConcurrentIncrementalSmoother::Result information
114 result.iterations = 1;
115 result.linearVariables = separatorValues_.size();
116 result.nonlinearVariables = isam2_.getLinearizationPoint().size() - separatorValues_.size();
117 result.error = isam2_.getFactorsUnsafe().error(values: isam2_.calculateEstimate());
118
119 // Calculate the marginal on the separator from the smoother factors
120 if(separatorValues_.size() > 0) {
121 gttic(presync);
122 updateSmootherSummarization();
123 gttoc(presync);
124 }
125
126 gttoc(update);
127
128 return result;
129}
130
131/* ************************************************************************* */
132void ConcurrentIncrementalSmoother::presync() {
133
134 gttic(presync);
135
136 gttoc(presync);
137}
138
139/* ************************************************************************* */
140void ConcurrentIncrementalSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& separatorValues) {
141
142 gttic(get_summarized_factors);
143
144 // Copy the previous calculated smoother summarization factors into the output
145 summarizedFactors.push_back(container: smootherSummarization_);
146
147 // Copy the separator values into the output
148 separatorValues.insert(values: separatorValues_);
149
150 gttoc(get_summarized_factors);
151}
152
153/* ************************************************************************* */
154void ConcurrentIncrementalSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues,
155 const NonlinearFactorGraph& summarizedFactors, const Values& separatorValues) {
156
157 gttic(synchronize);
158
159 // Store the new smoother factors and values for addition during the next update call
160 smootherFactors_ = smootherFactors;
161 smootherValues_ = smootherValues;
162
163 // Store the new filter summarization and separator, to be replaced during the next update call
164 filterSummarizationFactors_ = summarizedFactors;
165 separatorValues_ = separatorValues;
166
167 // Flag the next smoother update to include the synchronization data
168 synchronizationUpdatesAvailable_ = true;
169
170 gttoc(synchronize);
171}
172
173/* ************************************************************************* */
174void ConcurrentIncrementalSmoother::postsync() {
175
176 gttic(postsync);
177
178 gttoc(postsync);
179}
180
181/* ************************************************************************* */
182void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
183
184 // The smoother summarization factors are the resulting marginal factors on the separator
185 // variables that result from marginalizing out all of the other variables
186 // These marginal factors will be cached for later transmission to the filter using
187 // linear container factors
188
189 // Find all cliques that contain any separator variables
190 std::set<ISAM2Clique::shared_ptr> separatorCliques;
191 for(Key key: separatorValues_.keys()) {
192 ISAM2Clique::shared_ptr clique = isam2_[key];
193 separatorCliques.insert( x: clique );
194 }
195
196 // Create the set of clique keys LC:
197 KeyVector cliqueKeys;
198 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
199 for(Key key: clique->conditional()->frontals()) {
200 cliqueKeys.push_back(x: key);
201 }
202 }
203 std::sort(first: cliqueKeys.begin(), last: cliqueKeys.end());
204
205 // Gather all factors that involve only clique keys
206 std::set<size_t> cliqueFactorSlots;
207 for(Key key: cliqueKeys) {
208 for(size_t slot: isam2_.getVariableIndex()[key]) {
209 const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(i: slot);
210 if(factor) {
211 std::set<Key> factorKeys(factor->begin(), factor->end());
212 if(std::includes(first1: cliqueKeys.begin(), last1: cliqueKeys.end(), first2: factorKeys.begin(), last2: factorKeys.end())) {
213 cliqueFactorSlots.insert(x: slot);
214 }
215 }
216 }
217 }
218
219 // Remove any factor included in the filter summarization
220 for(size_t slot: filterSummarizationSlots_) {
221 cliqueFactorSlots.erase(x: slot);
222 }
223
224 // Create a factor graph from the identified factors
225 NonlinearFactorGraph graph;
226 for(size_t slot: cliqueFactorSlots) {
227 graph.push_back(factor: isam2_.getFactorsUnsafe().at(i: slot));
228 }
229
230 // Find the set of children of the separator cliques
231 std::set<ISAM2Clique::shared_ptr> childCliques;
232 // Add all of the children
233 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
234 childCliques.insert(first: clique->children.begin(), last: clique->children.end());
235 }
236 // Remove any separator cliques that were added because they were children of other separator cliques
237 for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
238 childCliques.erase(x: clique);
239 }
240
241 // Augment the factor graph with cached factors from the children
242 for(const ISAM2Clique::shared_ptr& clique: childCliques) {
243 LinearContainerFactor::shared_ptr factor(new LinearContainerFactor(clique->cachedFactor(), isam2_.getLinearizationPoint()));
244 graph.push_back( factor );
245 }
246
247 // Get the set of separator keys
248 const KeySet separatorKeys = separatorValues_.keySet();
249
250 // Calculate the marginal factors on the separator
251 smootherSummarization_ = internal::calculateMarginalFactors(graph, theta: isam2_.getLinearizationPoint(), remainingKeys: separatorKeys,
252 eliminateFunction: isam2_.params().getEliminationFunction());
253}
254
255/* ************************************************************************* */
256
257}/// namespace gtsam
258