diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h index c5fcd67ee..836168d15 100644 --- a/gtsam/inference/GenericSequentialSolver-inl.h +++ b/gtsam/inference/GenericSequentialSolver-inl.h @@ -32,18 +32,18 @@ namespace gtsam { template GenericSequentialSolver::GenericSequentialSolver( const FactorGraph& factorGraph) : - factors_(new FactorGraph (factorGraph)), structure_( - new VariableIndex(factorGraph)), eliminationTree_(EliminationTree< - FACTOR>::Create(*factors_, *structure_)) { + factors_(new FactorGraph(factorGraph)), + structure_(new VariableIndex(factorGraph)), + eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { } /* ************************************************************************* */ template GenericSequentialSolver::GenericSequentialSolver( - const typename FactorGraph::shared_ptr& factorGraph, + const sharedFactorGraph& factorGraph, const VariableIndex::shared_ptr& variableIndex) : - factors_(factorGraph), structure_(variableIndex), eliminationTree_( - EliminationTree::Create(*factors_, *structure_)) { + factors_(factorGraph), structure_(variableIndex), + eliminationTree_(EliminationTree::Create(*factors_, *structure_)) { } /* ************************************************************************* */ @@ -67,7 +67,7 @@ namespace gtsam { /* ************************************************************************* */ template void GenericSequentialSolver::replaceFactors( - const typename FactorGraph::shared_ptr& factorGraph) { + const sharedFactorGraph& 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(); @@ -91,8 +91,7 @@ namespace gtsam { typename EliminationTree::Eliminate function) 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 @@ -102,15 +101,15 @@ namespace gtsam { if (factor) factor->permuteWithInverse(*permutationInverse); // Eliminate all variables - typename BayesNet::shared_ptr bayesNet( - EliminationTree::Create(*factors_)->eliminate(function)); + typename BayesNet::shared_ptr + bayesNet(EliminationTree::Create(*factors_)->eliminate(function)); // 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); + if (factor) factor->permuteWithInverse(*permutation); // Take the joint marginal from the Bayes net. - typename FactorGraph::shared_ptr joint(new FactorGraph ); + sharedFactorGraph joint(new FactorGraph ); joint->reserve(js.size()); typename BayesNet::const_reverse_iterator conditional = bayesNet->rbegin(); diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h index 52da0a96d..0e6ad6792 100644 --- a/gtsam/inference/GenericSequentialSolver.h +++ b/gtsam/inference/GenericSequentialSolver.h @@ -24,19 +24,36 @@ namespace gtsam { + /** + * This solver implements sequential variable elimination for factor graphs. + * Underlying this is a column elimination tree, see Gilbert 2001 BIT. + * + * The elimination ordering is "baked in" to the variable indices at this + * stage, i.e. elimination proceeds in order from '0'. + * + * This is not the most efficient algorithm we provide, most efficient is the + * MultifrontalSolver, which examines and uses the clique structure. + * However, sequential variable elimination is easier to understand so this is a good + * starting point to learn about these algorithms and our implementation. + * Additionally, the first step of MFQR is symbolic sequential elimination. + */ template class GenericSequentialSolver: public Testable< GenericSequentialSolver > { protected: - // Store the original factors for computing marginals - typename FactorGraph::shared_ptr factors_; + typedef typename FactorGraph::shared_ptr sharedFactorGraph; - // Column structure of the factor graph + /** Store the original factors for computing marginals + * TODO Frank says: really? Marginals should be computed from result. + */ + sharedFactorGraph factors_; + + /** Store column structure of the factor graph. Why? */ VariableIndex::shared_ptr structure_; - // Elimination tree that performs elimination. + /** Elimination tree that performs elimination */ typename EliminationTree::shared_ptr eliminationTree_; public: @@ -53,7 +70,7 @@ namespace gtsam { * is the fastest. */ GenericSequentialSolver( - const typename FactorGraph::shared_ptr& factorGraph, + const sharedFactorGraph& factorGraph, const VariableIndex::shared_ptr& variableIndex); /** Print to cout */ @@ -67,8 +84,7 @@ namespace gtsam { * This function can be used if the numerical part of the factors changes, * such as during relinearization or adjusting of noise models. */ - void replaceFactors( - const typename FactorGraph::shared_ptr& factorGraph); + void replaceFactors(const sharedFactorGraph& factorGraph); /** * Eliminate the factor graph sequentially. Uses a column elimination tree @@ -79,8 +95,7 @@ namespace gtsam { /** * Compute the marginal joint over a set of variables, by integrating out - * all of the other variables. This function returns the result as a factor - * graph. + * all of the other variables. Returns the result as a factor graph. */ typename FactorGraph::shared_ptr jointFactorGraph( const std::vector& js,