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

New solver interface with all the files this time :-)

release/4.3a0
Richard Roberts 2010-10-21 23:04:42 +00:00
parent 5c68a07738
commit 2650939bb7
8 changed files with 534 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
inference/GenericMultifrontalSolver-inl.h Normal file
View File

@ -0,0 +1,86 @@
/**
* @file GenericMultifrontalSolver-inl.h
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericMultifrontalSolver.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/inference-inl.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
structure_(factorGraph),
eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
factors_.push_back(factorGraph);
}
/* ************************************************************************* */
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];
}
}

56
inference/GenericMultifrontalSolver.h Normal file
View File

@ -0,0 +1,56 @@
/**
* @file GenericMultifrontalSolver.h
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
#include <utility>
namespace gtsam {
template<class FACTOR>
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_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesNet<typename FACTOR::Conditional>::shared_ptr eliminate() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename FACTOR::shared_ptr marginal(Index j) const;
};
}

84
inference/GenericSequentialSolver-inl.h Normal file
View File

@ -0,0 +1,84 @@
/**
* @file GenericSequentialSolver.cpp
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/inference-inl.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
structure_(factorGraph),
eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
factors_.push_back(factorGraph);
}
/* ************************************************************************* */
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];
}
}

62
inference/GenericSequentialSolver.h Normal file
View File

@ -0,0 +1,62 @@
/**
* @file GenericSequentialSolver.h
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
#include <utility>
namespace gtsam {
template<class FACTOR>
class GenericSequentialSolver {
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 EliminationTree<FACTOR>::shared_ptr eliminationTree_;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesNet<typename FACTOR::Conditional>::shared_ptr eliminate() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename FACTOR::shared_ptr marginal(Index j) const;
/**
* 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.
*/
typename FactorGraph<FACTOR>::shared_ptr joint(const std::vector<Index>& js) const;
};
}

28
inference/SymbolicSequentialSolver.cpp Normal file
View File

@ -0,0 +1,28 @@
/**
* @file SymbolicSequentialSolver.cpp
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {
/* ************************************************************************* */
SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) :
Base(factorGraph) {}
/* ************************************************************************* */
typename BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const {
return Base::eliminate();
}
/* ************************************************************************* */
SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const {
return SymbolicFactorGraph::shared_ptr(new SymbolicFactorGraph(*Base::joint(js)));
}
}

48
inference/SymbolicSequentialSolver.h Normal file
View File

@ -0,0 +1,48 @@
/**
* @file SymbolicSequentialSolver.h
* @brief
* @author Richard Roberts
* @created Oct 21, 2010
*/
#pragma once
#include <gtsam/inference/GenericSequentialSolver.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
namespace gtsam {
class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
protected:
typedef GenericSequentialSolver<IndexFactor> Base;
public:
SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename BayesNet<IndexConditional>::shared_ptr eliminate() const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
IndexFactor::shared_ptr marginal(Index j) const;
/**
* Compute the marginal joint over a set of variables, 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 GaussianBayesNet with
* R*x = d. To get a mean and covariance matrix, use jointStandard(...)
*/
SymbolicFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
};
}

57
linear/GaussianSequentialSolver.cpp Normal file
View File

@ -0,0 +1,57 @@
/**
* @file SequentialSolver.cpp
* @brief
* @author Richard Roberts
* @created Oct 19, 2010
*/
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
namespace gtsam {
/* ************************************************************************* */
GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph) :
Base(factorGraph) {}
/* ************************************************************************* */
GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const {
return Base::eliminate();
}
/* ************************************************************************* */
VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
static const bool debug = false;
if(debug) this->factors_.print("GaussianSequentialSolver, eliminating ");
if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree ");
// Eliminate using the elimination tree
GaussianBayesNet::shared_ptr bayesNet(this->eliminate());
if(debug) bayesNet->print("GaussianSequentialSolver, Bayes net ");
// Allocate the solution vector if it is not already allocated
// VectorValues::shared_ptr solution = allocateVectorValues(*bayesNet);
// Back-substitute
VectorValues::shared_ptr solution(gtsam::optimize_(*bayesNet));
if(debug) solution->print("GaussianSequentialSolver, solution ");
return solution;
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const {
return Base::marginal(j);
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js)));
}
}

113
linear/GaussianSequentialSolver.h Normal file
View File

@ -0,0 +1,113 @@
/**
* @file SequentialSolver.h
* @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination.
* @author Richard Roberts
* @created Oct 19, 2010
*/
#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>
#include <utility>
#include <vector>
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.
*
* 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 GaussianSequentialSolver : GenericSequentialSolver<GaussianFactor> {
protected:
typedef GenericSequentialSolver<GaussianFactor> Base;
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the symbolic work of elimination.
*/
GaussianSequentialSolver(const FactorGraph<GaussianFactor>& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
GaussianBayesNet::shared_ptr 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;
/**
* Compute the marginal joint over a set of variables, 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 GaussianBayesNet with
* R*x = d. To get a mean and covariance matrix, use jointStandard(...)
*/
GaussianFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a mean
* vector and covariance matrix. The variables will be ordered in the
* return values as they are ordered in the 'js' argument, not as they are
* ordered in the original factor graph. Compared to jointCanonical, which
* returns a GaussianBayesNet, this function back-substitutes the BayesNet to
* obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
// std::pair<Vector, Matrix> jointStandard(const std::vector<Index>& js) const;
};
}