diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index dbeb7769b..bd391bf46 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -27,83 +27,93 @@ namespace gtsam { -/* ************************************************************************* */ -template -GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) : - factors_(new FactorGraph(factorGraph)), structure_(new VariableIndex(factorGraph)), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) {} + /* ************************************************************************* */ + template + GenericSequentialSolver::GenericSequentialSolver(// + const FactorGraph& factorGraph) : + factors_(new FactorGraph (factorGraph)), structure_( + new VariableIndex(factorGraph)), eliminationTree_(EliminationTree< + FACTOR>::Create(*factors_, *structure_)) { + } -/* ************************************************************************* */ -template -GenericSequentialSolver::GenericSequentialSolver(const typename FactorGraph::shared_ptr& factorGraph, - const VariableIndex::shared_ptr& variableIndex) : - factors_(factorGraph), structure_(variableIndex), - eliminationTree_(EliminationTree::Create(*factors_, *structure_)) {} + /* ************************************************************************* */ + template + GenericSequentialSolver::GenericSequentialSolver( + const typename FactorGraph::shared_ptr& factorGraph, + const VariableIndex::shared_ptr& variableIndex) : + factors_(factorGraph), structure_(variableIndex), eliminationTree_( + EliminationTree::Create(*factors_, *structure_)) { + } -/* ************************************************************************* */ -template -void GenericSequentialSolver::replaceFactors(const typename FactorGraph::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(); - factors_ = factorGraph; - eliminationTree_ = EliminationTree::Create(*factors_, *structure_); -} + /* ************************************************************************* */ + template + void GenericSequentialSolver::replaceFactors( + const typename FactorGraph::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(); + factors_ = factorGraph; + eliminationTree_ = EliminationTree::Create(*factors_, *structure_); + } -/* ************************************************************************* */ -template -typename BayesNet::shared_ptr GenericSequentialSolver::eliminate() const { - return eliminationTree_->eliminate(); -} + /* ************************************************************************* */ + template + typename BayesNet::shared_ptr // + GenericSequentialSolver::eliminate() const { + return eliminationTree_->eliminate(); + } -/* ************************************************************************* */ -template -typename FactorGraph::shared_ptr GenericSequentialSolver::jointFactorGraph(const std::vector& js) const { + /* ************************************************************************* */ + template + typename FactorGraph::shared_ptr // + GenericSequentialSolver::jointFactorGraph( + const std::vector& 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 permutationInverse(permutation->inverse()); + // Compute a COLAMD permutation with the marginal variable constrained to the end. + 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); - } + // 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); - // Eliminate all variables - typename BayesNet::shared_ptr bayesNet( - EliminationTree::Create(*factors_)->eliminate()); + // Eliminate all variables + typename BayesNet::shared_ptr bayesNet( + EliminationTree::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); - } + // 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); - // Take the joint marginal from the Bayes net. - typename FactorGraph::shared_ptr joint(new FactorGraph); - joint->reserve(js.size()); - typename BayesNet::const_reverse_iterator conditional = bayesNet->rbegin(); - for(size_t i = 0; i < js.size(); ++i) { - joint->push_back((*(conditional++))->toFactor()); } + // Take the joint marginal from the Bayes net. + typename FactorGraph::shared_ptr joint(new FactorGraph ); + joint->reserve(js.size()); + typename BayesNet::const_reverse_iterator + conditional = bayesNet->rbegin(); - // Undo the permutation on the eliminated joint marginal factors - BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) { - factor->permuteWithInverse(*permutation); } + for (size_t i = 0; i < js.size(); ++i) + joint->push_back((*(conditional++))->toFactor()); - return joint; -} + // Undo the permutation on the eliminated joint marginal factors + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) + factor->permuteWithInverse(*permutation); -/* ************************************************************************* */ -template -typename FACTOR::shared_ptr GenericSequentialSolver::marginalFactor(Index j) const { - // Create a container for the one variable index - vector js(1); js[0] = j; + return joint; + } - // Call joint and return the only factor in the factor graph it returns - return (*this->jointFactorGraph(js))[0]; -} + /* ************************************************************************* */ + template + typename FACTOR::shared_ptr // + GenericSequentialSolver::marginalFactor(Index j) const { + // Create a container for the one variable index + vector 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