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