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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

215
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -28,92 +28,142 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
GenericSequentialSolver<FACTOR>::GenericSequentialSolver( GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const FactorGraph<FACTOR>& factorGraph) : const FactorGraph<FACTOR>& factorGraph) :
factors_(new FactorGraph<FACTOR>(factorGraph)), factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(
structure_(new VariableIndex(factorGraph)), new VariableIndex(factorGraph)), eliminationTree_(
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) { 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> template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr // GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
GenericSequentialSolver<FACTOR>::jointBayesNet( const sharedFactorGraph& factorGraph,
const std::vector<Index>& js, Eliminate function) const { 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)); template<class FACTOR>
Permutation::shared_ptr permutationInverse(permutation->inverse()); 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 // 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 // copies. Other parts of the code may hold shared_ptr's to these factors so
// we must undo the permutation before returning. // we must undo the permutation before returning.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_) BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutationInverse); if (factor)
factor->permuteWithInverse(*permutationInverse);
// Eliminate all variables // Eliminate using elimination tree provided
typename BayesNet<Conditional>::shared_ptr typename EliminationTree<FACTOR>::shared_ptr etree;
bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function)); 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. // Undo the permutation on the original factors and on the structure.
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_) BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
if (factor) factor->permuteWithInverse(*permutation); 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();
// Undo the permutation on the conditionals // Undo the permutation on the conditionals
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet) 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; return bayesNet;
} }
@ -125,22 +175,23 @@ namespace gtsam {
const std::vector<Index>& js, Eliminate function) const { const std::vector<Index>& js, Eliminate function) const {
// Eliminate all variables // Eliminate all variables
typename BayesNet<Conditional>::shared_ptr typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js,
bayesNet = jointBayesNet(js,function); function);
return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet); return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
typename boost::shared_ptr<FACTOR> // typename boost::shared_ptr<FACTOR> //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const { GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
// Create a container for the one variable index Eliminate function) const {
std::vector<Index> js(1); // Create a container for the one variable index
js[0] = j; std::vector<Index> js(1);
js[0] = j;
// Call joint and return the only factor in the factor graph it returns // Call joint and return the only factor in the factor graph it returns
return (*this->jointFactorGraph(js, function))[0]; return (*this->jointFactorGraph(js, function))[0];
} }
} // namespace gtsam } // namespace gtsam

205
gtsam/inference/GenericSequentialSolver.h
View File

@ -18,112 +18,141 @@
#pragma once #pragma once
#include <utility>
#include <boost/function.hpp>
#include <vector>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
namespace gtsam { class VariableIndex; } #include <boost/function.hpp>
namespace gtsam { template<class FACTOR> class EliminationTree; } #include <boost/optional.hpp>
namespace gtsam { template<class FACTOR> class FactorGraph; }
namespace gtsam { template<class CONDITIONAL> class BayesNet; } #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 { namespace gtsam {
/** /**
* This solver implements sequential variable elimination for factor graphs. * This solver implements sequential variable elimination for factor graphs.
* Underlying this is a column elimination tree, see Gilbert 2001 BIT. * Underlying this is a column elimination tree, see Gilbert 2001 BIT.
* *
* The elimination ordering is "baked in" to the variable indices at this * The elimination ordering is "baked in" to the variable indices at this
* stage, i.e. elimination proceeds in order from '0'. * stage, i.e. elimination proceeds in order from '0'.
* *
* This is not the most efficient algorithm we provide, most efficient is the * This is not the most efficient algorithm we provide, most efficient is the
* MultifrontalSolver, which examines and uses the clique structure. * MultifrontalSolver, which examines and uses the clique structure.
* However, sequential variable elimination is easier to understand so this is a good * However, sequential variable elimination is easier to understand so this is a good
* starting point to learn about these algorithms and our implementation. * starting point to learn about these algorithms and our implementation.
* Additionally, the first step of MFQR is symbolic sequential elimination. * Additionally, the first step of MFQR is symbolic sequential elimination.
* \nosubgrouping * \nosubgrouping
*/ */
template<class FACTOR> template<class FACTOR>
class GenericSequentialSolver { class GenericSequentialSolver {
protected: protected:
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph; typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
typedef typename FACTOR::ConditionalType Conditional; 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 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 /** Store the original factors for computing marginals
* TODO Frank says: really? Marginals should be computed from result. * TODO Frank says: really? Marginals should be computed from result.
*/ */
sharedFactorGraph factors_; sharedFactorGraph factors_;
/** Store column structure of the factor graph. Why? */ /** Store column structure of the factor graph. Why? */
boost::shared_ptr<VariableIndex> structure_; boost::shared_ptr<VariableIndex> structure_;
/** Elimination tree that performs elimination */ /** Elimination tree that performs elimination */
boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_; boost::shared_ptr<EliminationTree<FACTOR> > eliminationTree_;
/** concept checks */ /** concept checks */
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree) // 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:
/// @{
/** /// @name Standard Constructors
* 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);
/** /**
* Construct the solver with a shared pointer to a factor graph and to a * Construct the solver for a factor graph. This builds the elimination
* VariableIndex. The solver will store these pointers, so this constructor * tree, which already does some of the work of elimination.
* is the fastest. */
*/ GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph);
GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
/// @} /**
/// @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 */ /** Print to cout */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const; void print(const std::string& name = "GenericSequentialSolver: ") const;
/// @} /** Test whether is equal to another */
/// @name Standard Interface bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/// @{
/** /// @}
* Replace the factor graph with a new one having the same structure. The /// @name Standard Interface
* 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 * Replace the factor graph with a new one having the same structure. The
* to recursively eliminate. * This function can be used if the numerical part of the factors changes,
*/ * such as during relinearization or adjusting of noise models.
typename boost::shared_ptr<BayesNet<Conditional> > */
eliminate(Eliminate function) const; 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 * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net * all of the other variables. Returns the result as a Bayes net
*/ */
typename BayesNet<Conditional>::shared_ptr sharedBayesNet
jointBayesNet(const std::vector<Index>& js, Eliminate function) const; jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
/** /**
* Compute the marginal joint over a set of variables, by integrating out * Compute the marginal joint over a set of variables, by integrating out
@ -132,17 +161,19 @@ namespace gtsam {
typename FactorGraph<FACTOR>::shared_ptr typename FactorGraph<FACTOR>::shared_ptr
jointFactorGraph(const std::vector<Index>& js, Eliminate function) const; jointFactorGraph(const std::vector<Index>& js, Eliminate function) const;
/** /**
* Compute the marginal Gaussian density over a variable, by integrating out * Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor. * all of the other variables. This function returns the result as a factor.
*/ */
typename boost::shared_ptr<FACTOR> typename boost::shared_ptr<FACTOR>
marginalFactor(Index j, Eliminate function) const; marginalFactor(Index j, Eliminate function) const;
/// @} /// @}
}; // GenericSequentialSolver }
;
// GenericSequentialSolver
} // namespace gtsam }// namespace gtsam
#include <gtsam/inference/GenericSequentialSolver-inl.h> #include <gtsam/inference/GenericSequentialSolver-inl.h>

8
gtsam/inference/SymbolicSequentialSolver.h
View File

@ -55,6 +55,14 @@ namespace gtsam {
SymbolicBayesNet::shared_ptr eliminate() const SymbolicBayesNet::shared_ptr eliminate() const
{ return Base::eliminate(&EliminateSymbolic); }; { 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 * Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a Bayes net. * all of the other variables. Returns the result as a Bayes net.