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

Formatting only

release/4.3a0
Frank Dellaert 2011-03-17 19:34:00 +00:00
parent 3d2feb0406
commit 2c54218100
1 changed files with 76 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -29,20 +29,26 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(new VariableIndex(factorGraph)),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(//
const FactorGraph<FACTOR>& factorGraph) :
factors_(new FactorGraph<FACTOR> (factorGraph)), structure_(
new VariableIndex(factorGraph)), eliminationTree_(EliminationTree<
FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph,
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const typename FactorGraph<FACTOR>::shared_ptr& factorGraph,
const VariableIndex::shared_ptr& variableIndex) :
factors_(factorGraph), structure_(variableIndex),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {}
factors_(factorGraph), structure_(variableIndex), eliminationTree_(
EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::replaceFactors(const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
void GenericSequentialSolver<FACTOR>::replaceFactors(
const typename FactorGraph<FACTOR>::shared_ptr& factorGraph) {
// Reset this shared pointer first to deallocate if possible - for big
// problems there may not be enough memory to store two copies.
eliminationTree_.reset();
@ -52,58 +58,62 @@ void GenericSequentialSolver<FACTOR>::replaceFactors(const typename FactorGraph<
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr GenericSequentialSolver<FACTOR>::eliminate() const {
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
GenericSequentialSolver<FACTOR>::eliminate() const {
return eliminationTree_->eliminate();
}
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::jointFactorGraph(const std::vector<Index>& js) const {
typename FactorGraph<FACTOR>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointFactorGraph(
const std::vector<Index>& js) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(*structure_, js));
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(
*structure_, js));
Permutation::shared_ptr permutationInverse(permutation->inverse());
// Permute the factors - NOTE that this permutes the original factors, not
// copies. Other parts of the code may hold shared_ptr's to these factors so
// we must undo the permutation before returning.
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
if(factor)
factor->permuteWithInverse(*permutationInverse);
}
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutationInverse);
// Eliminate all variables
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr bayesNet(
EliminationTree<FACTOR>::Create(*factors_)->eliminate());
// Undo the permuation on the original factors and on the structure.
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_) {
if(factor)
factor->permuteWithInverse(*permutation);
}
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutation);
// Take the joint marginal from the Bayes net.
typename FactorGraph<FACTOR>::shared_ptr joint(new FactorGraph<FACTOR> );
joint->reserve(js.size());
typename BayesNet<typename FACTOR::ConditionalType>::const_reverse_iterator conditional = bayesNet->rbegin();
for(size_t i = 0; i < js.size(); ++i) {
joint->push_back((*(conditional++))->toFactor()); }
typename BayesNet<typename FACTOR::ConditionalType>::const_reverse_iterator
conditional = bayesNet->rbegin();
for (size_t i = 0; i < js.size(); ++i)
joint->push_back((*(conditional++))->toFactor());
// Undo the permutation on the eliminated joint marginal factors
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) {
factor->permuteWithInverse(*permutation); }
BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint)
factor->permuteWithInverse(*permutation);
return joint;
}
/* ************************************************************************* */
template<class FACTOR>
typename FACTOR::shared_ptr GenericSequentialSolver<FACTOR>::marginalFactor(Index j) const {
typename FACTOR::shared_ptr //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j) const {
// Create a container for the one variable index
vector<Index> js(1); js[0] = j;
vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns
return (*this->jointFactorGraph(js))[0];
}
}
} // namespace gtsam