ConcurrentIncrementalFilter.cpp
Go to the documentation of this file.
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 
20 #include <gtsam/base/timing.h>
21 #include <gtsam/base/debug.h>
22 
23 namespace gtsam {
24 
25 /* ************************************************************************* */
26 void ConcurrentIncrementalFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const {
27  std::cout << s;
28  isam2_.print("");
29 }
30 
31 /* ************************************************************************* */
33  const ConcurrentIncrementalFilter* filter = dynamic_cast<const ConcurrentIncrementalFilter*>(&rhs);
34  return filter
35  && isam2_.equals(filter->isam2_)
42 }
43 
44 /* ************************************************************************* */
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(removedFactors.end(), removeFactorIndices->begin(), 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[](key) = group;
74  }
75  ++group;
76  }
77  // Assign new variables to the root
78  for(const auto key: newTheta.keys()) {
79  orderingConstraints->operator[](key) = group;
80  }
81  // Set marginalizable variables to Group0
82  for(Key key: *keysToMove){
83  orderingConstraints->operator[](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) {
98  GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
99  while(*key_iter != key) {
100  markedKeys.insert(*key_iter);
101  ++key_iter;
102  }
103  for(const ISAM2Clique::shared_ptr& child: clique->children) {
104  RecursiveMarkAffectedKeys(key, child, 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, removedFactors, orderingConstraints, noRelinKeys, 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
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(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) {
133  }
134  gttoc(cache_smoother_factors);
135 
136  gttic(marginalize);
137  FactorIndices marginalFactorsIndices;
138  FactorIndices deletedFactorsIndices;
139  isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
140  currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
141  for(size_t index: deletedFactorsIndices) {
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();
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 /* ************************************************************************* */
168 
169  gttic(presync);
170 
171  gttoc(presync);
172 }
173 
174 /* ************************************************************************* */
175 void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smootherSummarization, const Values& smootherSummarizationValues) {
176 
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
194  Values values;
195  values.insert(smootherSummarizationValues);
196  for(Key key: newSeparatorKeys) {
197  if(!values.exists(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(key);
206  }
207 
208  // Calculate the summarized factor on just the new separator keys
209  NonlinearFactorGraph currentSmootherSummarization = internal::calculateMarginalFactors(graph, values, newSeparatorKeys,
211 
212  // Remove the old factors on the separator and insert the new ones
214  ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, {}, noRelinKeys, {}, 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;
220 
221  if(debug) std::cout << "ConcurrentIncrementalFilter::synchronize End" << std::endl;
222 
224 }
225 
226 /* ************************************************************************* */
227 void 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(key, isam2_.getLinearizationPoint().at(key));
237  }
238 
239  gttoc(get_summarized_factors);
240 }
241 
242 /* ************************************************************************* */
244 
245  gttic(get_smoother_factors);
246 
247  // Copy the previous calculated smoother summarization factors into the output
248  smootherFactors.push_back(smootherFactors_);
249  smootherValues.insert(smootherValues_);
250 
251  gttoc(get_smoother_factors);
252 }
253 
254 /* ************************************************************************* */
256 
257  gttic(postsync);
258 
259  // Clear out the factors and values that were just sent to the smoother
262 
263  gttoc(postsync);
264 }
265 
266 
267 /* ************************************************************************* */
268 void 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(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) {
272 
273  // Mark the frontal keys of the current clique
274  for(Key idx: clique->conditional()->frontals()) {
275  additionalKeys.insert(idx);
276  }
277 
278  // Recursively mark all of the children
279  for(const ISAM2Clique::shared_ptr& child: clique->children) {
280  RecursiveMarkAffectedKeys(key, 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 /* ************************************************************************* */
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(removedFactorSlots.end(), slots.begin(), slots.end());
296  }
297 
298  // Sort and remove duplicates
299  std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
300  removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
301 
302  // Remove any linear/marginal factor that made it into the set
303  for(size_t index: factorsToIgnore) {
304  removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
305  }
306 
307  return removedFactorSlots;
308 }
309 
311 // TODO: Make this a static function
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(slot);
318  if(factor) {
319  shortcutKeys.insert(factor->begin(), factor->end());
320  }
321  }
323  shortcutKeys.insert(key);
324  }
325 
326  // Combine the previous shortcut factor with all of the new factors being sent to the smoother
328  graph.push_back(removedFactors);
330  Values values;
331  values.insert(smootherValues_);
333  // Calculate the summarized factor on the shortcut keys
334  smootherShortcut_ = internal::calculateMarginalFactors(graph, values, shortcutKeys,
336 }
337 
338 /* ************************************************************************* */
339 // TODO: Make this a static function
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) {
352  separatorCliques.insert( 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(key);
360  }
361  }
362  std::sort(cliqueKeys.begin(), 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(slot);
369  if(factor) {
370  std::set<Key> factorKeys(factor->begin(), factor->end());
371  if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
372  cliqueFactorSlots.insert(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(slot);
381  }
382 
383  // Create a factor graph from the identified factors
385  for(size_t slot: cliqueFactorSlots) {
386  graph.push_back(isam2_.getFactorsUnsafe().at(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(clique->children.begin(), 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(clique);
398  }
399 
400  // Augment the factor graph with cached factors from the children
401  for(const ISAM2Clique::shared_ptr& clique: childCliques) {
403  graph.push_back( factor );
404  }
405 
406  // Calculate the marginal factors on the separator
407  NonlinearFactorGraph filterSummarization = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys,
409 
410  return filterSummarization;
411 }
412 
413 /* ************************************************************************* */
414 }
const gtsam::Symbol key('X', 0)
void clear()
Definition: Values.h:298
size_t nonlinearVariables
The number of variables that can be relinearized.
KeySet keys() const
Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const std::optional< FastList< Key > > &keysToMove={}, const std::optional< FactorIndices > &removeFactorIndices={})
Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph...
size_t variablesReeliminated
Definition: ISAM2Result.h:82
Global debugging flags.
const ValueType at(Key j) const
Definition: Values-inl.h:204
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
ISAM2 isam2_
The iSAM2 inference engine.
leaf::MyValues values
KeyVector keys() const
Definition: Values.cpp:218
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const std::optional< FastMap< Key, int > > &constrainedKeys={}, const std::optional< FastList< Key > > &noRelinKeys={}, const std::optional< FastList< Key > > &extraReelimKeys={}, bool force_relinearize=false)
Definition: ISAM2.cpp:400
virtual bool equals(const ISAM2 &other, double tol=1e-9) const
Definition: ISAM2.cpp:57
bool equals(const ConcurrentFilter &rhs, double tol=1e-9) const override
NonlinearFactorGraph graph
void getSmootherFactors(NonlinearFactorGraph &smootherFactors, Values &smootherValues) override
static constexpr bool debug
NonlinearFactorGraph previousSmootherSummarization_
The smoother summarization on the old separator sent by the smoother during the last synchronization...
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition: ISAM2.h:217
static void RecursiveMarkAffectedKeys(const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
void getSummarizedFactors(NonlinearFactorGraph &filterSummarization, Values &filterSummarizationValues) override
FactorIndices newFactorsIndices
Definition: ISAM2Result.h:95
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition: Factor.h:36
std::shared_ptr< This > shared_ptr
Definition: ISAM2Clique.h:41
#define gttic(label)
Definition: timing.h:295
const NonlinearFactorGraph & getFactorsUnsafe() const
Definition: ISAM2.h:269
Values result
GaussianFactorGraph::Eliminate getEliminationFunction() const
Definition: ISAM2Params.h:323
void print(const std::string &s="Concurrent Incremental Filter:\, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
void updateShortcut(const NonlinearFactorGraph &removedFactors)
size_t variablesRelinearized
Definition: ISAM2Result.h:74
std::shared_ptr< This > shared_ptr
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph &graph, const Values &theta, const KeySet &remainingKeys, const GaussianFactorGraph::Eliminate &eliminateFunction)
RealScalar s
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
size_t size() const
Definition: Values.h:178
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
size_t iterations
The number of optimizer iterations performed.
void marginalizeLeaves(const FastList< Key > &leafKeys, FactorIndices *marginalFactorsIndices=nullptr, FactorIndices *deletedFactorsIndices=nullptr)
Definition: ISAM2.cpp:483
NonlinearFactorGraph calculateFilterSummarization() const
void synchronize(const NonlinearFactorGraph &smootherSummarization, const Values &smootherSummarizationValues) override
traits
Definition: chartTesting.h:28
std::vector< float > Values
const KeySet & getFixedVariables() const
Definition: ISAM2.h:277
virtual void resize(size_t size)
Definition: FactorGraph.h:389
FactorIndices currentSmootherSummarizationSlots_
The slots in factor graph that correspond to the current smoother summarization on the current separa...
#define gttoc(label)
Definition: timing.h:296
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
std::shared_ptr< This > shared_ptr
size_t linearVariables
The number of variables that must keep a constant linearization point.
const VariableIndex & getVariableIndex() const
Definition: ISAM2.h:274
void insert(Key j, const Value &val)
Definition: Values.cpp:155
const G double tol
Definition: Group.h:86
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
NonlinearFactorGraph smootherShortcut_
A set of conditional factors from the old separator to the current separator (recursively calculated ...
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Values smootherValues_
A temporary holding place for the linearization points of all keys being sent to the smoother...
bool exists(Key j) const
Definition: Values.cpp:93
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
NonlinearFactorGraph smootherFactors_
A temporary holding place for the set of full nonlinear factors being sent to the smoother...
static FactorIndices FindAdjacentFactors(const ISAM2 &isam2, const FastList< Key > &keys, const FactorIndices &factorsToIgnore)
const ISAM2Params & params() const
Definition: ISAM2.h:279
Timing utilities.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:03