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

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

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