gtsam  4.0.0
gtsam
EliminateableFactorGraph-inst.h
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 #include <boost/tuple/tuple.hpp>
24 
25 namespace gtsam {
26 
27  /* ************************************************************************* */
28  template<class FACTORGRAPH>
29  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
31  OptionalOrdering ordering, const Eliminate& function,
32  OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
33  {
34  if(ordering && variableIndex) {
35  gttic(eliminateSequential);
36  // Do elimination
37  EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
38  boost::shared_ptr<BayesNetType> bayesNet;
39  boost::shared_ptr<FactorGraphType> factorGraph;
40  boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
41  // If any factors are remaining, the ordering was incomplete
42  if(!factorGraph->empty())
44  // Return the Bayes net
45  return bayesNet;
46  }
47  else if(!variableIndex) {
48  // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
49  // for no variable index first so that it's always computed if we need to call COLAMD because
50  // no Ordering is provided.
51  VariableIndex computedVariableIndex(asDerived());
52  return eliminateSequential(ordering, function, computedVariableIndex, orderingType);
53  }
54  else /*if(!ordering)*/ {
55  // If no Ordering provided, compute one and call this function again. We are guaranteed to
56  // have a VariableIndex already here because we computed one if needed in the previous 'else'
57  // block.
58  if (orderingType == Ordering::METIS) {
59  Ordering computedOrdering = Ordering::Metis(asDerived());
60  return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
61  } else {
62  Ordering computedOrdering = Ordering::Colamd(*variableIndex);
63  return eliminateSequential(computedOrdering, function, variableIndex, orderingType);
64  }
65  }
66  }
67 
68  /* ************************************************************************* */
69  template<class FACTORGRAPH>
70  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
72  OptionalOrdering ordering, const Eliminate& function,
73  OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const
74  {
75  if(ordering && variableIndex) {
76  gttic(eliminateMultifrontal);
77  // Do elimination with given ordering
78  EliminationTreeType etree(asDerived(), *variableIndex, *ordering);
79  JunctionTreeType junctionTree(etree);
80  boost::shared_ptr<BayesTreeType> bayesTree;
81  boost::shared_ptr<FactorGraphType> factorGraph;
82  boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
83  // If any factors are remaining, the ordering was incomplete
84  if(!factorGraph->empty())
86  // Return the Bayes tree
87  return bayesTree;
88  }
89  else if(!variableIndex) {
90  // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
91  // for no variable index first so that it's always computed if we need to call COLAMD because
92  // no Ordering is provided.
93  VariableIndex computedVariableIndex(asDerived());
94  return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType);
95  }
96  else /*if(!ordering)*/ {
97  // If no Ordering provided, compute one and call this function again. We are guaranteed to
98  // have a VariableIndex already here because we computed one if needed in the previous 'else'
99  // block.
100  if (orderingType == Ordering::METIS) {
101  Ordering computedOrdering = Ordering::Metis(asDerived());
102  return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
103  } else {
104  Ordering computedOrdering = Ordering::Colamd(*variableIndex);
105  return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
106  }
107  }
108  }
109 
110  /* ************************************************************************* */
111  template<class FACTORGRAPH>
112  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
114  const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
115  {
116  if(variableIndex) {
117  gttic(eliminatePartialSequential);
118  // Do elimination
119  EliminationTreeType etree(asDerived(), *variableIndex, ordering);
120  return etree.eliminate(function);
121  } else {
122  // If no variable index is provided, compute one and call this function again
123  VariableIndex computedVariableIndex(asDerived());
124  return eliminatePartialSequential(ordering, function, computedVariableIndex);
125  }
126  }
127 
128  /* ************************************************************************* */
129  template<class FACTORGRAPH>
130  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
132  const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
133  {
134  if(variableIndex) {
135  gttic(eliminatePartialSequential);
136  // Compute full ordering
137  Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
138 
139  // Split off the part of the ordering for the variables being eliminated
140  Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
141  return eliminatePartialSequential(ordering, function, variableIndex);
142  } else {
143  // If no variable index is provided, compute one and call this function again
144  VariableIndex computedVariableIndex(asDerived());
145  return eliminatePartialSequential(variables, function, computedVariableIndex);
146  }
147  }
148 
149  /* ************************************************************************* */
150  template<class FACTORGRAPH>
151  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
153  const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
154  {
155  if(variableIndex) {
156  gttic(eliminatePartialMultifrontal);
157  // Do elimination
158  EliminationTreeType etree(asDerived(), *variableIndex, ordering);
159  JunctionTreeType junctionTree(etree);
160  return junctionTree.eliminate(function);
161  } else {
162  // If no variable index is provided, compute one and call this function again
163  VariableIndex computedVariableIndex(asDerived());
164  return eliminatePartialMultifrontal(ordering, function, computedVariableIndex);
165  }
166  }
167 
168  /* ************************************************************************* */
169  template<class FACTORGRAPH>
170  std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
172  const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
173  {
174  if(variableIndex) {
175  gttic(eliminatePartialMultifrontal);
176  // Compute full ordering
177  Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
178 
179  // Split off the part of the ordering for the variables being eliminated
180  Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
181  return eliminatePartialMultifrontal(ordering, function, variableIndex);
182  } else {
183  // If no variable index is provided, compute one and call this function again
184  VariableIndex computedVariableIndex(asDerived());
185  return eliminatePartialMultifrontal(variables, function, computedVariableIndex);
186  }
187  }
188 
189  /* ************************************************************************* */
190  template<class FACTORGRAPH>
191  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
193  boost::variant<const Ordering&, const KeyVector&> variables,
194  OptionalOrdering marginalizedVariableOrdering,
195  const Eliminate& function, OptionalVariableIndex variableIndex) const
196  {
197  if(variableIndex)
198  {
199  if(marginalizedVariableOrdering)
200  {
201  gttic(marginalMultifrontalBayesNet);
202  // An ordering was provided for the marginalized variables, so we can first eliminate them
203  // in the order requested.
204  boost::shared_ptr<BayesTreeType> bayesTree;
205  boost::shared_ptr<FactorGraphType> factorGraph;
206  boost::tie(bayesTree,factorGraph) =
207  eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
208 
209  if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
210  {
211  // An ordering was also provided for the unmarginalized variables, so we can also
212  // eliminate them in the order requested.
213  return factorGraph->eliminateSequential(*varsAsOrdering, function);
214  }
215  else
216  {
217  // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
218  return factorGraph->eliminateSequential(boost::none, function);
219  }
220  }
221  else
222  {
223  // No ordering was provided for the marginalized variables, so order them using constrained
224  // COLAMD.
225  bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
226  const KeyVector* variablesOrOrdering =
227  unmarginalizedAreOrdered ?
228  boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
229 
230  Ordering totalOrdering =
231  Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
232 
233  // Split up ordering
234  const size_t nVars = variablesOrOrdering->size();
235  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
236  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
237 
238  // Call this function again with the computed orderings
239  return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
240  }
241  } else {
242  // If no variable index is provided, compute one and call this function again
243  VariableIndex index(asDerived());
244  return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
245  }
246  }
247 
248  /* ************************************************************************* */
249  template<class FACTORGRAPH>
250  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
252  boost::variant<const Ordering&, const KeyVector&> variables,
253  OptionalOrdering marginalizedVariableOrdering,
254  const Eliminate& function, OptionalVariableIndex variableIndex) const
255  {
256  if(variableIndex)
257  {
258  if(marginalizedVariableOrdering)
259  {
260  gttic(marginalMultifrontalBayesTree);
261  // An ordering was provided for the marginalized variables, so we can first eliminate them
262  // in the order requested.
263  boost::shared_ptr<BayesTreeType> bayesTree;
264  boost::shared_ptr<FactorGraphType> factorGraph;
265  boost::tie(bayesTree,factorGraph) =
266  eliminatePartialMultifrontal(*marginalizedVariableOrdering, function, *variableIndex);
267 
268  if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
269  {
270  // An ordering was also provided for the unmarginalized variables, so we can also
271  // eliminate them in the order requested.
272  return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
273  }
274  else
275  {
276  // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
277  return factorGraph->eliminateMultifrontal(boost::none, function);
278  }
279  }
280  else
281  {
282  // No ordering was provided for the marginalized variables, so order them using constrained
283  // COLAMD.
284  bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
285  const KeyVector* variablesOrOrdering =
286  unmarginalizedAreOrdered ?
287  boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
288 
289  Ordering totalOrdering =
290  Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
291 
292  // Split up ordering
293  const size_t nVars = variablesOrOrdering->size();
294  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
295  Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
296 
297  // Call this function again with the computed orderings
298  return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
299  }
300  } else {
301  // If no variable index is provided, compute one and call this function again
302  VariableIndex computedVariableIndex(asDerived());
303  return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
304  }
305  }
306 
307  /* ************************************************************************* */
308  template<class FACTORGRAPH>
309  boost::shared_ptr<FACTORGRAPH>
311  const KeyVector& variables,
312  const Eliminate& function, OptionalVariableIndex variableIndex) const
313  {
314  if(variableIndex)
315  {
316  // Compute a total ordering for all variables
317  Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
318 
319  // Split out the part for the marginalized variables
320  Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
321 
322  // Eliminate and return the remaining factor graph
323  return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
324  }
325  else
326  {
327  // If no variable index is provided, compute one and call this function again
328  VariableIndex computedVariableIndex(asDerived());
329  return marginal(variables, function, computedVariableIndex);
330  }
331  }
332 
333 
334 }
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of some variables, in ordering provided, to produce a Bayes tree and a re...
Definition: EliminateableFactorGraph-inst.h:152
boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrdering ordering=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none, OptionalOrderingType orderingType=boost::none) const
Do sequential elimination of all variables to produce a Bayes net.
Definition: EliminateableFactorGraph-inst.h:30
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
Exceptions that may be thrown by inference algorithms.
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition: Ordering.h:82
boost::optional< const Ordering & > OptionalOrdering
Typedef for an optional ordering as an argument to elimination functions.
Definition: EliminateableFactorGraph.h:92
boost::optional< Ordering::OrderingType > OptionalOrderingType
Typedef for an optional ordering type.
Definition: EliminateableFactorGraph.h:98
EliminationTraitsType::JunctionTreeType JunctionTreeType
Junction tree type that can do multifrontal elimination of this graph.
Definition: EliminateableFactorGraph.h:82
boost::optional< const VariableIndex & > OptionalVariableIndex
Typedef for an optional variable index as an argument to elimination functions.
Definition: EliminateableFactorGraph.h:95
EliminationTraitsType::EliminationTreeType EliminationTreeType
Elimination tree type that can do sequential elimination of this graph.
Definition: EliminateableFactorGraph.h:76
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition: EliminateableFactorGraph-inst.h:113
boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost::variant< const Ordering &, const KeyVector & > variables, OptionalOrdering marginalizedVariableOrdering=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes tree.
Definition: EliminateableFactorGraph-inst.h:251
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:128
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition: Ordering.cpp:212
An inference algorithm was called with inconsistent arguments.
Definition: inferenceExceptions.h:29
Variable elimination algorithms for factor graphs.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition: Ordering.h:101
boost::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:89
Definition: Ordering.h:34
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, OptionalOrdering marginalizedVariableOrdering=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition: EliminateableFactorGraph-inst.h:192
boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal factor graph of the requested variables.
Definition: EliminateableFactorGraph-inst.h:310
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrdering ordering=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none, OptionalOrderingType orderingType=boost::none) const
Do multifrontal elimination of all variables to produce a Bayes tree.
Definition: EliminateableFactorGraph-inst.h:71