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

Adding elimination algorithms to factor graph eliminateable base class

release/4.3a0
Richard Roberts 2013-06-06 15:36:38 +00:00
parent 626d66bdf4
commit d11833317f
3 changed files with 199 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/inference/EliminateableFactorGraph-inst.h Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file EliminateableFactorGraph.h
* @brief Variable elimination algorithms for factor graphs
* @author Richard Roberts
* @date Apr 21, 2013
*/
#pragma once
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/inferenceExceptions.h>
namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
boost::shared_ptr<typename ELIMINATIONTREE::BayesNetType>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateSequential(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{
// Do elimination
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result;
if(ordering) {
// Do elimination with given ordering
result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function);
} else {
// Compute ordering
OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
result = EliminationTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
}
// If any factors are remaining, the ordering was incomplete
if(!result.second->empty())
throw InconsistentEliminationRequested();
// Return the Bayes net
return result.first;
}
/* ************************************************************************* */
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType>
EliminateableFactorGraph<DERIVED,ELIMINATIONTREE,JUNCTIONTREE>::eliminateMultifrontal(
const Eliminate& function, OptionalOrdering ordering, const VariableIndexUnordered& variableIndex) const
{
// Do elimination
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result;
if(ordering) {
// Do elimination with given ordering
result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function);
} else {
// Compute ordering
OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
result = JunctionTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
}
// If any factors are remaining, the ordering was incomplete
if(!result.second->empty())
throw InconsistentEliminationRequested();
// Return the Bayes tree
return result.first;
}
}

104
gtsam/inference/EliminateableFactorGraph.h
View File

@ -13,7 +13,7 @@
* @file EliminateableFactorGraph.h
* @brief Variable elimination algorithms for factor graphs
* @author Richard Roberts
* @date Oct 21, 2010
* @date Apr 21, 2013
*/
#pragma once
@ -22,21 +22,24 @@
#include <boost/function.hpp>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
namespace gtsam {
/** EliminateableFactorGraph is a base class for factor graphs that contains elimination
* algorithms. Any factor graph holding eliminateable factors can derive from this class to
* expose functions for computing marginals, conditional marginals, doing full multifrontal and
* expose functions for computing marginals, conditional marginals, doing multifrontal and
* sequential elimination, etc. */
template<class DERIVED, class FACTOR, class BAYESNET, class BAYESTREE>
template<class DERIVED, class ELIMINATIONTREE, class JUNCTIONTREE>
class EliminateableFactorGraph {
public:
typedef EliminateableFactorGraph<DERIVED, FACTOR, BAYESNET, BAYESTREE> This;
typedef EliminateableFactorGraph<DERIVED, ELIMINATIONTREE, JUNCTIONTREE> This;
typedef DERIVED FactorGraphType;
typedef FACTOR FactorType;
typedef BAYESNET BayesNetType;
typedef BAYESTREE BayesTreeType;
typedef ELIMINATIONTREE EliminationTreeType;
typedef JUNCTIONTREE JunctionTreeType;
typedef typename EliminationTreeType::FactorType FactorType;
typedef typename EliminationTreeType::BayesNetType BayesNetType;
typedef typename JunctionTreeType::BayesTreeType BayesTreeType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<FactorType> sharedFactor;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
@ -46,28 +49,85 @@ namespace gtsam {
typedef boost::optional<const OrderingUnordered&> OptionalOrdering;
/** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not
* provided, the ordering provided by COLAMD will be used. */
* provided, the ordering provided by COLAMD will be used.
*
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);
* \endcode
*
* <b> Example - Full QR elimination in specified order:
* \code
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, myOrdering);
* \endcode
*
* <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
* \code
* VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
* boost::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, boost::none, varIndex);
* \endcode
* */
boost::shared_ptr<BayesNetType>
eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none) const;
eliminateSequential(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const;
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
* provided, the ordering provided by COLAMD will be used. */
boost::shared_ptr<BayesNetType>
eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none) const;
* provided, the ordering provided by COLAMD will be used.
*
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
* \code
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateCholesky);
* \endcode
*
* <b> Example - Full QR elimination in specified order:
* \code
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering);
* \endcode
*
* <b> Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: </b>
* \code
* VariableIndex varIndex(graph); // Build variable index
* Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
* boost::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex);
* \endcode
* */
boost::shared_ptr<BayesTreeType>
eliminateMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this)) const;
/** Do sequential elimination of some variables to produce a Bayes net and a remaining factor
* graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c
* variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A
* \f$. */
/** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
* where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$
* B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential(const Eliminate& function, OptionalOrdering ordering = boost::none);
eliminatePartialSequential(const Eliminate& function, const Ordering& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of some variables to produce a Bayes tree and a remaining factor
* graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, where \f$ A = \f$ \c
* variables, \f$ X \f$ is all the variables in the factor graph, and \f$ B = X\backslash A
* \f$. */
/** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(const Eliminate& function, OptionalOrdering ordering = boost::none);
eliminatePartialSequential(const Eliminate& function, const std::vector& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(const Eliminate& function, const Ordering& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
/** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
* \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
* \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(const Eliminate& function, const std::vector& ordering,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
};
}

42
gtsam/inference/inferenceExceptions.h Normal file
View File

@ -0,0 +1,42 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file inferenceExceptions.h
* @brief Exceptions that may be thrown by inference algorithms
* @author Richard Roberts
* @date Apr 25, 2013
*/
#pragma once
#include <gtsam/global_includes.h>
#include <boost/lexical_cast.hpp>
#include <exception>
namespace gtsam {
/** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or
* variable index were inconsistent with each other, or a full elimination routine was called
* with an ordering that does not include all of the variables. */
class InconsistentEliminationRequested : public std::exception {
public:
InconsistentEliminationRequested() throw() {}
virtual ~InconsistentEliminationRequested() throw() {}
virtual const char* what() const throw() {
return
"An inference algorithm was called with inconsistent arguments. The\n"
"factor graph, ordering, or variable index were inconsistent with each\n"
"other, or a full elimination routine was called with an ordering that\n"
"does not include all of the variables.";
}
};
}