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