conditionalBayesNet and an internal eliminate - developed for making shortcuts

release/4.3a0
Frank Dellaert 2012-09-16 17:52:14 +00:00
parent db57f1872a
commit 338ea6e920
3 changed files with 259 additions and 169 deletions

View File

@ -28,92 +28,142 @@
namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const FactorGraph<FACTOR>& factorGraph) :
factors_(new FactorGraph<FACTOR>(factorGraph)),
structure_(new VariableIndex(factorGraph)),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex) :
factors_(factorGraph), structure_(variableIndex),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::print(const std::string& s) const {
this->factors_->print(s + " factors:");
this->structure_->print(s + " structure:\n");
this->eliminationTree_->print(s + " etree:");
}
/* ************************************************************************* */
template<class FACTOR>
bool GenericSequentialSolver<FACTOR>::equals(
const GenericSequentialSolver& expected, double tol) const {
if (!this->factors_->equals(*expected.factors_, tol)) return false;
if (!this->structure_->equals(*expected.structure_, tol)) return false;
if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) return false;
return true;
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::replaceFactors(
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();
factors_ = factorGraph;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > //
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
return eliminationTree_->eliminate(function);
}
/* ************************************************************************* */
template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const FactorGraph<FACTOR>& factorGraph) :
factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(
new VariableIndex(factorGraph)), eliminationTree_(
EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointBayesNet(
const std::vector<Index>& js, Eliminate function) const {
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex) :
factors_(factorGraph), structure_(variableIndex), eliminationTree_(
EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
// Compute a COLAMD permutation with the marginal variables constrained to the end.
Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
Permutation::shared_ptr permutationInverse(permutation->inverse());
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::print(const std::string& s) const {
this->factors_->print(s + " factors:");
this->structure_->print(s + " structure:\n");
this->eliminationTree_->print(s + " etree:");
}
/* ************************************************************************* */
template<class FACTOR>
bool GenericSequentialSolver<FACTOR>::equals(
const GenericSequentialSolver& expected, double tol) const {
if (!this->factors_->equals(*expected.factors_, tol))
return false;
if (!this->structure_->equals(*expected.structure_, tol))
return false;
if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol))
return false;
return true;
}
/* ************************************************************************* */
template<class FACTOR>
void GenericSequentialSolver<FACTOR>::replaceFactors(
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();
factors_ = factorGraph;
eliminationTree_ = EliminationTree<FACTOR>::Create(*factors_, *structure_);
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
return eliminationTree_->eliminate(function);
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
Eliminate function, boost::optional<size_t> nrToEliminate) const {
// Create inverse permutation
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 boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutationInverse);
if (factor)
factor->permuteWithInverse(*permutationInverse);
// Eliminate all variables
typename BayesNet<Conditional>::shared_ptr
bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
// Eliminate using elimination tree provided
typename EliminationTree<FACTOR>::shared_ptr etree;
if (nrToEliminate) {
VariableIndex structure(*factors_, *nrToEliminate);
etree = EliminationTree<FACTOR>::Create(*factors_, structure);
} else
etree = EliminationTree<FACTOR>::Create(*factors_);
sharedBayesNet bayesNet = etree->eliminate(function);
// Undo the permutation on the original factors and on the structure.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutation);
// Get rid of conditionals on variables that we want to marginalize out
size_t nrMarginalizedOut = bayesNet->size()-js.size();
for(int i=0;i<nrMarginalizedOut;i++)
bayesNet->pop_front();
if (factor)
factor->permuteWithInverse(permutation);
// Undo the permutation on the conditionals
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
c->permuteWithInverse(*permutation);
c->permuteWithInverse(permutation);
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::conditionalBayesNet(
const std::vector<Index>& js, size_t nrFrontals,
Eliminate function) const {
// Compute a COLAMD permutation with the marginal variables constrained to the end.
// TODO in case of nrFrontals, the order of js has to be respected here !
Permutation::shared_ptr permutation(
inference::PermutationCOLAMD(*structure_, js));
// Eliminate only variables J \cup F from P(J,F,S) to get P(F|S)
size_t nrVariables = factors_->keys().size(); // TODO expensive!
size_t nrMarginalized = nrVariables - js.size();
size_t nrToEliminate = nrMarginalized + nrFrontals;
sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate);
// Get rid of conditionals on variables that we want to marginalize out
for (int i = 0; i < nrMarginalized; i++)
bayesNet->pop_front();
return bayesNet;
}
/* ************************************************************************* */
template<class FACTOR>
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::jointBayesNet(const std::vector<Index>& js,
Eliminate function) const {
// Compute a COLAMD permutation with the marginal variables constrained to the end.
Permutation::shared_ptr permutation(
inference::PermutationCOLAMD(*structure_, js));
// Eliminate all variables
sharedBayesNet bayesNet = eliminate(*permutation, function);
// Get rid of conditionals on variables that we want to marginalize out
size_t nrMarginalizedOut = bayesNet->size() - js.size();
for (int i = 0; i < nrMarginalizedOut; i++)
bayesNet->pop_front();
return bayesNet;
}
@ -125,22 +175,23 @@ namespace gtsam {
const std::vector<Index>& js, Eliminate function) const {
// Eliminate all variables
typename BayesNet<Conditional>::shared_ptr
bayesNet = jointBayesNet(js,function);
typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js,
function);
return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
}
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<FACTOR> //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
// Create a container for the one variable index
std::vector<Index> js(1);
js[0] = j;
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<FACTOR> //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
Eliminate function) const {
// Create a container for the one variable index
std::vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns
return (*this->jointFactorGraph(js, function))[0];
}
// Call joint and return the only factor in the factor graph it returns
return (*this->jointFactorGraph(js, function))[0];
}
} // namespace gtsam

View File

@ -18,112 +18,141 @@
#pragma once
#include <utility>
#include <boost/function.hpp>
#include <vector>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
namespace gtsam { class VariableIndex; }
namespace gtsam { template<class FACTOR> class EliminationTree; }
namespace gtsam { template<class FACTOR> class FactorGraph; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; }
#include <boost/function.hpp>
#include <boost/optional.hpp>
#include <utility>
#include <vector>
namespace gtsam {
class VariableIndex;
class Permutation;
}
namespace gtsam {
template<class FACTOR> class EliminationTree;
}
namespace gtsam {
template<class FACTOR> class FactorGraph;
}
namespace gtsam {
template<class CONDITIONAL> class BayesNet;
}
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.
* \nosubgrouping
*/
template<class FACTOR>
class GenericSequentialSolver {
/**
* 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.
* \nosubgrouping
*/
template<class FACTOR>
class GenericSequentialSolver {
protected:
protected:
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional;
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional;
typedef typename boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet;
typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
typedef boost::function<
EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
/** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result.
*/
sharedFactorGraph factors_;
/** 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? */
boost::shared_ptr<VariableIndex> structure_;
/** Store column structure of the factor graph. Why? */
boost::shared_ptr<VariableIndex> structure_;
/** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
/** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
/** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
public:
/**
* Eliminate in a different order, given a permutation
* If given a number of variables to eliminate, will only eliminate that many
*/
sharedBayesNet
eliminate(const Permutation& permutation, Eliminate function,
boost::optional<size_t> nrToEliminate = boost::none) const;
/// @name Standard Constructors
/// @{
public:
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
/// @name Standard Constructors
/// @{
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
*/
GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
/// @}
/// @name Testable
/// @{
/**
* Construct the solver with a shared pointer to a factor graph and to a
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericSequentialSolver(const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/// @}
/// @name Testable
/// @{
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/// @}
/// @name Standard Interface
/// @{
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/**
* Replace the factor graph with a new one having the same structure. The
* 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 sharedFactorGraph& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename boost::shared_ptr<BayesNet<Conditional> >
eliminate(Eliminate function) const;
/**
* Replace the factor graph with a new one having the same structure. The
* 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 sharedFactorGraph& factorGraph);
/**
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
sharedBayesNet eliminate(Eliminate function) const;
/**
* Compute a conditional density P(F|S) while marginalizing out variables J
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
* Returns the result as a Bayes net.
*/
sharedBayesNet
conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals,
Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net
*/
typename BayesNet<Conditional>::shared_ptr
jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
sharedBayesNet
jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
@ -132,17 +161,19 @@ namespace gtsam {
typename FactorGraph<FACTOR>::shared_ptr
jointFactorGraph(const std::vector<Index>& js, Eliminate function) 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 boost::shared_ptr<FACTOR>
marginalFactor(Index j, Eliminate function) 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 boost::shared_ptr<FACTOR>
marginalFactor(Index j, Eliminate function) const;
/// @}
/// @}
}; // GenericSequentialSolver
}
;
// GenericSequentialSolver
} // namespace gtsam
}// namespace gtsam
#include <gtsam/inference/GenericSequentialSolver-inl.h>

View File

@ -55,6 +55,14 @@ namespace gtsam {
SymbolicBayesNet::shared_ptr eliminate() const
{ return Base::eliminate(&EliminateSymbolic); };
/**
* Compute a conditional density P(F|S) while marginalizing out variables J
* P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
* Returns the result as a Bayes net.
*/
SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const
{ return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net.