New solver interface with all the files this time :-)
Richard Roberts
parent
5c68a07738
commit
2650939bb7
|
|
@ -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];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -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];
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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)));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue