125 lines
3.6 KiB
C++
125 lines
3.6 KiB
C++
/**
|
|
* @file testIncremental.cpp
|
|
* @brief Unit tests for graph-based iSAM
|
|
* @author Michael Kaess
|
|
* @author Viorela Ila
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <boost/assign/std/list.hpp> // for operator +=
|
|
using namespace boost::assign;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "SymbolicBayesNet.h"
|
|
#include "SymbolicFactorGraph.h"
|
|
#include "GaussianBayesNet.h"
|
|
#include "Ordering.h"
|
|
#include "BayesTree-inl.h"
|
|
#include "smallExample.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
|
|
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
|
|
|
/* ************************************************************************* */
|
|
|
|
void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor>& newFactors) {
|
|
|
|
// Remove the contaminated part of the Bayes tree
|
|
FactorGraph<SymbolicFactor> factors;
|
|
SymbolicBayesTree::Cliques orphans;
|
|
BOOST_FOREACH(boost::shared_ptr<SymbolicFactor> factor, newFactors) {
|
|
|
|
FactorGraph<SymbolicFactor> factors1;
|
|
SymbolicBayesTree::Cliques orphans1;
|
|
boost::tie(factors1, orphans1) = bayesTree.removeTop<SymbolicFactor>(factor);
|
|
|
|
factors.push_back(factors1);
|
|
orphans.splice (orphans.begin(), orphans1);
|
|
}
|
|
|
|
// add the factors themselves
|
|
factors.push_back(newFactors);
|
|
|
|
// create an ordering for the new and contaminated factors
|
|
Ordering ordering = factors.getOrdering();
|
|
|
|
// eliminate into a Bayes net
|
|
SymbolicBayesNet bayesNet = eliminate<SymbolicFactor,SymbolicConditional>(factors,ordering);
|
|
|
|
// insert conditionals back in, straight into the topless bayesTree
|
|
SymbolicBayesNet::const_reverse_iterator rit;
|
|
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
|
bayesTree.insert(*rit);
|
|
|
|
// add orphans to the bottom of the new tree
|
|
BOOST_FOREACH(SymbolicBayesTree::sharedClique orphan, orphans) {
|
|
string key = orphan->separator_.front(); // todo: assumes there is a separator...
|
|
SymbolicBayesTree::sharedClique parent = bayesTree[key];
|
|
parent->children_ += orphan;
|
|
}
|
|
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( BayesTree, iSAM )
|
|
{
|
|
// Conditionals for ASIA example from the tutorial with A and D evidence
|
|
SymbolicConditional::shared_ptr
|
|
B(new SymbolicConditional("B")),
|
|
L(new SymbolicConditional("L", "B")),
|
|
E(new SymbolicConditional("E", "B", "L")),
|
|
S(new SymbolicConditional("S", "L", "B")),
|
|
T(new SymbolicConditional("T", "E", "L")),
|
|
X(new SymbolicConditional("X", "E"));
|
|
|
|
// Create using insert
|
|
SymbolicBayesTree bayesTree;
|
|
bayesTree.insert(B);
|
|
bayesTree.insert(L);
|
|
bayesTree.insert(E);
|
|
bayesTree.insert(S);
|
|
bayesTree.insert(T);
|
|
bayesTree.insert(X);
|
|
|
|
// Now we modify the Bayes tree by inserting a new factor over B and S
|
|
|
|
// New conditionals in modified top of the tree
|
|
SymbolicConditional::shared_ptr
|
|
S_(new SymbolicConditional("S")),
|
|
L_(new SymbolicConditional("L", "S")),
|
|
E_(new SymbolicConditional("E", "L", "S")),
|
|
B_(new SymbolicConditional("B", "E", "L", "S"));
|
|
|
|
// Create expected Bayes tree
|
|
SymbolicBayesTree expected;
|
|
expected.insert(S_);
|
|
expected.insert(L_);
|
|
expected.insert(E_);
|
|
expected.insert(B_);
|
|
expected.insert(T);
|
|
expected.insert(X);
|
|
|
|
// create new factors to be inserted
|
|
SymbolicFactorGraph factorGraph;
|
|
factorGraph.push_factor("B","S");
|
|
factorGraph.push_factor("B");
|
|
|
|
// do incremental inference
|
|
update(bayesTree, factorGraph);
|
|
|
|
// Check whether the same
|
|
CHECK(assert_equal(expected,bayesTree));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|