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

More work on elimination and BayesTree

release/4.3a0
Richard Roberts 2013-06-06 15:36:51 +00:00
parent 1423d684f7
commit ec2df2df3c
29 changed files with 734 additions and 440 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
gtsam/base/tests/testTreeTraversal.cpp Normal file
View File

@ -0,0 +1,109 @@
/* ----------------------------------------------------------------------------
* 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 testTreeTraversal
* @brief Unit tests for tree traversal, cloning, and printing
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <vector>
#include <list>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/assign/std/list.hpp>
using boost::assign::operator+=;
using namespace std;
struct TestNode {
typedef boost::shared_ptr<TestNode> shared_ptr;
int data;
vector<shared_ptr> children;
TestNode(int data) : data(data) {}
};
struct TestForest {
typedef TestNode::shared_ptr sharedNode;
vector<sharedNode> roots_;
const vector<sharedNode>& roots() const { return roots_; }
};
TestForest makeTestForest() {
// 0 1
// / \
// 2 3
// |
// 4
TestForest forest;
forest.roots_.push_back(boost::make_shared<TestNode>(0));
forest.roots_.push_back(boost::make_shared<TestNode>(1));
forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(2));
forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(3));
forest.roots_[0]->children[1]->children.push_back(boost::make_shared<TestNode>(4));
return forest;
}
/* ************************************************************************* */
struct PreOrderVisitor {
// This visitor stores the nodes visited so the visit order can be checked later. It also returns
// the current node index as the data and checks that the parent index properly matches the index
// referenced in the node.
std::list<int> visited;
bool parentsMatched;
PreOrderVisitor() : parentsMatched(true) {}
int operator()(const TestNode::shared_ptr& node, int parentData) {
visited.push_back(node->data);
// Check parent index
const int expectedParentIndex =
node->data == 0 ? -1 :
node->data == 1 ? -1 :
node->data == 2 ? 0 :
node->data == 3 ? 0 :
node->data == 4 ? 0 :
(throw std::runtime_error("Unexpected node index"), -1);
if(expectedParentIndex != parentData)
parentsMatched = false;
return node->data;
}
};
/* ************************************************************************* */
struct PostOrderVisitor {
// This visitor stores the nodes visited so the visit order can be checked later.
std::list<int> visited;
void operator()(const TestNode::shared_ptr& node, int myData) {
visited.push_back(node->data);
}
};
/* ************************************************************************* */
TEST(treeTraversal, DepthFirst)
{
// Get test forest
TestForest testForest = makeTestForest();
// Expected pre-order
std::list<int> preOrderExpected;
preOrderExpected += 0, 2, 3, 4, 1;
std::list<int> postOrderExpected;
postOrderExpected += 2, 4, 3, 0, 1;
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

53
gtsam/base/treeTraversal-inst.h
View File

@ -18,6 +18,10 @@
#include <gtsam/base/FastList.h>
#include <stack>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
namespace gtsam {
/** Internal functions used for traversing trees */
@ -29,30 +33,15 @@ namespace gtsam {
template<typename NODE, typename DATA>
struct TraversalNode {
bool expanded;
NODE& const treeNode;
const boost::shared_ptr<NODE>& treeNode;
DATA data;
TraversalNode(NODE* _treeNode, const DATA& _data) :
TraversalNode(const boost::shared_ptr<NODE>& _treeNode, const DATA& _data) :
expanded(false), treeNode(_treeNode), data(_data) {}
};
// Functional object to visit a node and then add it to the traversal stack
template<typename NODE, typename VISITOR, typename DATA, typename STACK>
struct PreOrderExpand {
VISITOR& visitor_;
DATA& parentData_;
STACK& stack_;
PreOrderExpand(VISITOR& visitor, DATA& parentData, STACK& stack) :
visitor_(visitor), parentData_(parentData), stack_(stack) {}
template<typename P>
void operator()(const P& nodePointer) {
// Add node
stack_.push(TraversalNode<NODE,DATA>(*nodePointer, visitor_(nodePointer, parentData_)));
}
};
/// Do nothing - default argument for post-visitor for tree traversal
template<typename NODE, typename DATA>
void no_op(const NODE& node, const DATA& data) {}
void no_op(const boost::shared_ptr<NODE>& node, const DATA& data) {}
}
/** Traverse a forest depth-first with pre-order and post-order visits.
@ -72,16 +61,19 @@ namespace gtsam {
template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
{
// Typedefs
typedef typename FOREST::Node Node;
typedef boost::shared_ptr<Node> sharedNode;
// Depth first traversal stack
typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
typedef std::stack<TraversalNode, FastList<TraversalNode> > Stack;
typedef PreOrderExpand<typename FOREST::Node, VISITOR_PRE, DATA, Stack> Expander;
Stack stack;
// Add roots to stack (use reverse iterators so children are processed in the order they
// appear)
(void) std::for_each(forest.roots().rbegin(), forest.roots().rend(),
Expander(visitorPre, rootData, stack));
BOOST_REVERSE_FOREACH(const sharedNode& root, forest.roots())
stack.push(TraversalNode(root, visitorPre(root, rootData)));
// Traverse
while(!stack.empty())
@ -97,8 +89,8 @@ namespace gtsam {
} else {
// If not already visited, visit the node and add its children (use reverse iterators so
// children are processed in the order they appear)
(void) std::for_each(node.treeNode.children.rbegin(), node.treeNode.children.rend(),
Expander(visitorPre, node.data, stack));
BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->children)
stack.push(TraversalNode(child, visitorPre(child, node.data)));
node.expanded = true;
}
}
@ -118,8 +110,7 @@ namespace gtsam {
template<class FOREST, typename DATA, typename VISITOR_PRE>
void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
{
DepthFirstForest<FOREST, DATA, VISITOR_PRE, void(&)(const typename FOREST::Node&, const DATA&)>(
forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
DepthFirstForest(forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
}
@ -127,10 +118,11 @@ namespace gtsam {
/** Traversal function for CloneForest */
namespace {
template<typename NODE>
boost::shared_ptr<NODE> CloneForestVisitorPre(const NODE& node, const boost::shared_ptr<NODE>& parentPointer)
boost::shared_ptr<NODE>
CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
{
// Clone the current node and add it to its cloned parent
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(node);
boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
parentPointer->children.push_back(clone);
return clone;
}
@ -155,10 +147,11 @@ namespace gtsam {
/** Traversal function for PrintForest */
namespace {
template<typename NODE>
std::string PrintForestVisitorPre(const NODE& node, const std::string& parentString, const KeyFormatter& formatter)
std::string
PrintForestVisitorPre(const boost::shared_ptr<NODE>& node, const std::string& parentString, const KeyFormatter& formatter)
{
// Print the current node
node.print(parentString + "-", formatter);
node->print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
@ -169,7 +162,7 @@ namespace gtsam {
template<class FOREST>
void PrintForest(const FOREST& forest, const std::string& str, const KeyFormatter& keyFormatter) {
typedef typename FOREST::Node Node;
DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, formatter));
DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, keyFormatter));
}
}

11
gtsam/inference/BayesNetUnordered-inst.h
View File

@ -19,7 +19,9 @@
#pragma once
#include <gtsam/inference/BayesNetUnordered.h>
#include <gtsam/inference/FactorGraphUnordered-inst.h>
#include <boost/foreach.hpp>
#include <fstream>
namespace gtsam {
@ -32,17 +34,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNetUnordered<CONDITIONAL>::saveGraph(
const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter)
void BayesNetUnordered<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Index me = frontals.front();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
BOOST_FOREACH(Index p, parents)
BOOST_FOREACH(Key p, parents)
of << p << "->" << me << std::endl;
}

244
gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h
View File

@ -22,22 +22,22 @@
namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
std::vector<Key>
BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::separator_setminus_B(derived_ptr B) const
BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::separator_setminus_B(derived_ptr B) const
{
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
std::vector<Key> S_setminus_B;
std::set_difference(p_F_S->beginParents(), p_F_S->endParents(),
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::shortcut_indices(
derived_ptr B, const FactorGraphUnordered<FactorType>& p_Cp_B) const
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut_indices(
derived_ptr B, const FactorGraphType& p_Cp_B) const
{
gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys();
@ -54,31 +54,31 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::print(
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::print(
const std::string& s, const KeyFormatter& keyFormatter) const
{
conditional_->print(s, keyFormatter);
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::treeSize() const {
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::treeSize() const {
size_t size = 1;
BOOST_FOREACH(const derived_ptr& child, children_)
BOOST_FOREACH(const derived_ptr& child, children)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
size_t BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::numCachedSeparatorMarginals() const
{
if (!cachedSeparatorMarginal_)
return 0;
size_t subtree_count = 1;
BOOST_FOREACH(const derived_ptr& child, children_)
BOOST_FOREACH(const derived_ptr& child, children)
subtree_count += child->numCachedSeparatorMarginals();
return subtree_count;
@ -89,146 +89,146 @@ namespace gtsam {
// clique on the root. We can compute it recursively from the parent shortcut
// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
BayesNetUnordered<CONDITIONAL> BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::shortcut(
derived_ptr B, Eliminate function) const
{
gttic(BayesTreeCliqueBaseUnordered_shortcut);
//template<class DERIVED, class FACTORGRAPH, class BAYESNET>
//BayesNetUnordered<CONDITIONAL> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut(
// derived_ptr B, Eliminate function) const
//{
// gttic(BayesTreeCliqueBaseUnordered_shortcut);
// We only calculate the shortcut when this clique is not B
// and when the S\B is not empty
std::vector<Key> S_setminus_B = separator_setminus_B(B);
if (B.get() != this && !S_setminus_B.empty()) {
// // We only calculate the shortcut when this clique is not B
// // and when the S\B is not empty
// std::vector<Key> S_setminus_B = separator_setminus_B(B);
// if (B.get() != this && !S_setminus_B.empty()) {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_shortcut);
FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBaseUnordered_shortcut);
p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
// // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
// derived_ptr parent(parent_.lock());
// gttoc(BayesTreeCliqueBaseUnordered_shortcut);
// FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
// gttic(BayesTreeCliqueBaseUnordered_shortcut);
// p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B
std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
// // Determine the variables we want to keepSet, S union B
// std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
if(factor) factor->reduceWithInverse(inverseReduction); }
inverseReduction.applyInverse(keep);
gttoc(Reduce);
// // Reduce the variable indices to start at zero
// gttic(Reduce);
// const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
// if(factor) factor->reduceWithInverse(inverseReduction); }
// inverseReduction.applyInverse(keep);
// gttoc(Reduce);
// Create solver that will marginalize for us
GenericSequentialSolver<FactorType> solver(p_Cp_B);
// // Create solver that will marginalize for us
// GenericSequentialSolver<FactorType> solver(p_Cp_B);
// Finally, we only want to have S\B variables in the Bayes net, so
size_t nrFrontals = S_setminus_B.size();
BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
// // Finally, we only want to have S\B variables in the Bayes net, so
// size_t nrFrontals = S_setminus_B.size();
// BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
// Undo the reduction
gttic(Undo_Reduce);
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
if (factor) factor->permuteWithInverse(reduction); }
result.permuteWithInverse(reduction);
gttoc(Undo_Reduce);
// // Undo the reduction
// gttic(Undo_Reduce);
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
// if (factor) factor->permuteWithInverse(reduction); }
// result.permuteWithInverse(reduction);
// gttoc(Undo_Reduce);
assertInvariants();
// assertInvariants();
return result;
} else {
return BayesNet<CONDITIONAL>();
}
}
// return result;
// } else {
// return BayesNet<CONDITIONAL>();
// }
//}
/* ************************************************************************* */
// separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseUnordered<
DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
{
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
// Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_) {
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
// If this is the root, there is no separator
if (R.get() == this) {
// we are root, return empty
FactorGraph<FactorType> empty;
cachedSeparatorMarginal_ = empty;
} else {
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
// now add the parent conditional
p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
//template<class DERIVED, class FACTORGRAPH, class BAYESNET>
//FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered<
// DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
//{
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
// // Check if the Separator marginal was already calculated
// if (!cachedSeparatorMarginal_) {
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
// // If this is the root, there is no separator
// if (R.get() == this) {
// // we are root, return empty
// FactorGraph<FactorType> empty;
// cachedSeparatorMarginal_ = empty;
// } else {
// // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// // initialize P(Cp) with the parent separator marginal
// derived_ptr parent(parent_.lock());
// gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
// gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
// FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
// // now add the parent conditional
// p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
// Reduce the variable indices to start at zero
gttic(Reduce);
const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
if(factor) factor->reduceWithInverse(inverseReduction); }
// // Reduce the variable indices to start at zero
// gttic(Reduce);
// const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
// internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
// BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
// if(factor) factor->reduceWithInverse(inverseReduction); }
// The variables we want to keepSet are exactly the ones in S
sharedConditional p_F_S = this->conditional();
std::vector<Key> indicesS(p_F_S->beginParents(), p_F_S->endParents());
inverseReduction.applyInverse(indicesS);
gttoc(Reduce);
// // The variables we want to keepSet are exactly the ones in S
// sharedConditional p_F_S = this->conditional();
// std::vector<Key> indicesS(p_F_S->beginParents(), p_F_S->endParents());
// inverseReduction.applyInverse(indicesS);
// gttoc(Reduce);
// Create solver that will marginalize for us
GenericSequentialSolver<FactorType> solver(p_Cp);
// // Create solver that will marginalize for us
// GenericSequentialSolver<FactorType> solver(p_Cp);
cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
// cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
// Undo the reduction
gttic(Undo_Reduce);
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
if (factor) factor->permuteWithInverse(reduction); }
BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
if (factor) factor->permuteWithInverse(reduction); }
gttoc(Undo_Reduce);
}
} else {
gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit);
}
// // Undo the reduction
// gttic(Undo_Reduce);
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
// if (factor) factor->permuteWithInverse(reduction); }
// BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
// if (factor) factor->permuteWithInverse(reduction); }
// gttoc(Undo_Reduce);
// }
// } else {
// gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit);
// }
// return the shortcut P(S||B)
return *cachedSeparatorMarginal_; // return the cached version
}
// // return the shortcut P(S||B)
// return *cachedSeparatorMarginal_; // return the cached version
//}
/* ************************************************************************* */
// marginal2, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseUnordered<
DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
{
gttic(BayesTreeCliqueBaseUnordered_marginal2);
// initialize with separator marginal P(S)
FactorGraph<FactorType> p_C(this->separatorMarginal(R, function));
// add the conditional P(F|S)
p_C.push_back(this->conditional()->toFactor());
return p_C;
}
//template<class DERIVED, class FACTORGRAPH, class BAYESNET>
//FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered<
// DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
//{
// gttic(BayesTreeCliqueBaseUnordered_marginal2);
// // initialize with separator marginal P(S)
// FactorGraph<FactorType> p_C(this->separatorMarginal(R, function));
// // add the conditional P(F|S)
// p_C.push_back(this->conditional()->toFactor());
// return p_C;
//}
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
void BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::deleteCachedShortcuts() {
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the
// root are also generated. So, if this clique's cached shortcut is set,
// recursively call over all child cliques. Otherwise, it is unnecessary.
if (cachedSeparatorMarginal_) {
BOOST_FOREACH(derived_ptr& child, children_) {
BOOST_FOREACH(derived_ptr& child, children) {
child->deleteCachedShortcuts();
}

51
gtsam/inference/BayesTreeCliqueBaseUnordered.h
View File

@ -22,7 +22,7 @@
#include <gtsam/inference/BayesNetUnordered.h>
namespace gtsam {
template<class CONDITIONAL, class CLIQUE> class BayesTreeUnordered;
template<class CLIQUE> class BayesTreeUnordered;
}
namespace gtsam {
@ -43,18 +43,21 @@ namespace gtsam {
template<class DERIVED, class FACTORGRAPH, class BAYESNET>
struct BayesTreeCliqueBaseUnordered {
public:
private:
typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET> This;
typedef DERIVED DerivedType;
typedef FACTORGRAPH FactorGraphType;
typedef BAYESNET BayesNetType;
typedef BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef boost::shared_ptr<DerivedType> derived_ptr;
typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
typedef FactorGraphType::FactorType FactorType;
public:
typedef FACTORGRAPH FactorGraphType;
typedef BAYESNET BayesNetType;
typedef typename BayesNetType::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename FactorGraphType::FactorType FactorType;
typedef typename FactorGraphType::Eliminate Eliminate;
protected:
@ -87,8 +90,7 @@ namespace gtsam {
}
/** print this node */
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// @}
/// @name Standard Interface
@ -113,14 +115,14 @@ namespace gtsam {
/// @name Advanced Interface
/// @{
/** return the conditional P(S|Root) on the separator given the root */
BayesNetType shortcut(derived_ptr root, Eliminate function) const;
///** return the conditional P(S|Root) on the separator given the root */
//BayesNetType shortcut(derived_ptr root, Eliminate function) const;
/** return the marginal P(S) on the separator */
FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const;
///** return the marginal P(S) on the separator */
//FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const;
/** return the marginal P(C) of the clique, using marginal caching */
FactorGraphType marginal2(derived_ptr root, Eliminate function) const;
///** return the marginal P(C) of the clique, using marginal caching */
//FactorGraphType marginal2(derived_ptr root, Eliminate function) const;
/**
* This deletes the cached shortcuts of all cliques (subtree) below this clique.
@ -146,30 +148,13 @@ namespace gtsam {
* The factor graph p_Cp_B has keys from the parent clique Cp and from B.
* But we only keep the variables not in S union B.
*/
std::vector<Key> shortcut_indices(derived_ptr B,
const FactorGraph<FactorType>& p_Cp_B) const;
std::vector<Key> shortcut_indices(derived_ptr B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
private:
/**
* Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
BayesTreeCliqueBaseUnordered(const This& other) {
assert(false);
}
/** Cliques cannot be copied except by the clone() method, which does not
* copy the parent and child pointers.
*/
This& operator=(const This& other) {
assert(false);
return *this;
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>

4
gtsam/inference/BayesTreeCliqueDefault.h
View File

@ -40,13 +40,13 @@ namespace gtsam {
public BayesTreeCliqueBaseUnordered<BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET>, FACTORGRAPH, BAYESNET> {
public:
typedef typename BAYESNET::ConditionalType ConditionalType;
typedef typename FACTORGRAPH::FactorType FactorType;
typedef BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET> This;
typedef BayesTreeCliqueBaseUnordered<This, FACTORGRAPH, BAYESNET> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
BayesTreeCliqueDefaultUnordered() {}
BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
BayesTreeCliqueDefaultUnordered(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
BayesTreeCliqueDefaultUnordered(const boost::shared_ptr<ConditionalType>& conditional) : Base(conditional) {}
private:
/** Serialization function */

119
gtsam/inference/BayesTreeUnordered-inst.h
View File

@ -21,8 +21,10 @@
#pragma once
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/foreach.hpp>
#include <fstream>
namespace gtsam {
@ -31,16 +33,17 @@ namespace gtsam {
typename BayesTreeUnordered<CLIQUE>::CliqueData
BayesTreeUnordered<CLIQUE>::getCliqueData() const {
CliqueData data;
getCliqueData(data, root_);
BOOST_FOREACH(const sharedClique& root, roots_)
getCliqueData(data, root);
return data;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) {
data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
data.separatorSizes.push_back(clique->conditional()->nrParents());
BOOST_FOREACH(sharedClique c, clique->children) {
getCliqueData(data, c);
}
}
@ -48,16 +51,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
size_t BayesTreeUnordered<CLIQUE>::numCachedSeparatorMarginals() const {
return (root_) ? root_->numCachedSeparatorMarginals() : 0;
size_t count = 0;
BOOST_FOREACH(const sharedClique& root, roots_)
count += root->numCachedSeparatorMarginals();
return count;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const {
if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
std::ofstream of(s.c_str());
of<< "digraph G{\n";
saveGraph(of, root_, keyFormatter);
BOOST_FOREACH(const sharedClique& root, roots_)
saveGraph(of, root, keyFormatter);
of<<"}";
of.close();
}
@ -77,7 +84,7 @@ namespace gtsam {
parent += indexFormatter(index);
}
if( clique != root_){
if(clique->parent()){
parent += " : ";
s << parentnum << "->" << num << "\n";
}
@ -91,7 +98,7 @@ namespace gtsam {
s << parent;
parentnum = num;
BOOST_FOREACH(sharedClique c, clique->children_) {
BOOST_FOREACH(sharedClique c, clique->children) {
num++;
saveGraph(s, c, indexFormatter, parentnum);
}
@ -152,27 +159,24 @@ namespace gtsam {
/* ************************************************************************* */
template<class CLIQUE>
typename BayesTreeUnordered<CLIQUE>::sharedClique
BayesTreeUnordered<CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
sharedClique new_clique(new Clique(conditional));
addClique(new_clique, parent_clique);
return new_clique;
size_t BayesTreeUnordered<CLIQUE>::size() const {
size_t size = 0;
BOOST_FOREACH(const sharedClique& clique, roots_)
size += clique->treeSize();
return size;
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, (*clique)->frontals())
BOOST_FOREACH(Index j, clique->conditional()->frontals())
nodes_[j] = clique;
if (parent_clique != NULL) {
clique->parent_ = parent_clique;
parent_clique->children_.push_back(clique);
parent_clique->children.push_back(clique);
} else {
assert(!root_);
root_ = clique;
roots_.push_back(clique);
}
clique->assertInvariants();
}
/* ************************************************************************* */
@ -181,13 +185,11 @@ namespace gtsam {
const sharedConditional& conditional, std::list<sharedClique>& child_cliques)
{
sharedClique new_clique(new Clique(conditional));
nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
BOOST_FOREACH(Index j, conditional->frontals())
nodes_[j] = new_clique;
new_clique->children_ = child_cliques;
new_clique->children.assign(child_cliques.begin(), child_cliques.end());
BOOST_FOREACH(sharedClique& child, child_cliques)
child->parent_ = new_clique;
new_clique->assertInvariants();
return new_clique;
}
@ -196,16 +198,16 @@ namespace gtsam {
void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
{
if (clique->isRoot())
root_.reset();
roots_.erase(std::find(roots_.begin(), roots_.end(), clique));
else { // detach clique from parent
sharedClique parent = clique->parent_.lock();
typename FastList<sharedClique>::iterator child = std::find(parent->children().begin(), parent->children().end(), clique);
assert(child != parent->children().end());
parent->children().erase(child);
typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
assert(child != parent->children.end());
parent->children.erase(child);
}
// orphan my children
BOOST_FOREACH(sharedClique child, clique->children_)
BOOST_FOREACH(sharedClique child, clique->children)
child->parent_ = typename Clique::weak_ptr();
BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
@ -304,7 +306,9 @@ namespace gtsam {
template<class CLIQUE>
BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<CLIQUE>::operator=(const This& other) {
this->clear();
other.cloneTo(*this);
std::vector<sharedClique> clonedRoots = treeTraversal::CloneForest(other);
BOOST_FOREACH(const sharedClique& root, clonedRoots)
insertRoot(root);
return *this;
}
@ -312,24 +316,25 @@ namespace gtsam {
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
treeTraversal::PrintForest(*this, s, keyFormatter)
treeTraversal::PrintForest(*this, s, keyFormatter);
}
/* ************************************************************************* */
// binary predicate to test equality of a pair for use in equals
template<class CLIQUE>
bool check_sharedCliques(
const typename BayesTreeUnordered<CLIQUE>::sharedClique& v1,
const typename BayesTreeUnordered<CLIQUE>::sharedClique& v2
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v1,
const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v2
) {
return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
return v1.first == v2.first &&
((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
}
/* ************************************************************************* */
template<class CLIQUE>
bool BayesTreeUnordered<CLIQUE>::equals(const BayesTreeUnordered<CLIQUE>& other, double tol) const {
return size()==other.size() &&
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
}
/* ************************************************************************* */
@ -348,31 +353,15 @@ namespace gtsam {
BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
// Fill index for each child
typedef typename BayesTreeUnordered<CLIQUE>::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& child, subtree->children_) {
BOOST_FOREACH(const sharedClique& child, subtree->children) {
fillNodesIndex(child); }
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::insert(const sharedClique& subtree) {
if(subtree) {
// Find the parent clique of the new subtree. By the running intersection
// property, those separator variables in the subtree that are ordered
// lower than the highest frontal variable of the subtree root will all
// appear in the separator of the subtree root.
if(subtree->conditional()->parents().empty()) {
assert(!root_);
root_ = subtree;
} else {
Key parentRepresentative = findParentClique(subtree->conditional()->parents());
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += subtree;
subtree->parent_ = parent; // set new parent!
}
// Now fill in the nodes index
fillNodesIndex(subtree);
}
void BayesTreeUnordered<CLIQUE>::insertRoot(const sharedClique& subtree) {
roots_.push_back(subtree); // Add to roots
fillNodesIndex(subtree); // Populate nodes index
}
/* ************************************************************************* */
@ -576,7 +565,15 @@ namespace gtsam {
void BayesTreeUnordered<CLIQUE>::clear() {
// Remove all nodes and clear the root pointer
nodes_.clear();
root_.reset();
roots_.clear();
}
/* ************************************************************************* */
template<class CLIQUE>
void BayesTreeUnordered<CLIQUE>::deleteCachedShortcuts() {
BOOST_FOREACH(const sharedClique& root, roots_) {
root->deleteCachedShortcuts();
}
}
/* ************************************************************************* */
@ -596,7 +593,8 @@ namespace gtsam {
this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
// add children to list of orphans (splice also removed them from clique->children_)
orphans.splice(orphans.begin(), clique->children_);
orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end());
clique->children.clear();
bn.push_back(clique->conditional());
@ -638,15 +636,16 @@ namespace gtsam {
// Remove the first clique from its parents
if(!subtree->isRoot())
subtree->parent()->children().remove(subtree);
subtree->parent()->children.erase(std::find(
subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
else
root_.reset();
roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
// Add all subtree cliques and erase the children and parent of each
for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
{
// Add children
BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
BOOST_FOREACH(const sharedClique& child, (*clique)->children) {
cliques.push_back(child); }
// Delete cached shortcuts
@ -658,7 +657,7 @@ namespace gtsam {
// Erase the parent and children pointers
(*clique)->parent_.reset();
(*clique)->children_.clear();
(*clique)->children.clear();
}
return cliques;

30
gtsam/inference/BayesTreeUnordered.h
View File

@ -47,6 +47,8 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
typedef Clique Node; ///< Synonym for Clique (TODO: remove)
typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
typedef typename CLIQUE::ConditionalType ConditionalType;
typedef boost::shared_ptr<ConditionalType> sharedConditional;
typedef typename CLIQUE::BayesNetType BayesNetType;
@ -128,12 +130,7 @@ namespace gtsam {
/// @{
/** number of cliques */
inline size_t size() const {
if(root_)
return root_->treeSize();
else
return 0;
}
size_t size() const;
/** Check if there are any cliques in the tree */
inline bool empty() const {
@ -152,7 +149,7 @@ namespace gtsam {
if(c == nodes_.end())
throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
else
return *c;
return c->second;
}
/** Gather data on all cliques */
@ -205,9 +202,7 @@ namespace gtsam {
void clear();
/** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
void deleteCachedShortcuts() {
root_->deleteCachedShortcuts();
}
void deleteCachedShortcuts();
/**
* Remove path from clique to root and return that path as factors
@ -226,14 +221,10 @@ namespace gtsam {
* Remove the requested subtree. */
Cliques removeSubtree(const sharedClique& subtree);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/** Insert a new subtree with known parent clique. This function does not check that the
* specified parent is the correct parent. This function updates all of the internal data
* structures associated with adding a subtree, such as populating the nodes index. */
void insertRoot(const sharedClique& subtree);
/**
* Create a clone of this object as a shared pointer
@ -255,9 +246,6 @@ namespace gtsam {
/** remove a clique: warning, can result in a forest */
void removeClique(sharedClique clique);
/** add a clique (top down) */
sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
/** add a clique (top down) */
void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());

3
gtsam/inference/ConditionalUnordered.h
View File

@ -75,7 +75,8 @@ namespace gtsam {
void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** check equality */
bool equals(const This& c, double tol = 1e-9) const { return nrFrontals_ == c.nrFrontals_; }
bool equals(const This& c, double tol = 1e-9) const {
return asFactor().equals(c.asFactor()) && nrFrontals_ == c.nrFrontals_; }
/// @}
/// @name Standard Interface

13
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -35,11 +35,11 @@ namespace gtsam {
std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> > result;
if(ordering) {
// Do elimination with given ordering
result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function);
result = ELIMINATIONTREE(asDerived(), variableIndex, *ordering).eliminate(function);
} else {
// Compute ordering
OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
result = EliminationTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex);
result = ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering).eliminate(function);
}
// If any factors are remaining, the ordering was incomplete
@ -62,11 +62,11 @@ namespace gtsam {
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> > result;
if(ordering) {
// Do elimination with given ordering
result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function);
result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, *ordering)).eliminate(function);
} else {
// Compute ordering
OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
result = JunctionTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex);
result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering)).eliminate(function);
}
// If any factors are remaining, the ordering was incomplete
@ -76,4 +76,5 @@ namespace gtsam {
// Return the Bayes tree
return result.first;
}
}

7
gtsam/inference/EliminateableFactorGraph.h
View File

@ -121,6 +121,13 @@ namespace gtsam {
eliminatePartialMultifrontal(const Eliminate& function, const std::vector<Key>& variables,
const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
private:
// Access the derived factor graph class
const FACTORGRAPH& asDerived() const { return static_cast<const FACTORGRAPH&>(*this); }
// Access the derived factor graph class
FACTORGRAPH& asDerived() { return static_cast<FACTORGRAPH&>(*this); }
};
}

9
gtsam/inference/EliminationTreeUnordered-inst.h
View File

@ -26,6 +26,7 @@
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/inference/inference-inst.h>
namespace gtsam {
@ -64,8 +65,8 @@ namespace gtsam {
void EliminationTreeUnordered<BAYESNET,GRAPH>::Node::print(
const std::string& str, const KeyFormatter& keyFormatter) const
{
std::cout << str << "(" << formatter(node->key) << ")\n";
BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) {
std::cout << str << "(" << keyFormatter(key) << ")\n";
BOOST_FOREACH(const sharedFactor& factor, factors) {
if(factor)
factor->print(str + "| ");
else
@ -77,7 +78,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph,
const VariableIndexUnordered& structure, const std::vector<Key>& order)
const VariableIndexUnordered& structure, const OrderingUnordered& order)
{
gttic(ET_Create1);
@ -156,7 +157,7 @@ namespace gtsam {
/* ************************************************************************* */
template<class BAYESNET, class GRAPH>
EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(
const FactorGraphType& factorGraph, const std::vector<Key>& order)
const FactorGraphType& factorGraph, const OrderingUnordered& order)
{
gttic(ET_Create2);
// Build variable index first

5
gtsam/inference/EliminationTreeUnordered.h
View File

@ -27,6 +27,7 @@ class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
namespace gtsam {
class VariableIndexUnordered;
class OrderingUnordered;
/**
* An elimination tree is a data structure used intermediately during
@ -98,14 +99,14 @@ namespace gtsam {
* @return The elimination tree
*/
EliminationTreeUnordered(const FactorGraphType& factorGraph,
const VariableIndexUnordered& structure, const std::vector<Key>& order);
const VariableIndexUnordered& structure, const OrderingUnordered& order);
/** Build the elimination tree of a factor graph. Note that this has to compute the column
* structure as a VariableIndex, so if you already have this precomputed, use the other
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order);
EliminationTreeUnordered(const FactorGraphType& factorGraph, const OrderingUnordered& order);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */

2
gtsam/inference/FactorGraphUnordered.h
View File

@ -60,7 +60,7 @@ namespace gtsam {
public:
/// @name Standard Constructor
/// @name Standard Constructors
/// @{
/** Default constructor */

151
gtsam/inference/JunctionTreeUnordered-inst.h
View File

@ -26,47 +26,19 @@
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
namespace gtsam {
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedFactor
JunctionTreeUnordered<BAYESTREE,GRAPH>::Node::eliminate(
const boost::shared_ptr<BayesTreeType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
{
// This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
assert(childrenResults.size() == children.size());
// Gather factors
std::vector<sharedFactor> gatheredFactors;
gatheredFactors.reserve(factors.size() + children.size());
gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end());
gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end());
// Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, keys);
// Add conditional to BayesNet
output->push_back(eliminationResult.first);
// Return result
return eliminationResult.second;
}
namespace {
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
struct ConstructorTraversalData {
const ConstructorTraversalData* parentData;
ConstructorTraversalData* const parentData;
typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
std::vector<SymbolicConditionalUnordered::shared_ptr> childSymbolicConditionals;
std::vector<SymbolicFactorUnordered::shared_ptr> childSymbolicFactors;
ConstructorTraversalData(const ConstructorTraversalData* _parentData) : parentData(_parentData) {}
ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {}
};
/* ************************************************************************* */
@ -74,12 +46,12 @@ namespace gtsam {
template<class BAYESTREE, class GRAPH, class ETREE_NODE>
ConstructorTraversalData<BAYESTREE,GRAPH> ConstructorTraversalVisitorPre(
const boost::shared_ptr<ETREE_NODE>& node,
const ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
{
// On the pre-order pass, before children have been visited, we just set up a traversal data
// structure with its own JT node, and create a child pointer in its parent.
ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node>();
myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node>();
myData.myJTNode->keys.push_back(node->key);
myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
parentData.myJTNode->children.push_back(myData.myJTNode);
@ -93,17 +65,27 @@ namespace gtsam {
const boost::shared_ptr<ETREE_NODE>& node,
const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
{
// In this post-order visitor, we combine the symbolic elimination results from the
// elimination tree children and symbolically eliminate the current elimination tree node. We
// then check whether each of our elimination tree child nodes should be merged with us. The
// check for this is that our number of symbolic elimination parents is exactly 1 less than
// our child's symbolic elimination parents - this condition indicates that eliminating the
// current node did not introduce any parents beyond those already in the child.
// Do symbolic elimination for this node
std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size());
// Add symbolic versions of the ETree node factors
BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) {
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(factor)); }
symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
SymbolicFactorUnordered::FromKeys(*factor))); }
// Add symbolic factors passed up from children
symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
std::vector<Key> keyAsVector(1); keyAsVector[0] = node->key;
std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
// Store symbolic elimination results
// Store symbolic elimination results in the parent
myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
@ -118,7 +100,7 @@ namespace gtsam {
if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
// Get a reference to the child, adjusting the index to account for children previously
// merged and removed from the child list.
const typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node& childToMerge =
const typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node& childToMerge =
*myData.myJTNode->children[child - nrMergedChildren];
// Merge keys, factors, and children.
myData.myJTNode->keys.insert(myData.myJTNode->keys.end(), childToMerge.keys.begin(), childToMerge.keys.end());
@ -129,12 +111,77 @@ namespace gtsam {
}
}
}
/* ************************************************************************* */
// Elimination traversal data - stores a pointer to the parent data and collects the factors
// resulting from elimination of the children. Also sets up BayesTree cliques with parent and
// child pointers.
template<class JUNCTIONTREE>
struct EliminationData {
EliminationData* const parentData;
std::vector<typename JUNCTIONTREE::sharedFactor> childFactors;
boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode;
EliminationData(EliminationData* _parentData, size_t nChildren) :
parentData(_parentData),
bayesTreeNode(boost::make_shared<typename JUNCTIONTREE::BayesTreeType::Node>()) {
childFactors.reserve(nChildren);
// Set up BayesTree parent and child pointers
if(parentData) {
bayesTreeNode->parent_ = parentData->bayesTreeNode;
parentData->bayesTreeNode->children.push_back(bayesTreeNode);
}
}
};
/* ************************************************************************* */
// Elimination pre-order visitor - just creates the EliminationData structure for the visited
// node.
template<class JUNCTIONTREE>
EliminationData<JUNCTIONTREE> eliminationPreOrderVisitor(
const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& parentData)
{
return EliminationData<JUNCTIONTREE>(&parentData, node->children.size());
}
/* ************************************************************************* */
// Elimination post-order visitor - combine the child factors with our own factors, add the
// resulting conditional to the BayesTree, and add the remaining factor to the parent. The
// extra argument 'eliminationFunction' is passed from JunctionTree::eliminate using
// boost::bind.
template<class JUNCTIONTREE>
void eliminationPostOrderVisitor(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData,
const typename JUNCTIONTREE::Eliminate& eliminationFunction)
{
// Typedefs
typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
typedef typename JUNCTIONTREE::FactorType FactorType;
typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
// Gather factors
assert(node->children.size() == myData.childFactors.size());
std::vector<sharedFactor> gatheredFactors;
gatheredFactors.reserve(node->factors.size() + node->children.size());
gatheredFactors.insert(gatheredFactors.end(), node->factors.begin(), node->factors.end());
gatheredFactors.insert(gatheredFactors.end(), myData.childFactors.begin(), myData.childFactors.end());
// Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, node->keys);
// Store conditional in BayesTree clique
myData.bayesTreeNode->conditional_ = eliminationResult.first;
// Store remaining factor in parent's gathered factors
myData.parentData->childFactors.push_back(eliminationResult.second);
}
}
/* ************************************************************************* */
template<class BAYESTREE, class GRAPH>
template<class ETREE>
JunctionTreeUnordered<BAYESTREE,GRAPH>::JunctionTreeUnordered(const ETREE& eliminationTree)
JunctionTreeUnordered<BAYESTREE,GRAPH>
JunctionTreeUnordered<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
{
// Here we rely on the BayesNet having been produced by this elimination tree, such that the
// conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect
@ -144,16 +191,20 @@ namespace gtsam {
// Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather
// the created junction tree roots in a dummy Node.
typedef typename ETREE::Node ETreeNode;
ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
rootData.myJTNode = boost::make_shared<Node>(); // Make a dummy node to gather the junction tree roots
treeTraversal.DepthFirstForest(eliminationTree, rootData,
ConstructorTraversalVisitorPre<BAYESREE,GRAPH>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH>);
treeTraversal::DepthFirstForest(eliminationTree, rootData,
ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
// Assign roots from the dummy node
roots_ = rootData.myJTNode->children;
This result;
result.roots_ = rootData.myJTNode->children;
// Transfer remaining factors from elimination tree
remainingFactors_ = eliminationTree.remainingFactors();
result.remainingFactors_ = eliminationTree.remainingFactors();
return result;
}
/* ************************************************************************* */
@ -175,16 +226,22 @@ namespace gtsam {
std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
JunctionTreeUnordered<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
{
// Allocate result
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
// Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
// that contains all of the roots as its children. rootsContainer also stores the remaining
// uneliminated factors passed up from the roots.
EliminationData<This> rootsContainer(0, roots_.size());
treeTraversal::DepthFirstForest(*this, rootsContainer, eliminationPreOrderVisitor<This>,
boost::bind(eliminationPostOrderVisitor<This>, _1, _2, function));
// Run tree elimination algorithm
std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
// Create BayesTree from roots stored in the dummy BayesTree node.
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
BOOST_FOREACH(const typename BayesTreeType::sharedNode& root, rootsContainer.bayesTreeNode->children)
result->insertRoot(root);
// Add remaining factors that were not involved with eliminated variables
boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
allRemainingFactors->push_back(rootsContainer.childFactors.begin(), rootsContainer.childFactors.end());
// Return result
return std::make_pair(result, allRemainingFactors);

16
gtsam/inference/JunctionTreeUnordered.h
View File

@ -71,9 +71,6 @@ namespace gtsam {
Keys keys; ///< Frontal keys of this node
Factors factors; ///< Factors associated with this node
Children children; ///< sub-trees
sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
};
typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
@ -83,17 +80,17 @@ namespace gtsam {
/** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
std::vector<sharedNode>& roots_;
std::vector<sharedNode> roots_;
std::vector<sharedFactor> remainingFactors_;
public:
protected:
/// @name Standard Constructors
/// @{
/** Build the junction tree from an elimination tree. */
template<class ETREE>
JunctionTreeUnordered(const ETREE& eliminationTree);
static This FromEliminationTree(const ETREE& eliminationTree);
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
@ -105,6 +102,8 @@ namespace gtsam {
/// @}
public:
/// @name Standard Interface
/// @{
@ -129,6 +128,11 @@ namespace gtsam {
/// @}
private:
// Private default constructor (used in static construction methods)
JunctionTreeUnordered() {}
};
}

95
gtsam/inference/OrderingUnordered.cpp Normal file
View File

@ -0,0 +1,95 @@
/* ----------------------------------------------------------------------------
* 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 Ordering.cpp
* @author Richard Roberts
* @date Sep 2, 2010
*/
#pragma once
#include <vector>
#include <limits>
#include <gtsam/inference/OrderingUnordered.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
OrderingUnordered OrderingUnordered::COLAMD(const VariableIndexUnordered& variableIndex)
{
gttic(OrderingUnordered_COLAMD);
gttic(Prepare);
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
vector<int> cmember(nVars, 0); // For now, no constraints
// Fill in input data for COLAMD
p[0] = 0;
int count = 0;
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
size_t index = 0;
BOOST_FOREACH(const VariableIndexUnordered::const_iterator::value_type key_factors, variableIndex) {
// Arrange factor indices into COLAMD format
const VariableIndexUnordered::Factors& column = key_factors.second;
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(size_t factorIndex, column) {
if(lastFactorId != numeric_limits<size_t>::max())
assert(factorIndex > lastFactorId);
A[count++] = (int)factorIndex; // copy sparse column
}
p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
// Store key in array and increment index
keys[index] = key_factors.first;
++ index;
}
assert((size_t)count == variableIndex.nEntries());
//double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
double knobs[CCOLAMD_KNOBS];
ccolamd_set_defaults(knobs);
knobs[CCOLAMD_DENSE_ROW]=-1;
knobs[CCOLAMD_DENSE_COL]=-1;
int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
gttoc(Prepare);
// call colamd, result will be in p
/* returns (1) if successful, (0) otherwise*/
if(nVars > 0) {
gttic(ccolamd);
int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
if(rv != 1)
throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
}
// ccolamd_report(stats);
gttic(Fill_Ordering);
// Convert elimination ordering in p to an ordering
OrderingUnordered result;
result.assign(p.begin(), p.end() - 1);
gttoc(Fill_Ordering);
return result;
}
}

17
gtsam/inference/OrderingUnordered.h
View File

@ -20,10 +20,23 @@
#include <vector>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndexUnordered.h>
#include <gtsam/inference/FactorGraphUnordered.h>
namespace gtsam {
class OrderingUnordered : std::vector<Key> {
class OrderingUnordered : public std::vector<Key> {
public:
OrderingUnordered() {}
/// Create an empty ordering
GTSAM_EXPORT OrderingUnordered() {}
/// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
/// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
/// VariableIndexUnordered&)
template<class FACTOR>
static OrderingUnordered COLAMD(const FactorGraphUnordered<FACTOR>& graph) {
return COLAMD(VariableIndexUnordered(graph));
}
static GTSAM_EXPORT OrderingUnordered COLAMD(const VariableIndexUnordered& variableIndex);
};
}

5
gtsam/inference/inference-inst.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/bind.hpp>
#include <utility>
#include <gtsam/base/treeTraversal-inst.h>
@ -49,11 +50,11 @@ namespace gtsam {
/* ************************************************************************* */
template<class TREE, class RESULT>
void eliminationPostOrderVisitor(const typename TREE::Node& const node, EliminationData<TREE>& myData,
void eliminationPostOrderVisitor(const typename TREE::sharedNode& node, EliminationData<TREE>& myData,
RESULT& result, const typename TREE::Eliminate& eliminationFunction)
{
// Call eliminate on the node and add the result to the parent's gathered factors
myData.parentData->childFactors.push_back(node.eliminate(result, eliminationFunction, myData.childFactors));
myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors));
}
}

1
gtsam/symbolic/SymbolicBayesNetUnordered.cpp
View File

@ -16,6 +16,7 @@
* @author Richard Roberts
*/
#include <gtsam/inference/BayesNetUnordered-inst.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
namespace gtsam {

2
gtsam/symbolic/SymbolicBayesNetUnordered.h
View File

@ -27,7 +27,7 @@ namespace gtsam {
/** Symbolic Bayes Net
* \nosubgrouping
*/
class SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
class GTSAM_EXPORT SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
public:

144
gtsam/symbolic/SymbolicBayesTreeUnordered.cpp
View File

@ -19,80 +19,86 @@
#include <boost/foreach.hpp>
#include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeUnordered-inst.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h>
namespace gtsam {
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional)
{
static const bool debug = false;
void SymbolicBayesTreeUnordered::noop() const {
// get indices and parents
const typename CONDITIONAL::Parents& parents = conditional->parents();
if(debug) conditional->print("Adding conditional ");
// if no parents, start a new root clique
if (parents.empty()) {
if(debug) std::cout << "No parents so making root" << std::endl;
bayesTree.root_ = bayesTree.addClique(conditional);
return;
}
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Index parentRepresentative = bayesTree.findParentClique(parents);
if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
sharedClique parent_clique = bayesTree[parentRepresentative];
if(debug) parent_clique->print("Parent clique is ");
// if the parents and parent clique have the same size, add to parent clique
if ((*parent_clique)->size() == size_t(parents.size())) {
if(debug) std::cout << "Adding to parent clique" << std::endl;
#ifndef NDEBUG
// Debug check that the parent indices of the new conditional match the indices
// currently in the clique.
// list<Index>::const_iterator parent = parents.begin();
// typename Clique::const_iterator cond = parent_clique->begin();
// while(parent != parents.end()) {
// assert(cond != parent_clique->end());
// assert(*parent == (*cond)->key());
// ++ parent;
// ++ cond;
// }
#endif
addToCliqueFront(bayesTree, conditional, parent_clique);
} else {
if(debug) std::cout << "Starting new clique" << std::endl;
// otherwise, start a new clique and add it to the tree
bayesTree.addClique(conditional,parent_clique);
}
}
/* ************************************************************************* */
void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
static const bool debug = false;
#ifndef NDEBUG
// Debug check to make sure the conditional variable is ordered lower than
// its parents and that all of its parents are present either in this
// clique or its separator.
BOOST_FOREACH(Key parent, conditional->parents()) {
assert(parent > conditional->lastFrontalKey());
const Clique& cliquer(*clique);
assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
}
#endif
if(debug) conditional->print("Adding conditional ");
if(debug) clique->print("To clique ");
Index j = conditional->lastFrontalKey();
this->nodes_.resize(std::max(j+1, this->nodes_.size()));
this->nodes_[j] = clique;
FastVector<Index> newIndices(clique->conditional()->size() + 1);
newIndices[0] = j;
std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
if(debug) clique->print("Expanded clique is ");
clique->assertInvariants();
}
// /* ************************************************************************* */
// void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional)
// {
// static const bool debug = false;
//
// // get indices and parents
// const SymbolicConditionalUnordered::Parents& parents = conditional->parents();
//
// if(debug) conditional->print("Adding conditional ");
//
// // if no parents, start a new root clique
// if (parents.empty()) {
// if(debug) std::cout << "No parents so making root" << std::endl;
// addClique(boost::make_shared<SymbolicBayesTreeCliqueUnordered>(conditional));
// return;
// }
//
// // otherwise, find the parent clique by using the index data structure
// // to find the lowest-ordered parent
// Index parentRepresentative = findParentClique(parents);
// if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
// sharedClique parent_clique = bayesTree[parentRepresentative];
// if(debug) parent_clique->print("Parent clique is ");
//
// // if the parents and parent clique have the same size, add to parent clique
// if ((*parent_clique)->size() == size_t(parents.size())) {
// if(debug) std::cout << "Adding to parent clique" << std::endl;
//#ifndef NDEBUG
// // Debug check that the parent indices of the new conditional match the indices
// // currently in the clique.
// // list<Index>::const_iterator parent = parents.begin();
// // typename Clique::const_iterator cond = parent_clique->begin();
// // while(parent != parents.end()) {
// // assert(cond != parent_clique->end());
// // assert(*parent == (*cond)->key());
// // ++ parent;
// // ++ cond;
// // }
//#endif
// addToCliqueFront(bayesTree, conditional, parent_clique);
// } else {
// if(debug) std::cout << "Starting new clique" << std::endl;
// // otherwise, start a new clique and add it to the tree
// bayesTree.addClique(conditional,parent_clique);
// }
// }
//
// /* ************************************************************************* */
// void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
// static const bool debug = false;
//#ifndef NDEBUG
// // Debug check to make sure the conditional variable is ordered lower than
// // its parents and that all of its parents are present either in this
// // clique or its separator.
// BOOST_FOREACH(Key parent, conditional->parents()) {
// assert(parent > conditional->lastFrontalKey());
// const Clique& cliquer(*clique);
// assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
// }
//#endif
// if(debug) conditional->print("Adding conditional ");
// if(debug) clique->print("To clique ");
// Index j = conditional->lastFrontalKey();
// this->nodes_.resize(std::max(j+1, this->nodes_.size()));
// this->nodes_[j] = clique;
// FastVector<Index> newIndices(clique->conditional()->size() + 1);
// newIndices[0] = j;
// std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
// clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
// if(debug) clique->print("Expanded clique is ");
// clique->assertInvariants();
// }
}

32
gtsam/symbolic/SymbolicBayesTreeUnordered.h
View File

@ -19,18 +19,33 @@
#pragma once
#include <gtsam/inference/BayesTreeUnordered.h>
#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
#include <gtsam/inference/BayesTreeCliqueDefault.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
namespace gtsam {
class SymbolicBayesTreeUnordered :
public BayesTreeUnordered<BayesTreeCliqueBaseUnordered<>
/* ************************************************************************* */
class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered :
public BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered>
{
public:
typedef SymbolicBayesTreeCliqueUnordered This;
typedef BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
SymbolicBayesTreeCliqueUnordered() {}
SymbolicBayesTreeCliqueUnordered(const SymbolicConditionalUnordered::shared_ptr& conditional) : Base(conditional) {}
};
/* ************************************************************************* */
class GTSAM_EXPORT SymbolicBayesTreeUnordered :
public BayesTreeUnordered<SymbolicBayesTreeCliqueUnordered>
{
public:
/** Insert a new conditional */
void insert(const sharedConditional& conditional);
//void insert(const sharedConditional& conditional);
protected:
@ -39,7 +54,12 @@ namespace gtsam {
* parents are already in the clique or its separators. This function does
* not check for this condition, it just updates the data structures.
*/
void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
//void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
private:
// Dummy method to export class
void noop() const;
};

17
gtsam/symbolic/SymbolicConditionalUnordered.h
View File

@ -50,16 +50,16 @@ namespace gtsam {
SymbolicConditionalUnordered() {}
/** No parents */
SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(0) {}
SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(1) {}
/** Single parent */
SymbolicConditionalUnordered(Index j, Index parent) : BaseFactor(j, parent), BaseConditional(1) {}
/** Two parents */
SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(2) {}
SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {}
/** Three parents */
SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {}
SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {}
/** Named constructor from an arbitrary number of keys and frontals */
template<typename ITERATOR>
@ -77,6 +77,17 @@ namespace gtsam {
/// @}
/// @name Testable
/** Print with optional formatter */
void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
BaseConditional::print(str, keyFormatter); }
/** Check equality */
bool equals(const This& c, double tol = 1e-9) const { return BaseConditional::equals(c); }
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;

6
gtsam/symbolic/SymbolicEliminationTreeUnordered.h
View File

@ -16,6 +16,8 @@
* @author Richard Roberts
*/
#pragma once
#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
#include <gtsam/inference/EliminationTreeUnordered.h>
@ -38,7 +40,7 @@ namespace gtsam {
* @return The elimination tree
*/
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph,
const VariableIndexUnordered& structure, const std::vector<Key>& order) :
const VariableIndexUnordered& structure, const OrderingUnordered& order) :
Base(factorGraph, structure, order) {}
/** Build the elimination tree of a factor graph. Note that this has to compute the column
@ -46,7 +48,7 @@ namespace gtsam {
* constructor instead.
* @param factorGraph The factor graph for which to build the elimination tree
*/
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector<Key>& order) :
SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order) :
Base(factorGraph, order) {}
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are

14
gtsam/symbolic/SymbolicFactorGraphUnordered.h
View File

@ -24,9 +24,9 @@
#include <gtsam/symbolic/SymbolicFactorUnordered.h>
namespace gtsam { class SymbolicConditionalUnordered; }
namespace gtsam { class SymbolicBayesNet; }
namespace gtsam { class SymbolicBayesNetUnordered; }
namespace gtsam { class SymbolicEliminationTreeUnordered; }
namespace gtsam { class SymbolicBayesTree; }
namespace gtsam { class SymbolicBayesTreeUnordered; }
namespace gtsam { class SymbolicJunctionTreeUnordered; }
namespace gtsam {
@ -37,18 +37,16 @@ namespace gtsam {
class GTSAM_EXPORT SymbolicFactorGraphUnordered:
public FactorGraphUnordered<SymbolicFactorUnordered>,
public EliminateableFactorGraph<
SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered,
SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered>
{
public:
typedef SymbolicFactorGraphUnordered This;
typedef FactorGraphUnordered<SymbolicFactorUnordered> Base;
typedef EliminateableFactorGraph<
SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
BaseEliminateable;
SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered,
SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> BaseEliminateable;
typedef BaseEliminateable::Eliminate Eliminate;
/// @name Standard Constructors

2
gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp
View File

@ -21,7 +21,7 @@
namespace gtsam {
SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered() {
void SymbolicJunctionTreeUnordered::noop() const {
}

6
gtsam/symbolic/SymbolicJunctionTreeUnordered.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
* @return The elimination tree
*/
SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree) :
Base(eliminationTree) {}
Base(Base::FromEliminationTree(eliminationTree)) {}
/** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
* copied, factors are not cloned. */
@ -51,8 +51,8 @@ namespace gtsam {
private:
/// Private default constructor
SymbolicJunctionTreeUnordered();
// Dummy method to export class type
void noop() const;
};
}

6
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -77,7 +77,7 @@ TEST(EliminationTree, Create)
SymbolicEliminationTreeUnordered expected = EliminationTreeUnorderedTester::buildHardcodedTree(fg);
// Build from factor graph
vector<size_t> order;
OrderingUnordered order;
order += 0,1,2,3,4;
SymbolicEliminationTreeUnordered actual(fg, order);
@ -108,7 +108,7 @@ TEST_UNSAFE(EliminationTree, eliminate )
fg.push_factor(3, 4);
// eliminate
vector<size_t> order;
OrderingUnordered order;
order += 0,1,2,3,4;
SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first;
@ -131,7 +131,7 @@ TEST(EliminationTree, disconnected_graph) {
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
vector<size_t> order;
OrderingUnordered order;
order += 0,1,2,3,4;
SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;