EliminateableFactorGraph-inst.h
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 
19 #pragma once
20 
23 
24 namespace gtsam {
25 
26  /* ************************************************************************* */
27  template<class FACTORGRAPH>
28  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
30  OptionalOrderingType orderingType, const Eliminate& function,
31  OptionalVariableIndex variableIndex) const {
32  if(!variableIndex) {
33  // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
34  // for no variable index first so that it's always computed if we need to call COLAMD because
35  // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
36  // before creating ordering.
37  VariableIndex computedVariableIndex(asDerived());
38  return eliminateSequential(orderingType, function, std::cref(computedVariableIndex));
39  }
40  else {
41  // Compute an ordering and call this function again. We are guaranteed to have a
42  // VariableIndex already here because we computed one if needed in the previous 'if' block.
43  if (orderingType == Ordering::METIS) {
44  Ordering computedOrdering = Ordering::Metis(asDerived());
45  return eliminateSequential(computedOrdering, function, variableIndex);
46  } else if (orderingType == Ordering::COLAMD) {
47  Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
48  return eliminateSequential(computedOrdering, function, variableIndex);
49  } else if (orderingType == Ordering::NATURAL) {
50  Ordering computedOrdering = Ordering::Natural(asDerived());
51  return eliminateSequential(computedOrdering, function, variableIndex);
52  } else {
53  Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
54  asDerived(), *variableIndex);
55  return eliminateSequential(computedOrdering, function, variableIndex);
56  }
57  }
58  }
59 
60  /* ************************************************************************* */
61  template<class FACTORGRAPH>
62  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
64  const Ordering& ordering, const Eliminate& function,
65  OptionalVariableIndex variableIndex) const
66  {
67  if(!variableIndex) {
68  // If no VariableIndex provided, compute one and call this function again
69  VariableIndex computedVariableIndex(asDerived());
70  return eliminateSequential(ordering, function, std::cref(computedVariableIndex));
71  } else {
72  gttic(eliminateSequential);
73  // Do elimination
74  EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
75  const auto [bayesNet, factorGraph] = etree.eliminate(function);
76  // If any factors are remaining, the ordering was incomplete
77  if(!factorGraph->empty())
79  // Return the Bayes net
80  return bayesNet;
81  }
82  }
83 
84  /* ************************************************************************* */
85  template <class FACTORGRAPH>
86  std::shared_ptr<
89  OptionalOrderingType orderingType, const Eliminate& function,
90  OptionalVariableIndex variableIndex) const {
91  if (!variableIndex) {
92  // If no VariableIndex provided, compute one and call this function again
93  // IMPORTANT: we check for no variable index first so that it's always
94  // computed if we need to call COLAMD because no Ordering is provided.
95  // When removing optional from VariableIndex, create VariableIndex before
96  // creating ordering.
97  VariableIndex computedVariableIndex(asDerived());
98  return eliminateMultifrontal(orderingType, function,
99  std::cref(computedVariableIndex));
100  } else {
101  // Compute an ordering and call this function again. We are guaranteed to
102  // have a VariableIndex already here because we computed one if needed in
103  // the previous 'if' block.
104  if (orderingType == Ordering::METIS) {
105  Ordering computedOrdering = Ordering::Metis(asDerived());
106  return eliminateMultifrontal(computedOrdering, function, variableIndex);
107  } else if (orderingType == Ordering::COLAMD) {
108  Ordering computedOrdering = Ordering::Colamd((*variableIndex).get());
109  return eliminateMultifrontal(computedOrdering, function, variableIndex);
110  } else if (orderingType == Ordering::NATURAL) {
111  Ordering computedOrdering = Ordering::Natural(asDerived());
112  return eliminateMultifrontal(computedOrdering, function, variableIndex);
113  } else {
114  Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
115  asDerived(), *variableIndex);
116  return eliminateMultifrontal(computedOrdering, function, variableIndex);
117  }
118  }
119  }
120 
121  /* ************************************************************************* */
122  template<class FACTORGRAPH>
123  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
125  const Ordering& ordering, const Eliminate& function,
126  OptionalVariableIndex variableIndex) const
127  {
128  if(!variableIndex) {
129  // If no VariableIndex provided, compute one and call this function again
130  VariableIndex computedVariableIndex(asDerived());
131  return eliminateMultifrontal(ordering, function, std::cref(computedVariableIndex));
132  } else {
133  gttic(eliminateMultifrontal);
134  // Do elimination with given ordering
135  EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
136  JunctionTreeType junctionTree(etree);
137  const auto [bayesTree, factorGraph] = junctionTree.eliminate(function);
138  // If any factors are remaining, the ordering was incomplete
139  if(!factorGraph->empty())
141  // Return the Bayes tree
142  return bayesTree;
143  }
144  }
145 
146  /* ************************************************************************* */
147  template<class FACTORGRAPH>
148  std::pair<std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, std::shared_ptr<FACTORGRAPH> >
150  const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
151  {
152  if(variableIndex) {
153  gttic(eliminatePartialSequential);
154  // Do elimination
155  EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
156  return etree.eliminate(function);
157  } else {
158  // If no variable index is provided, compute one and call this function again
159  VariableIndex computedVariableIndex(asDerived());
160  return eliminatePartialSequential(ordering, function, std::cref(computedVariableIndex));
161  }
162  }
163 
164  /* ************************************************************************* */
165  template<class FACTORGRAPH>
166  std::pair<std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, std::shared_ptr<FACTORGRAPH> >
168  const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
169  {
170  if(variableIndex) {
171  gttic(eliminatePartialSequential);
172  // Compute full ordering
173  Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables);
174 
175  // Split off the part of the ordering for the variables being eliminated
176  Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
177  return eliminatePartialSequential(ordering, function, variableIndex);
178  } else {
179  // If no variable index is provided, compute one and call this function again
180  VariableIndex computedVariableIndex(asDerived());
181  return eliminatePartialSequential(variables, function, std::cref(computedVariableIndex));
182  }
183  }
184 
185  /* ************************************************************************* */
186  template<class FACTORGRAPH>
187  std::pair<std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, std::shared_ptr<FACTORGRAPH> >
189  const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
190  {
191  if(variableIndex) {
192  gttic(eliminatePartialMultifrontal);
193  // Do elimination
194  EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering);
195  JunctionTreeType junctionTree(etree);
196  return junctionTree.eliminate(function);
197  } else {
198  // If no variable index is provided, compute one and call this function again
199  VariableIndex computedVariableIndex(asDerived());
200  return eliminatePartialMultifrontal(ordering, function, std::cref(computedVariableIndex));
201  }
202  }
203 
204  /* ************************************************************************* */
205  template<class FACTORGRAPH>
206  std::pair<std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, std::shared_ptr<FACTORGRAPH> >
208  const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
209  {
210  if(variableIndex) {
211  gttic(eliminatePartialMultifrontal);
212  // Compute full ordering
213  Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables);
214 
215  // Split off the part of the ordering for the variables being eliminated
216  Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
217  return eliminatePartialMultifrontal(ordering, function, variableIndex);
218  } else {
219  // If no variable index is provided, compute one and call this function again
220  VariableIndex computedVariableIndex(asDerived());
221  return eliminatePartialMultifrontal(variables, function, std::cref(computedVariableIndex));
222  }
223  }
224 
225  /* ************************************************************************* */
226  template<class FACTORGRAPH>
227  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
229  const Ordering& variables,
230  const Eliminate& function, OptionalVariableIndex variableIndex) const
231  {
232  if(!variableIndex) {
233  // If no variable index is provided, compute one and call this function again
234  VariableIndex index(asDerived());
235  return marginalMultifrontalBayesNet(variables, function, std::cref(index));
236  } else {
237  // No ordering was provided for the marginalized variables, so order them using constrained
238  // COLAMD.
239  constexpr bool forceOrder = true;
240  Ordering totalOrdering =
241  Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
242 
243  // Split up ordering
244  const size_t nVars = variables.size();
245  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
246  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
247 
248  // Call this function again with the computed orderings
249  return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
250  }
251  }
252 
253  /* ************************************************************************* */
254  template<class FACTORGRAPH>
255  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
257  const KeyVector& variables,
258  const Eliminate& function, OptionalVariableIndex variableIndex) const
259  {
260  if(!variableIndex) {
261  // If no variable index is provided, compute one and call this function again
262  VariableIndex index(asDerived());
263  return marginalMultifrontalBayesNet(variables, function, std::cref(index));
264  } else {
265  // No ordering was provided for the marginalized variables, so order them using constrained
266  // COLAMD.
267  const constexpr bool forceOrder = false;
268  Ordering totalOrdering =
269  Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
270 
271  // Split up ordering
272  const size_t nVars = variables.size();
273  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
274  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
275 
276  // Call this function again with the computed orderings
277  return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
278  }
279  }
280 
281  /* ************************************************************************* */
282  template<class FACTORGRAPH>
283  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
285  const Ordering& variables,
286  const Ordering& marginalizedVariableOrdering,
287  const Eliminate& function, OptionalVariableIndex variableIndex) const
288  {
289  if(!variableIndex) {
290  // If no variable index is provided, compute one and call this function again
291  VariableIndex index(asDerived());
292  return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
293  } else {
294  gttic(marginalMultifrontalBayesNet);
295  // An ordering was provided for the marginalized variables, so we can first eliminate them
296  // in the order requested.
297  const auto [bayesTree, factorGraph] =
298  eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
299 
300  // An ordering was also provided for the unmarginalized variables, so we can also
301  // eliminate them in the order requested.
302  return factorGraph->eliminateSequential(variables, function);
303  }
304  }
305 
306  /* ************************************************************************* */
307  template<class FACTORGRAPH>
308  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
310  const KeyVector& variables,
311  const Ordering& marginalizedVariableOrdering,
312  const Eliminate& function, OptionalVariableIndex variableIndex) const
313  {
314  if(!variableIndex) {
315  // If no variable index is provided, compute one and call this function again
316  VariableIndex index(asDerived());
317  return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
318  } else {
319  gttic(marginalMultifrontalBayesNet);
320  // An ordering was provided for the marginalized variables, so we can first eliminate them
321  // in the order requested.
322  const auto [bayesTree, factorGraph] =
323  eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
324 
325  // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
326  return factorGraph->eliminateSequential(Ordering::COLAMD, function);
327  }
328  }
329 
330  /* ************************************************************************* */
331  template<class FACTORGRAPH>
332  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
334  const Ordering& variables,
335  const Eliminate& function, OptionalVariableIndex variableIndex) const
336  {
337  if(!variableIndex) {
338  // If no variable index is provided, compute one and call this function again
339  VariableIndex computedVariableIndex(asDerived());
340  return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex));
341  } else {
342  // No ordering was provided for the marginalized variables, so order them using constrained
343  // COLAMD.
344  constexpr bool forceOrder = true;
345  Ordering totalOrdering =
346  Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
347 
348  // Split up ordering
349  const size_t nVars = variables.size();
350  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
351  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
352 
353  // Call this function again with the computed orderings
354  return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
355  }
356  }
357 
358  /* ************************************************************************* */
359  template<class FACTORGRAPH>
360  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
362  const KeyVector& variables,
363  const Eliminate& function, OptionalVariableIndex variableIndex) const
364  {
365  if(!variableIndex) {
366  // If no variable index is provided, compute one and call this function again
367  VariableIndex computedVariableIndex(asDerived());
368  return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex));
369  } else {
370  // No ordering was provided for the marginalized variables, so order them using constrained
371  // COLAMD.
372  constexpr bool forceOrder = false;
373  Ordering totalOrdering =
374  Ordering::ColamdConstrainedLast((*variableIndex).get(), variables, forceOrder);
375 
376  // Split up ordering
377  const size_t nVars = variables.size();
378  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
379  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
380 
381  // Call this function again with the computed orderings
382  return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, variableIndex);
383  }
384  }
385 
386  /* ************************************************************************* */
387  template<class FACTORGRAPH>
388  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
390  const Ordering& variables,
391  const Ordering& marginalizedVariableOrdering,
392  const Eliminate& function, OptionalVariableIndex variableIndex) const
393  {
394  if(!variableIndex) {
395  // If no variable index is provided, compute one and call this function again
396  VariableIndex computedVariableIndex(asDerived());
397  return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex));
398  } else {
399  gttic(marginalMultifrontalBayesTree);
400  // An ordering was provided for the marginalized variables, so we can first eliminate them
401  // in the order requested.
402  const auto [bayesTree, factorGraph] =
403  eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
404 
405  // An ordering was also provided for the unmarginalized variables, so we can also
406  // eliminate them in the order requested.
407  return factorGraph->eliminateMultifrontal(variables, function);
408  }
409  }
410 
411  /* ************************************************************************* */
412  template<class FACTORGRAPH>
413  std::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
415  const KeyVector& variables,
416  const Ordering& marginalizedVariableOrdering,
417  const Eliminate& function, OptionalVariableIndex variableIndex) const
418  {
419  if(!variableIndex) {
420  // If no variable index is provided, compute one and call this function again
421  VariableIndex computedVariableIndex(asDerived());
422  return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex));
423  } else {
424  gttic(marginalMultifrontalBayesTree);
425  // An ordering was provided for the marginalized variables, so we can first eliminate them
426  // in the order requested.
427  const auto [bayesTree, factorGraph] =
428  eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex);
429 
430  // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
431  return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
432  }
433  }
434 
435  /* ************************************************************************* */
436  template<class FACTORGRAPH>
437  std::shared_ptr<FACTORGRAPH>
439  const KeyVector& variables,
440  const Eliminate& function, OptionalVariableIndex variableIndex) const
441  {
442  if(variableIndex)
443  {
444  // Compute a total ordering for all variables
445  Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), variables);
446 
447  // Split out the part for the marginalized variables
448  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
449 
450  // Eliminate and return the remaining factor graph
451  return eliminatePartialMultifrontal(marginalizationOrdering, function, variableIndex).second;
452  }
453  else
454  {
455  // If no variable index is provided, compute one and call this function again
456  VariableIndex computedVariableIndex(asDerived());
457  return marginal(variables, function, std::cref(computedVariableIndex));
458  }
459  }
460 
461 
462 }
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition: Ordering.cpp:210
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
std::optional< std::reference_wrapper< const VariableIndex > > OptionalVariableIndex
std::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Variable elimination algorithms for factor graphs.
std::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Exceptions that may be thrown by inference algorithms.
static enum @1107 ordering
static Ordering Colamd(const FACTOR_GRAPH &graph)
#define gttic(label)
Definition: timing.h:295
std::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
EliminationTraitsType::JunctionTreeType JunctionTreeType
Junction tree type that can do multifrontal elimination of this graph.
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
traits
Definition: chartTesting.h:28
EliminationTraitsType::EliminationTreeType EliminationTreeType
Elimination tree type that can do sequential elimination of this graph.
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
EliminationTraitsType::BayesTreeType BayesTreeType
Bayes tree type produced by multifrontal elimination.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::optional< Ordering::OrderingType > OptionalOrderingType
Typedef for an optional ordering type.
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.


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