12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added EliminateableFactorGraph::marginal and eliminatePartialSequential with vector of keys instead of ordering

release/4.3a0
Richard Roberts 2013-08-06 16:10:11 +00:00
parent 972695da3a
commit 61fe49ab7e
2 changed files with 53 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -101,6 +101,26 @@ namespace gtsam {
} }
} }
/* ************************************************************************* */
template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(variableIndex) {
gttic(eliminatePartialSequential);
// Compute full ordering
Ordering fullOrdering = Ordering::COLAMDConstrainedFirst(*variableIndex, variables);
// Split off the part of the ordering for the variables being eliminated
Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
return eliminatePartialSequential(ordering, function, variableIndex);
} else {
// If no variable index is provided, compute one and call this function again
return eliminatePartialSequential(variables, function, VariableIndex(asDerived()));
}
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> > std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
@ -231,4 +251,31 @@ namespace gtsam {
} }
} }
/* ************************************************************************* */
template<class FACTORGRAPH>
boost::shared_ptr<FACTORGRAPH>
EliminateableFactorGraph<FACTORGRAPH>::marginal(
const std::vector<Key>& variables,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const
{
if(variableIndex)
{
// Compute a total ordering for all variables
Ordering totalOrdering = Ordering::COLAMDConstrainedLast(*variableIndex, variables);
// Split out the part for the marginalized variables
Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
// Eliminate and return the remaining factor graph
return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
}
else
{
// If no variable index is provided, compute one and call this function again
return marginal(variables, function, VariableIndex(asDerived()));
}
}
} }

6
gtsam/inference/EliminateableFactorGraph.h
View File

@ -217,6 +217,12 @@ namespace gtsam {
const Eliminate& function = EliminationTraits::DefaultEliminate, const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
/** Compute the marginal factor graph of the requested variables. */
boost::shared_ptr<FactorGraphType> marginal(
const std::vector<Key>& variables,
const Eliminate& function = EliminationTraits::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
private: private:
// Access the derived factor graph class // Access the derived factor graph class