Continued work on unordered classes and elimination algorithm
Richard Roberts
parent
ffc55ad026
commit
5d05737798
|
|
@ -5,6 +5,7 @@ set (gtsam_subdirs
|
|||
base
|
||||
geometry
|
||||
inference
|
||||
symbolic
|
||||
discrete
|
||||
linear
|
||||
nonlinear
|
||||
|
|
@ -76,6 +77,7 @@ set(gtsam_srcs
|
|||
${base_srcs}
|
||||
${geometry_srcs}
|
||||
${inference_srcs}
|
||||
${symbolic_srcs}
|
||||
${discrete_srcs}
|
||||
${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
|
|
|
|||
|
|
@ -162,18 +162,21 @@ namespace gtsam {
|
|||
|
||||
// Until the stack is empty
|
||||
while(!eliminationStack.empty()) {
|
||||
// Process the next node. If it has children, add its children to the stack and skip it -
|
||||
// we'll come back and eliminate it later after the children have been processed. If it has
|
||||
// no children, we can eliminate it immediately and remove it from the stack.
|
||||
// Process the next node. If it has children, add its children to the stack and mark it
|
||||
// expanded - we'll come back and eliminate it later after the children have been processed.
|
||||
EliminationNode& node = nodeStack.top();
|
||||
if(node.expanded) {
|
||||
// Remove from stack
|
||||
nodeStack.pop();
|
||||
|
||||
// Do a dense elimination step
|
||||
std::vector<Key> keyAsVector(1); keyAsVector[0] = node.key;
|
||||
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
|
||||
function(node.factors, node.key);
|
||||
function(node.factors, keyAsVector);
|
||||
|
||||
// Add conditional to BayesNet and remaining factor to parent
|
||||
bayesNet->push_back(eliminationResult.first);
|
||||
|
||||
// TODO: Don't add null factor?
|
||||
if(node.parent)
|
||||
node.parent->factors.push_back(eliminationResult.second);
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
class EliminationTreeTester; // for unit tests, see testEliminationTree
|
||||
class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -61,6 +61,8 @@ namespace gtsam {
|
|||
typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
|
||||
typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
|
||||
|
||||
private:
|
||||
|
||||
class Node {
|
||||
public:
|
||||
typedef boost::shared_ptr<Node> shared_ptr;
|
||||
|
|
@ -74,8 +76,6 @@ namespace gtsam {
|
|||
|
||||
typedef Node::shared_ptr sharedNode; ///< Shared pointer to Node
|
||||
|
||||
private:
|
||||
|
||||
/** concept check */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
|
||||
|
||||
|
|
@ -105,13 +105,13 @@ namespace gtsam {
|
|||
*/
|
||||
EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order);
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
/** TODO: Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
EliminationTreeUnordered(const This& other);
|
||||
EliminationTreeUnordered(const This& other) { throw std::runtime_error("Not implemented"); }
|
||||
|
||||
/** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
/** TODO: Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
* copied, factors are not cloned. */
|
||||
This& operator=(const This& other);
|
||||
This& operator=(const This& other) { throw std::runtime_error("Not implemented"); }
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -141,7 +141,7 @@ namespace gtsam {
|
|||
private:
|
||||
|
||||
/// Allow access to constructor and add methods for testing purposes
|
||||
friend class ::EliminationTreeTester;
|
||||
friend class ::EliminationTreeUnorderedTester;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -185,37 +185,6 @@ namespace gtsam {
|
|||
/** Get the last factor */
|
||||
sharedFactor back() const { return factors_.back(); }
|
||||
|
||||
///** Eliminate the first \c n frontal variables, returning the resulting
|
||||
// * conditional and remaining factor graph - this is very inefficient for
|
||||
// * eliminating all variables, to do that use EliminationTree or
|
||||
// * JunctionTree.
|
||||
// */
|
||||
//std::pair<sharedConditional, FactorGraph<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
|
||||
//
|
||||
///** Factor the factor graph into a conditional and a remaining factor graph. Given the factor
|
||||
// * graph \f$ f(X) \f$, and \c variables to factorize out \f$ V \f$, this function factorizes
|
||||
// * into \f$ f(X) = f(V;Y)f(Y) \f$, where \f$ Y := X \backslash V \f$ are the remaining
|
||||
// * variables. If \f$ f(X) = p(X) \f$ is a probability density or likelihood, the factorization
|
||||
// * produces a conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
|
||||
// *
|
||||
// * For efficiency, this function treats the variables to eliminate
|
||||
// * \c variables as fully-connected, so produces a dense (fully-connected)
|
||||
// * conditional on all of the variables in \c variables, instead of a sparse BayesNet. If the
|
||||
// * variables are not fully-connected, it is more efficient to sequentially factorize multiple
|
||||
// * times.
|
||||
// */
|
||||
//std::pair<sharedConditional, FactorGraph<FactorType> > eliminate(
|
||||
// const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
|
||||
// boost::optional<const VariableIndex&> variableIndex = boost::none) const;
|
||||
|
||||
///** Eliminate a single variable, by calling FactorGraph::eliminate. */
|
||||
//std::pair<sharedConditional, FactorGraph<FactorType> > eliminateOne(
|
||||
// KeyType variable, const Eliminate& eliminateFcn,
|
||||
// boost::optional<const VariableIndex&> variableIndex = boost::none) const {
|
||||
// std::vector<size_t> variables(1, variable);
|
||||
// return eliminate(variables, eliminateFcn, variableIndex);
|
||||
//}
|
||||
|
||||
/// @}
|
||||
/// @name Modifying Factor Graphs (imperative, discouraged)
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -77,13 +77,16 @@ public:
|
|||
FactorUnordered() {}
|
||||
|
||||
/** Construct unary factor */
|
||||
FactorUnordered(Key key) : keys_(1) { keys_[0] = key; }
|
||||
FactorUnordered(Key key) : keys_(1) {
|
||||
keys_[0] = key; }
|
||||
|
||||
/** Construct binary factor */
|
||||
FactorUnordered(Key key1, Key key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; }
|
||||
FactorUnordered(Key key1, Key key2) : keys_(2) {
|
||||
keys_[0] = key1; keys_[1] = key2; }
|
||||
|
||||
/** Construct ternary factor */
|
||||
FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; }
|
||||
FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) {
|
||||
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; }
|
||||
|
||||
/** Construct 4-way factor */
|
||||
FactorUnordered(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
|
||||
|
|
@ -102,15 +105,15 @@ public:
|
|||
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct n-way factor from iterator over keys. */
|
||||
template<typename ITERATOR> static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) {
|
||||
FactorUnordered result; result.keys_.assign(first, last); }
|
||||
|
||||
/** Construct n-way factor from container of keys. */
|
||||
template<class CONTAINER>
|
||||
static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); }
|
||||
|
||||
/** Construct n-way factor from iterator over keys. */
|
||||
template<class ITERATOR> static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) {
|
||||
FactorUnordered result; result.keys_.assign(first, last); }
|
||||
|
||||
/// @}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,104 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactorGraph.h
|
||||
* @date Oct 29, 2009
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Symbolic Factor Graph
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct empty factor graph */
|
||||
SymbolicFactorGraphUnordered() {}
|
||||
|
||||
///** Eliminate the first \c n frontal variables, returning the resulting
|
||||
// * conditional and remaining factor graph - this is very inefficient for
|
||||
// * eliminating all variables, to do that use EliminationTree or
|
||||
// * JunctionTree. Note that this version simply calls
|
||||
// * FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
|
||||
// * as the eliminate function argument.
|
||||
// */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
|
||||
//
|
||||
///** Factor the factor graph into a conditional and a remaining factor graph.
|
||||
// * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
|
||||
// * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
|
||||
// * \f$ Y := X\V \f$ are the remaining variables. If \f$ f(X) = p(X) \f$ is
|
||||
// * a probability density or likelihood, the factorization produces a
|
||||
// * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
|
||||
// *
|
||||
// * For efficiency, this function treats the variables to eliminate
|
||||
// * \c variables as fully-connected, so produces a dense (fully-connected)
|
||||
// * conditional on all of the variables in \c variables, instead of a sparse
|
||||
// * BayesNet. If the variables are not fully-connected, it is more efficient
|
||||
// * to sequentially factorize multiple times.
|
||||
// * Note that this version simply calls
|
||||
// * FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
|
||||
// * function argument.
|
||||
// */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminate(const std::vector<Index>& variables) const;
|
||||
|
||||
///** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
|
||||
//GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateOne(Index variable) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Push back unary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key);
|
||||
|
||||
/** Push back binary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2);
|
||||
|
||||
/** Push back ternary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3);
|
||||
|
||||
/** Push back 4-way factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4);
|
||||
};
|
||||
|
||||
/** Create a combined joint factor (new style for EliminationTree). */
|
||||
GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph<IndexFactor>& factors,
|
||||
const FastMap<Index, std::vector<Index> >& variableSlots);
|
||||
|
||||
/**
|
||||
* CombineAndEliminate provides symbolic elimination.
|
||||
* Combine and eliminate can also be called separately, but for this and
|
||||
* derived classes calling them separately generally does extra work.
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
|
||||
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,23 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactor.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
} // gtsam
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
# Install headers
|
||||
file(GLOB symbolic_headers "*.h")
|
||||
install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
set(symbolic_local_libs
|
||||
symbolic
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
)
|
||||
|
||||
# Files to exclude from compilation of tests and timing scripts
|
||||
set(symbolic_excluded_files
|
||||
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
|
||||
"" # Add to this list, with full path, to exclude
|
||||
)
|
||||
|
||||
# Build tests
|
||||
if (GTSAM_BUILD_TESTS)
|
||||
gtsam_add_subdir_tests(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}")
|
||||
endif(GTSAM_BUILD_TESTS)
|
||||
|
||||
# Build timing scripts
|
||||
if (GTSAM_BUILD_TIMING)
|
||||
gtsam_add_subdir_timing(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}")
|
||||
endif(GTSAM_BUILD_TIMING)
|
||||
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicBayesNet.h
|
||||
* @date Oct 29, 2009
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Symbolic Bayes Net
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered {
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct empty factor graph */
|
||||
SymbolicBayesNetUnordered() {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -15,7 +15,7 @@
|
|||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicConditionalUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -18,8 +18,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/SymbolicFactorUnordered.h>
|
||||
#include <gtsam/inference/ConditionalUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -62,14 +62,22 @@ namespace gtsam {
|
|||
SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {}
|
||||
|
||||
/** Named constructor from an arbitrary number of keys and frontals */
|
||||
template<class ITERATOR>
|
||||
static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) :
|
||||
template<typename ITERATOR>
|
||||
static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
|
||||
{
|
||||
SymbolicConditionalUnordered result;
|
||||
result.keys_.assign(firstKey, lastKey);
|
||||
result = BaseFactor::FromIterator(firstKey, lastKey);
|
||||
result.nrFrontals_ = nrFrontals;
|
||||
return result;
|
||||
}
|
||||
return result; }
|
||||
|
||||
/** Named constructor from an arbitrary number of keys and frontals */
|
||||
template<class CONTAINER>
|
||||
static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals)
|
||||
{
|
||||
SymbolicConditionalUnordered result;
|
||||
result = BaseFactor::FromKeys(keys);
|
||||
result.nrFrontals_ = nrFrontals;
|
||||
return result; }
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -16,15 +16,16 @@
|
|||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/SymbolicBayesNetUnordered.h>
|
||||
#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
|
||||
#include <gtsam/inference/EliminationTreeUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class SymbolicEliminationTreeUnordered : public EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> {
|
||||
class SymbolicEliminationTreeUnordered :
|
||||
public EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> {
|
||||
public:
|
||||
typedef EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> Base; ///< Base class
|
||||
typedef EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> Base; ///< Base class
|
||||
typedef SymbolicEliminationTreeUnordered This; ///< This class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
|
||||
|
||||
|
|
@ -45,7 +46,7 @@ namespace gtsam {
|
|||
* constructor instead.
|
||||
* @param factorGraph The factor graph for which to build the elimination tree
|
||||
*/
|
||||
SymbolicEliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order) :
|
||||
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector<Key>& order) :
|
||||
Base(factorGraph, order) {}
|
||||
|
||||
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
|
||||
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactorGraph.h
|
||||
* @date Oct 29, 2009
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/inference/FactorGraphUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Symbolic Factor Graph
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct empty factor graph */
|
||||
SymbolicFactorGraphUnordered() {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Push back unary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key);
|
||||
|
||||
/** Push back binary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2);
|
||||
|
||||
/** Push back ternary factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3);
|
||||
|
||||
/** Push back 4-way factor */
|
||||
GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4);
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SymbolicFactor.cpp
|
||||
* @author Richard Roberts
|
||||
* @date Oct 17, 2010
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
|
||||
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
|
||||
EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys)
|
||||
{
|
||||
// Gather all keys
|
||||
FastSet<Key> allKeys;
|
||||
BOOST_FOREACH(const SymbolicFactorUnordered::shared_ptr& factor, factors) {
|
||||
allKeys.insert(factor->begin(), factor->end()); }
|
||||
|
||||
// Sort frontal keys
|
||||
FastSet<Key> frontals(keys);
|
||||
const size_t nFrontals = keys.size();
|
||||
|
||||
// Build a key vector with the frontals followed by the separator
|
||||
vector<Key> orderedKeys(allKeys.size());
|
||||
std::copy(keys.begin(), keys.end(), orderedKeys.begin());
|
||||
std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
|
||||
|
||||
// Return resulting conditional and factor
|
||||
return make_pair(
|
||||
boost::make_shared<SymbolicConditionalUnordered>(
|
||||
SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)),
|
||||
boost::make_shared<SymbolicFactorUnordered>(
|
||||
SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end())));
|
||||
}
|
||||
|
||||
} // gtsam
|
||||
|
|
@ -48,7 +48,7 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SymbolicFactorUnordered() {}
|
||||
~SymbolicFactorUnordered() {}
|
||||
|
||||
/** Copy constructor */
|
||||
SymbolicFactorUnordered(const This& f) : Base(f) {}
|
||||
|
|
@ -80,7 +80,18 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class KeyIterator> SymbolicFactorUnordered(KeyIterator beginKey, KeyIterator endKey) : Base(beginKey, endKey) {}
|
||||
template<typename KEYITERATOR>
|
||||
static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) {
|
||||
SymbolicFactorUnordered result;
|
||||
result = Base::FromIterator(beginKey, endKey);
|
||||
return result; }
|
||||
|
||||
/** Constructor from a collection of keys */
|
||||
template<class CONTAINER>
|
||||
static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) {
|
||||
SymbolicFactorUnordered result;
|
||||
result = Base::FromKeys(keys);
|
||||
return result; }
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -91,7 +102,10 @@ namespace gtsam {
|
|||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
}
|
||||
|
||||
}; // IndexFactor
|
||||
|
||||
|
||||
std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
|
||||
EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testSymbolicEliminationTree.cpp
|
||||
* @brief
|
||||
* @author Richard Roberts
|
||||
* @date Oct 14, 2010
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
class EliminationTreeUnorderedTester {
|
||||
public:
|
||||
// build hardcoded tree
|
||||
static SymbolicEliminationTreeUnordered buildHardcodedTree(const SymbolicFactorGraphUnordered& fg) {
|
||||
|
||||
SymbolicEliminationTreeUnordered::sharedNode leaf0(new SymbolicEliminationTree);
|
||||
leaf0->add(fg[0]);
|
||||
leaf0->add(fg[1]);
|
||||
|
||||
SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTree(1));
|
||||
node1->add(fg[2]);
|
||||
node1->add(leaf0);
|
||||
|
||||
SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTree(2));
|
||||
node2->add(fg[3]);
|
||||
node2->add(node1);
|
||||
|
||||
SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTree(3));
|
||||
leaf3->add(fg[4]);
|
||||
|
||||
SymbolicEliminationTreeUnordered::sharedNode etree(new SymbolicEliminationTree(4));
|
||||
etree->add(leaf3);
|
||||
etree->add(node2);
|
||||
|
||||
return etree;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(EliminationTree, Create)
|
||||
{
|
||||
// create example factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg);
|
||||
|
||||
// Build from factor graph
|
||||
SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg);
|
||||
|
||||
CHECK(assert_equal(*expected,*actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test to drive elimination tree development
|
||||
// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST(EliminationTree, eliminate )
|
||||
{
|
||||
// create expected Chordal bayes Net
|
||||
SymbolicBayesNet expected;
|
||||
expected.push_front(boost::make_shared<IndexConditional>(4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(3,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(2,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(1,2,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(0,1,2));
|
||||
|
||||
// Create factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
// eliminate
|
||||
SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(EliminationTree, disconnected_graph) {
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 2);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue