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

Multifrontal QR using new solver interface

release/4.3a0
Richard Roberts 2010-10-22 00:06:54 +00:00
parent 31a080e4bf
commit 7c40fe32cf
9 changed files with 141 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
inference/BayesTree-inl.h
View File

@ -35,7 +35,7 @@ namespace lam = boost::lambda;
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {

1
inference/BayesTree.h
View File

@ -43,6 +43,7 @@ namespace gtsam {
public:
typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;

65
inference/GenericMultifrontalSolver-inl.h
View File

@ -19,66 +19,19 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
structure_(factorGraph),
eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
factors_.push_back(factorGraph);
GenericMultifrontalSolver<FACTOR>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
junctionTree_(new JunctionTree<FactorGraph<FACTOR> >(factorGraph)) {}
/* ************************************************************************* */
template<class FACTOR>
typename BayesTree<typename FACTOR::Conditional>::shared_ptr GenericMultifrontalSolver<FACTOR>::eliminate() const {
return junctionTree_->eliminate();
}
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::Conditional>::shared_ptr GenericSequentialSolver<FACTOR>::eliminate() const {
return eliminationTree_->eliminate();
}
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::joint(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 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);
}
// Eliminate all variables
typename BayesNet<typename FACTOR::Conditional>::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);
}
// 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::Conditional>::const_reverse_iterator conditional = bayesNet->rbegin();
for(size_t i = 0; i < js.size(); ++i) {
joint->push_back(typename FACTOR::shared_ptr(new FACTOR(**(conditional++)))); }
// 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>::marginal(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->joint(js))[0];
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR>::marginal(Index j) const {
return eliminate()->marginal(j);
}
}

10
inference/GenericMultifrontalSolver.h
View File

@ -20,14 +20,8 @@ class GenericMultifrontalSolver {
protected:
// Store the original factors for computing marginals
FactorGraph<FACTOR> factors_;
// Column structure of the factor graph
VariableIndex<> structure_;
// Elimination tree that performs elimination.
typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr eliminationTree_;
typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr junctionTree_;
public:
@ -41,7 +35,7 @@ public:
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesNet<typename FACTOR::Conditional>::shared_ptr eliminate() const;
typename BayesTree<typename FACTOR::Conditional>::shared_ptr eliminate() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out

3
linear/GaussianJunctionTree.h
View File

@ -23,6 +23,8 @@
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
/* ************************************************************************* */
@ -31,6 +33,7 @@ namespace gtsam {
*/
class GaussianJunctionTree: public JunctionTree<GaussianFactorGraph> {
public:
typedef boost::shared_ptr<GaussianJunctionTree> shared_ptr;
typedef JunctionTree<GaussianFactorGraph> Base;
typedef Base::sharedClique sharedClique;

35
linear/GaussianMultifrontalSolver.cpp Normal file
View File

@ -0,0 +1,35 @@
/**
* @file GaussianMultifrontalSolver.cpp
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/GenericMultifrontalSolver-inl.h>
namespace gtsam {
/* ************************************************************************* */
GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) :
junctionTree_(new GaussianJunctionTree(factorGraph)) {}
/* ************************************************************************* */
typename BayesTree<GaussianConditional>::sharedClique GaussianMultifrontalSolver::eliminate() const {
return junctionTree_->eliminate();
}
/* ************************************************************************* */
VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize()));
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
BayesTree<GaussianConditional> bayesTree;
bayesTree.insert(junctionTree_->eliminate());
return bayesTree.marginal(j);
}
}

89
linear/GaussianMultifrontalSolver.h Normal file
View File

@ -0,0 +1,89 @@
/**
* @file GaussianMultifrontalSolver.h
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <utility>
#include <vector>
namespace gtsam {
/** This solver uses sequential variable elimination to solve a
* GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a
* column elimination tree (inference/EliminationTree), 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'. A fill-reducing
* ordering is computed symbolically from the NonlinearFactorGraph, on the
* nonlinear side of gtsam. (To be precise, it is possible to permute an
* existing GaussianFactorGraph into a COLAMD ordering instead, this is done
* when computing marginals).
*
* This is not the most efficient algorithm we provide, most efficient is the
* MultifrontalSolver, which performs Multi-frontal QR factorization. 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.
*
* The EliminationTree recursively produces a BayesNet<GaussianFactor>,
* typedef'ed in linear/GaussianBayesNet, on which this class calls
* optimize(...) to perform back-substitution.
*/
class GaussianMultifrontalSolver {
protected:
GaussianJunctionTree::shared_ptr junctionTree_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesTree<GaussianConditional>::sharedClique eliminate() const;
/**
* Compute the least-squares solution of the GaussianFactorGraph. This
* eliminates to create a BayesNet and then back-substitutes this BayesNet to
* obtain the solution.
*/
VectorValues::shared_ptr optimize() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as an upper-
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
*/
GaussianFactor::shared_ptr marginal(Index j) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a mean
* vector and covariance matrix. Compared to marginalCanonical, which
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
// std::pair<Vector, Matrix> marginalStandard(Index j) const;
};
}

7
linear/GaussianSequentialSolver.h
View File

@ -8,8 +8,6 @@
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
@ -19,11 +17,6 @@
namespace gtsam {
/** A GaussianEliminationTree is just a typedef of the template EliminationTree */
typedef EliminationTree<GaussianFactor> GaussianEliminationTree;
/** This solver uses sequential variable elimination to solve a
* GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a
* column elimination tree (inference/EliminationTree), see Gilbert 2001 BIT.

2
linear/Makefile.am
View File

@ -22,7 +22,7 @@ check_PROGRAMS += tests/testVectorValues
#check_PROGRAMS += tests/testVectorMap tests/testVectorBTree
# Solvers
sources += GaussianSequentialSolver.cpp
sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp
# Gaussian Factor Graphs
headers += GaussianFactorSet.h Factorization.h