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

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

@ -32,9 +32,9 @@ 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_)) {
factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(
new VariableIndex(factorGraph)), eliminationTree_(
EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
@ -42,8 +42,8 @@ namespace gtsam {
GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex) :
factors_(factorGraph), structure_(variableIndex),
eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
factors_(factorGraph), structure_(variableIndex), eliminationTree_(
EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
}
/* ************************************************************************* */
@ -58,9 +58,12 @@ namespace gtsam {
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;
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;
}
@ -77,44 +80,91 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > //
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
return eliminationTree_->eliminate(function);
}
/* ************************************************************************* */
template<class FACTOR>
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
GenericSequentialSolver<FACTOR>::jointBayesNet(
const std::vector<Index>& js, Eliminate function) const {
typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
Eliminate function, boost::optional<size_t> nrToEliminate) const {
// 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());
// 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);
if (factor)
factor->permuteWithInverse(permutation);
// Undo the permutation on the conditionals
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
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();
// Undo the permutation on the conditionals
BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
c->permuteWithInverse(*permutation);
return bayesNet;
}
@ -125,8 +175,8 @@ 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);
}
@ -134,7 +184,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
typename boost::shared_ptr<FACTOR> //
GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
Eliminate function) const {
// Create a container for the one variable index
std::vector<Index> js(1);
js[0] = j;

59
gtsam/inference/GenericSequentialSolver.h
View File

@ -18,16 +18,28 @@
#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 {
@ -52,8 +64,10 @@ namespace gtsam {
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.
@ -70,6 +84,14 @@ namespace gtsam {
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
// GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
/**
* 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;
public:
/// @name Standard Constructors
@ -86,8 +108,7 @@ namespace gtsam {
* VariableIndex. The solver will store these pointers, so this constructor
* is the fastest.
*/
GenericSequentialSolver(
const sharedFactorGraph& factorGraph,
GenericSequentialSolver(const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
/// @}
@ -115,14 +136,22 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
typename boost::shared_ptr<BayesNet<Conditional> >
eliminate(Eliminate function) const;
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
sharedBayesNet
jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
/**
@ -141,7 +170,9 @@ namespace gtsam {
/// @}
}; // GenericSequentialSolver
}
;
// GenericSequentialSolver
}// namespace gtsam

8
gtsam/inference/SymbolicSequentialSolver.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.