180 lines
5.7 KiB
C++
180 lines
5.7 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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/inference/Symbol.h>
|
|
#include <gtsam/symbolic/SymbolicEliminationTree.h>
|
|
|
|
#include <boost/make_shared.hpp>
|
|
#include <vector>
|
|
|
|
#include "symbolicExampleGraphs.h"
|
|
|
|
using namespace gtsam;
|
|
using namespace gtsam::symbol_shorthand;
|
|
using namespace std;
|
|
using sharedNode = SymbolicEliminationTree::sharedNode;
|
|
|
|
class EliminationTreeTester {
|
|
public:
|
|
// build hardcoded tree
|
|
static SymbolicEliminationTree buildHardcodedTree(
|
|
const SymbolicFactorGraph& fg) {
|
|
sharedNode leaf0(new SymbolicEliminationTree::Node);
|
|
leaf0->key = 0;
|
|
leaf0->factors.push_back(fg[0]);
|
|
leaf0->factors.push_back(fg[1]);
|
|
|
|
sharedNode node1(new SymbolicEliminationTree::Node);
|
|
node1->key = 1;
|
|
node1->factors.push_back(fg[2]);
|
|
node1->children.push_back(leaf0);
|
|
|
|
sharedNode node2(new SymbolicEliminationTree::Node);
|
|
node2->key = 2;
|
|
node2->factors.push_back(fg[3]);
|
|
node2->children.push_back(node1);
|
|
|
|
sharedNode leaf3(new SymbolicEliminationTree::Node);
|
|
leaf3->key = 3;
|
|
leaf3->factors.push_back(fg[4]);
|
|
|
|
sharedNode root(new SymbolicEliminationTree::Node);
|
|
root->key = 4;
|
|
root->children.push_back(leaf3);
|
|
root->children.push_back(node2);
|
|
|
|
SymbolicEliminationTree tree;
|
|
tree.roots_.push_back(root);
|
|
return tree;
|
|
}
|
|
|
|
static SymbolicEliminationTree MakeTree(
|
|
const std::vector<sharedNode>& roots) {
|
|
SymbolicEliminationTree et;
|
|
et.roots_.assign(roots.begin(), roots.end());
|
|
return et;
|
|
}
|
|
};
|
|
|
|
template <typename FACTORS>
|
|
static sharedNode MakeNode(Key key, const FACTORS& factors) {
|
|
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
|
|
node->key = key;
|
|
SymbolicFactorGraph factorsAsGraph = factors;
|
|
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
|
|
return node;
|
|
}
|
|
|
|
template <typename FACTORS>
|
|
static sharedNode MakeNode(Key key, const FACTORS& factors,
|
|
const std::vector<sharedNode>& children) {
|
|
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
|
|
node->key = key;
|
|
SymbolicFactorGraph factorsAsGraph = factors;
|
|
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
|
|
node->children.assign(children.begin(), children.end());
|
|
return node;
|
|
}
|
|
|
|
template <typename Class>
|
|
class ListOf {
|
|
public:
|
|
ListOf(Class&& c) { result.push_back(c); }
|
|
|
|
ListOf& operator()(Class&& c) {
|
|
result.push_back(c);
|
|
return *this;
|
|
}
|
|
|
|
operator std::vector<Class>() { return result; }
|
|
|
|
private:
|
|
std::vector<Class> result;
|
|
};
|
|
|
|
using Nodes = ListOf<sharedNode>;
|
|
|
|
/* ************************************************************************* */
|
|
TEST(EliminationTree, Create) {
|
|
SymbolicEliminationTree expected =
|
|
EliminationTreeTester::buildHardcodedTree(simpleTestGraph1);
|
|
|
|
// Build from factor graph
|
|
Ordering order;
|
|
order += 0, 1, 2, 3, 4;
|
|
SymbolicEliminationTree actual(simpleTestGraph1, order);
|
|
|
|
CHECK(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(EliminationTree, Create2) {
|
|
// l1 l2
|
|
// / | / |
|
|
// x1 --- x2 --- x3 --- x4 --- x5
|
|
// \ |
|
|
// l3
|
|
SymbolicFactorGraph graph;
|
|
graph += SymbolicFactor(X(1), L(1));
|
|
graph += SymbolicFactor(X(1), X(2));
|
|
graph += SymbolicFactor(X(2), L(1));
|
|
graph += SymbolicFactor(X(2), X(3));
|
|
graph += SymbolicFactor(X(3), X(4));
|
|
graph += SymbolicFactor(X(4), L(2));
|
|
graph += SymbolicFactor(X(4), X(5));
|
|
graph += SymbolicFactor(L(2), X(5));
|
|
graph += SymbolicFactor(X(4), L(3));
|
|
graph += SymbolicFactor(X(5), L(3));
|
|
|
|
SymbolicEliminationTree expected =
|
|
EliminationTreeTester::MakeTree(Nodes(MakeNode(
|
|
X(3), SymbolicFactorGraph(),
|
|
Nodes(MakeNode(
|
|
X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))),
|
|
Nodes(MakeNode(
|
|
L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))),
|
|
Nodes(MakeNode(
|
|
X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))(
|
|
SymbolicFactor(X(1), X(2)))))))))(
|
|
MakeNode(
|
|
X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))),
|
|
Nodes(MakeNode(
|
|
L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))),
|
|
Nodes(MakeNode(
|
|
X(5),
|
|
SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))(
|
|
SymbolicFactor(L(2), X(5))),
|
|
Nodes(MakeNode(
|
|
L(3),
|
|
SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))(
|
|
SymbolicFactor(X(5), L(3))))))))))))));
|
|
|
|
const Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)};
|
|
SymbolicEliminationTree actual(graph, order);
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|