GaussianBayesTree added, testBayesTree split
Michael Kaess
parent
a4a552ea86
commit
4200271cf4
|
|
@ -0,0 +1,26 @@
|
||||||
|
/**
|
||||||
|
* @file GaussianBayesTree
|
||||||
|
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||||
|
* @author Michael Kaess
|
||||||
|
*/
|
||||||
|
|
||||||
|
// \callgraph
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <map>
|
||||||
|
#include <list>
|
||||||
|
#include <vector>
|
||||||
|
#include <boost/serialization/map.hpp>
|
||||||
|
#include <boost/serialization/list.hpp>
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
#include "Testable.h"
|
||||||
|
#include "BayesTree.h"
|
||||||
|
#include "GaussianConditional.h"
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
||||||
|
|
||||||
|
} /// namespace gtsam
|
||||||
|
|
@ -63,13 +63,16 @@ example = smallExample.cpp
|
||||||
# Inference
|
# Inference
|
||||||
headers += inference.h inference-inl.h
|
headers += inference.h inference-inl.h
|
||||||
headers += FactorGraph.h FactorGraph-inl.h
|
headers += FactorGraph.h FactorGraph-inl.h
|
||||||
headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h
|
headers += BayesNet.h BayesNet-inl.h
|
||||||
check_PROGRAMS += testFactorgraph testBayesTree testInference testOrdering
|
headers += BayesTree.h BayesTree-inl.h GaussianBayesTree.h
|
||||||
|
check_PROGRAMS += testFactorgraph testBayesTree testGaussianBayesTree testInference testOrdering
|
||||||
testFactorgraph_SOURCES = testFactorgraph.cpp
|
testFactorgraph_SOURCES = testFactorgraph.cpp
|
||||||
testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
||||||
|
testGaussianBayesTree_SOURCES = $(example) testGaussianBayesTree.cpp
|
||||||
testInference_SOURCES = $(example) testInference.cpp
|
testInference_SOURCES = $(example) testInference.cpp
|
||||||
testFactorgraph_LDADD = libgtsam.la
|
testFactorgraph_LDADD = libgtsam.la
|
||||||
testBayesTree_LDADD = libgtsam.la
|
testBayesTree_LDADD = libgtsam.la
|
||||||
|
testGaussianBayesTree_LDADD = libgtsam.la
|
||||||
testInference_LDADD = libgtsam.la
|
testInference_LDADD = libgtsam.la
|
||||||
testOrdering_SOURCES = testOrdering.cpp
|
testOrdering_SOURCES = testOrdering.cpp
|
||||||
testOrdering_LDADD = libgtsam.la
|
testOrdering_LDADD = libgtsam.la
|
||||||
|
|
|
||||||
|
|
@ -4,7 +4,7 @@
|
||||||
* @author Frank Dellaert
|
* @author Frank Dellaert
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "inference.h"
|
//#include "inference.h"
|
||||||
#include "FactorGraph-inl.h"
|
#include "FactorGraph-inl.h"
|
||||||
#include "BayesNet-inl.h"
|
#include "BayesNet-inl.h"
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -12,7 +12,6 @@ using namespace boost::assign;
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
#include "SymbolicBayesNet.h"
|
#include "SymbolicBayesNet.h"
|
||||||
#include "GaussianBayesNet.h"
|
|
||||||
#include "SymbolicFactorGraph.h"
|
#include "SymbolicFactorGraph.h"
|
||||||
#include "Ordering.h"
|
#include "Ordering.h"
|
||||||
#include "BayesTree-inl.h"
|
#include "BayesTree-inl.h"
|
||||||
|
|
@ -21,7 +20,7 @@ using namespace boost::assign;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
|
typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
|
||||||
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
// SLAM example from RSS sqrtSAM paper
|
// SLAM example from RSS sqrtSAM paper
|
||||||
SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
|
SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
|
||||||
|
|
@ -110,230 +109,6 @@ TEST( BayesTree, constructor )
|
||||||
CHECK(assert_equal(bayesTree,bayesTree2));
|
CHECK(assert_equal(bayesTree,bayesTree2));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
// Some numbers that should be consistent among all smoother tests
|
|
||||||
|
|
||||||
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
|
||||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
|
||||||
|
|
||||||
/* ************************************************************************* *
|
|
||||||
Bayes tree for smoother with "natural" ordering:
|
|
||||||
C1 x6 x7
|
|
||||||
C2 x5 : x6
|
|
||||||
C3 x4 : x5
|
|
||||||
C4 x3 : x4
|
|
||||||
C5 x2 : x3
|
|
||||||
C6 x1 : x2
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, linear_smoother_shortcuts )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
Ordering ordering;
|
|
||||||
for (int t = 1; t <= 7; t++)
|
|
||||||
ordering.push_back(symbol('x', t));
|
|
||||||
|
|
||||||
// eliminate using the "natural" ordering
|
|
||||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
|
||||||
|
|
||||||
// Create the Bayes tree
|
|
||||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
|
||||||
LONGS_EQUAL(6,bayesTree.size());
|
|
||||||
|
|
||||||
// Check the conditional P(Root|Root)
|
|
||||||
GaussianBayesNet empty;
|
|
||||||
GaussianBayesTree::sharedClique R = bayesTree.root();
|
|
||||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(empty,actual1,1e-4));
|
|
||||||
|
|
||||||
// Check the conditional P(C2|Root)
|
|
||||||
GaussianBayesTree::sharedClique C2 = bayesTree["x5"];
|
|
||||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(empty,actual2,1e-4));
|
|
||||||
|
|
||||||
// Check the conditional P(C3|Root)
|
|
||||||
Vector sigma3 = repeat(2, 0.61808);
|
|
||||||
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
|
|
||||||
GaussianBayesNet expected3;
|
|
||||||
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
|
|
||||||
GaussianBayesTree::sharedClique C3 = bayesTree["x4"];
|
|
||||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
|
||||||
|
|
||||||
// Check the conditional P(C4|Root)
|
|
||||||
Vector sigma4 = repeat(2, 0.661968);
|
|
||||||
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
|
|
||||||
GaussianBayesNet expected4;
|
|
||||||
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
|
|
||||||
GaussianBayesTree::sharedClique C4 = bayesTree["x3"];
|
|
||||||
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* *
|
|
||||||
Bayes tree for smoother with "nested dissection" ordering:
|
|
||||||
|
|
||||||
Node[x1] P(x1 | x2)
|
|
||||||
Node[x3] P(x3 | x2 x4)
|
|
||||||
Node[x5] P(x5 | x4 x6)
|
|
||||||
Node[x7] P(x7 | x6)
|
|
||||||
Node[x2] P(x2 | x4)
|
|
||||||
Node[x6] P(x6 | x4)
|
|
||||||
Node[x4] P(x4)
|
|
||||||
|
|
||||||
becomes
|
|
||||||
|
|
||||||
C1 x5 x6 x4
|
|
||||||
C2 x3 x2 : x4
|
|
||||||
C3 x1 : x2
|
|
||||||
C4 x7 : x6
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, balanced_smoother_marginals )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
Ordering ordering;
|
|
||||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
|
||||||
|
|
||||||
// eliminate using a "nested dissection" ordering
|
|
||||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
|
||||||
|
|
||||||
VectorConfig expectedSolution;
|
|
||||||
BOOST_FOREACH(string key, ordering)
|
|
||||||
expectedSolution.insert(key,zero(2));
|
|
||||||
VectorConfig actualSolution = optimize(chordalBayesNet);
|
|
||||||
CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
|
|
||||||
|
|
||||||
// Create the Bayes tree
|
|
||||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
|
||||||
LONGS_EQUAL(4,bayesTree.size());
|
|
||||||
|
|
||||||
// Check marginal on x1
|
|
||||||
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
|
|
||||||
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1");
|
|
||||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
|
||||||
|
|
||||||
// Check marginal on x2
|
|
||||||
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
|
|
||||||
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2");
|
|
||||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
|
||||||
|
|
||||||
// Check marginal on x3
|
|
||||||
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
|
|
||||||
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3");
|
|
||||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
|
||||||
|
|
||||||
// Check marginal on x4
|
|
||||||
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
|
|
||||||
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4");
|
|
||||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
|
||||||
|
|
||||||
// Check marginal on x7 (should be equal to x1)
|
|
||||||
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
|
|
||||||
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7");
|
|
||||||
CHECK(assert_equal(expected7,actual7,1e-4));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, balanced_smoother_shortcuts )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
Ordering ordering;
|
|
||||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
|
||||||
|
|
||||||
// Create the Bayes tree
|
|
||||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
|
||||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
|
||||||
|
|
||||||
// Check the conditional P(Root|Root)
|
|
||||||
GaussianBayesNet empty;
|
|
||||||
GaussianBayesTree::sharedClique R = bayesTree.root();
|
|
||||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(empty,actual1,1e-4));
|
|
||||||
|
|
||||||
// Check the conditional P(C2|Root)
|
|
||||||
GaussianBayesTree::sharedClique C2 = bayesTree["x3"];
|
|
||||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(empty,actual2,1e-4));
|
|
||||||
|
|
||||||
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
|
|
||||||
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
|
|
||||||
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
|
|
||||||
GaussianBayesTree::sharedClique C3 = bayesTree["x1"];
|
|
||||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
|
||||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, balanced_smoother_clique_marginals )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
Ordering ordering;
|
|
||||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
|
||||||
|
|
||||||
// Create the Bayes tree
|
|
||||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
|
||||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
|
||||||
|
|
||||||
// Check the clique marginal P(C3)
|
|
||||||
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
|
|
||||||
Vector sigma = repeat(2, 0.707107);
|
|
||||||
Matrix A12 = (-0.5)*eye(2);
|
|
||||||
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
|
|
||||||
GaussianBayesTree::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
|
||||||
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
|
|
||||||
GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
|
|
||||||
CHECK(assert_equal(expected,actual,1e-4));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, balanced_smoother_joint )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
Ordering ordering;
|
|
||||||
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
|
||||||
|
|
||||||
// Create the Bayes tree
|
|
||||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
|
||||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
|
||||||
|
|
||||||
// Conditional density elements reused by both tests
|
|
||||||
Vector sigma = repeat(2, 0.786146);
|
|
||||||
Matrix I = eye(2), A = -0.00429185*I;
|
|
||||||
|
|
||||||
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
|
|
||||||
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
|
|
||||||
push_front(expected1,"x1", zero(2), I, "x7", A, sigma);
|
|
||||||
GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
|
|
||||||
CHECK(assert_equal(expected1,actual1,1e-4));
|
|
||||||
|
|
||||||
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
|
|
||||||
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
|
|
||||||
push_front(expected2,"x7", zero(2), I, "x1", A, sigma);
|
|
||||||
GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
|
|
||||||
CHECK(assert_equal(expected2,actual2,1e-4));
|
|
||||||
|
|
||||||
// Check the joint density P(x1,x4), i.e. with a root variable
|
|
||||||
GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4);
|
|
||||||
Vector sigma14 = repeat(2, 0.784465);
|
|
||||||
Matrix A14 = -0.0769231*I;
|
|
||||||
push_front(expected3,"x1", zero(2), I, "x4", A14, sigma14);
|
|
||||||
GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
|
|
||||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
|
||||||
|
|
||||||
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
|
|
||||||
GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1);
|
|
||||||
Vector sigma41 = repeat(2, 0.668096);
|
|
||||||
Matrix A41 = -0.055794*I;
|
|
||||||
push_front(expected4,"x4", zero(2), I, "x1", A41, sigma41);
|
|
||||||
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
|
|
||||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* *
|
/* ************************************************************************* *
|
||||||
Bayes Tree for testing conversion to a forest of orphans needed for incremental.
|
Bayes Tree for testing conversion to a forest of orphans needed for incremental.
|
||||||
A,B
|
A,B
|
||||||
|
|
@ -555,53 +330,6 @@ TEST( BayesTree, iSAM_slam )
|
||||||
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, iSAM_smoother )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
|
|
||||||
// run iSAM for every factor
|
|
||||||
GaussianBayesTree actual;
|
|
||||||
BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
|
|
||||||
GaussianFactorGraph factorGraph;
|
|
||||||
factorGraph.push_back(factor);
|
|
||||||
actual.update(factorGraph);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
|
||||||
Ordering ordering;
|
|
||||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
|
||||||
GaussianBayesTree expected(smoother.eliminate(ordering));
|
|
||||||
|
|
||||||
CHECK(assert_equal(expected, actual));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
|
||||||
TEST( BayesTree, iSAM_smoother2 )
|
|
||||||
{
|
|
||||||
// Create smoother with 7 nodes
|
|
||||||
GaussianFactorGraph smoother = createSmoother(7);
|
|
||||||
|
|
||||||
// Create initial tree from first 4 timestamps in reverse order !
|
|
||||||
Ordering ord; ord += "x4","x3","x2","x1";
|
|
||||||
GaussianFactorGraph factors1;
|
|
||||||
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
|
||||||
GaussianBayesTree actual(factors1.eliminate(ord));
|
|
||||||
|
|
||||||
// run iSAM with remaining factors
|
|
||||||
GaussianFactorGraph factors2;
|
|
||||||
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
|
|
||||||
actual.update(factors2);
|
|
||||||
|
|
||||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
|
||||||
Ordering ordering;
|
|
||||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
|
||||||
GaussianBayesTree expected(smoother.eliminate(ordering));
|
|
||||||
|
|
||||||
CHECK(assert_equal(expected, actual));
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
int main() {
|
int main() {
|
||||||
TestResult tr;
|
TestResult tr;
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,147 @@
|
||||||
|
#! /bin/bash
|
||||||
|
|
||||||
|
# testGaussianBayesTree - temporary wrapper script for .libs/testGaussianBayesTree
|
||||||
|
# Generated by ltmain.sh (GNU libtool) 2.2.4 Debian-2.2.4-0ubuntu4
|
||||||
|
#
|
||||||
|
# The testGaussianBayesTree program cannot be directly executed until all the libtool
|
||||||
|
# libraries that it depends on are installed.
|
||||||
|
#
|
||||||
|
# This wrapper script should never be moved out of the build directory.
|
||||||
|
# If it is, it will not operate correctly.
|
||||||
|
|
||||||
|
# Sed substitution that helps us do robust quoting. It backslashifies
|
||||||
|
# metacharacters that are still active within double-quoted strings.
|
||||||
|
Xsed='/bin/sed -e 1s/^X//'
|
||||||
|
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
|
||||||
|
|
||||||
|
# Be Bourne compatible
|
||||||
|
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
|
||||||
|
emulate sh
|
||||||
|
NULLCMD=:
|
||||||
|
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
|
||||||
|
# is contrary to our usage. Disable this feature.
|
||||||
|
alias -g '${1+"$@"}'='"$@"'
|
||||||
|
setopt NO_GLOB_SUBST
|
||||||
|
else
|
||||||
|
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
|
||||||
|
fi
|
||||||
|
BIN_SH=xpg4; export BIN_SH # for Tru64
|
||||||
|
DUALCASE=1; export DUALCASE # for MKS sh
|
||||||
|
|
||||||
|
# The HP-UX ksh and POSIX shell print the target directory to stdout
|
||||||
|
# if CDPATH is set.
|
||||||
|
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
|
||||||
|
|
||||||
|
relink_command="(cd /home/kaess/borg/gtsam/cpp; { test -z \"\${LIBRARY_PATH+set}\" || unset LIBRARY_PATH || { LIBRARY_PATH=; export LIBRARY_PATH; }; }; { test -z \"\${COMPILER_PATH+set}\" || unset COMPILER_PATH || { COMPILER_PATH=; export COMPILER_PATH; }; }; { test -z \"\${GCC_EXEC_PREFIX+set}\" || unset GCC_EXEC_PREFIX || { GCC_EXEC_PREFIX=; export GCC_EXEC_PREFIX; }; }; { test -z \"\${LD_RUN_PATH+set}\" || unset LD_RUN_PATH || { LD_RUN_PATH=; export LD_RUN_PATH; }; }; LD_LIBRARY_PATH=/usr/lib/jvm/java-6-openjdk/jre/lib/i386/client:/usr/lib/jvm/java-6-openjdk/jre/lib/i386::/usr/local/public/ipp/5.3.1.062/ia32/sharedlib:/usr/local/lib:/usr/local/public/lib:/usr/lib/xulrunner-addons:/usr/lib/xulrunner-addons; export LD_LIBRARY_PATH; PATH=/home/kaess/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/home/kaess/bin:/usr/local/public/bin; export PATH; g++ -g -I/usr/local/ -fPIC -I.. -g -O2 -o \$progdir/\$file smallExample.o testGaussianBayesTree.o -L/home/kaess/borg/gtsam/CppUnitLite -lCppUnitLite ./.libs/libgtsam.so -L/home/kaess/borg/gtsam/colamd -lcolamd -Wl,-rpath -Wl,/home/kaess/borg/gtsam/cpp/.libs -Wl,-rpath -Wl,/home/kaess/lib)"
|
||||||
|
|
||||||
|
# This environment variable determines our operation mode.
|
||||||
|
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
|
||||||
|
# install mode needs the following variables:
|
||||||
|
generated_by_libtool_version='2.2.4'
|
||||||
|
notinst_deplibs=' libgtsam.la'
|
||||||
|
else
|
||||||
|
# When we are sourced in execute mode, $file and $ECHO are already set.
|
||||||
|
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
||||||
|
ECHO="echo"
|
||||||
|
file="$0"
|
||||||
|
# Make sure echo works.
|
||||||
|
if test "X$1" = X--no-reexec; then
|
||||||
|
# Discard the --no-reexec flag, and continue.
|
||||||
|
shift
|
||||||
|
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
|
||||||
|
# Yippee, $ECHO works!
|
||||||
|
:
|
||||||
|
else
|
||||||
|
# Restart under the correct shell, and then maybe $ECHO will work.
|
||||||
|
exec /bin/bash "$0" --no-reexec ${1+"$@"}
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
# Find the directory that this script lives in.
|
||||||
|
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
||||||
|
test "x$thisdir" = "x$file" && thisdir=.
|
||||||
|
|
||||||
|
# Follow symbolic links until we get to the real thisdir.
|
||||||
|
file=`ls -ld "$file" | /bin/sed -n 's/.*-> //p'`
|
||||||
|
while test -n "$file"; do
|
||||||
|
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
|
||||||
|
|
||||||
|
# If there was a directory component, then change thisdir.
|
||||||
|
if test "x$destdir" != "x$file"; then
|
||||||
|
case "$destdir" in
|
||||||
|
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
|
||||||
|
*) thisdir="$thisdir/$destdir" ;;
|
||||||
|
esac
|
||||||
|
fi
|
||||||
|
|
||||||
|
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
|
||||||
|
file=`ls -ld "$thisdir/$file" | /bin/sed -n 's/.*-> //p'`
|
||||||
|
done
|
||||||
|
|
||||||
|
# Usually 'no', except on cygwin/mingw when embedded into
|
||||||
|
# the cwrapper.
|
||||||
|
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
|
||||||
|
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
|
||||||
|
# special case for '.'
|
||||||
|
if test "$thisdir" = "."; then
|
||||||
|
thisdir=`pwd`
|
||||||
|
fi
|
||||||
|
# remove .libs from thisdir
|
||||||
|
case "$thisdir" in
|
||||||
|
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
|
||||||
|
.libs ) thisdir=. ;;
|
||||||
|
esac
|
||||||
|
fi
|
||||||
|
|
||||||
|
# Try to get the absolute directory name.
|
||||||
|
absdir=`cd "$thisdir" && pwd`
|
||||||
|
test -n "$absdir" && thisdir="$absdir"
|
||||||
|
|
||||||
|
program=lt-'testGaussianBayesTree'
|
||||||
|
progdir="$thisdir/.libs"
|
||||||
|
|
||||||
|
if test ! -f "$progdir/$program" ||
|
||||||
|
{ file=`ls -1dt "$progdir/$program" "$progdir/../$program" 2>/dev/null | /bin/sed 1q`; \
|
||||||
|
test "X$file" != "X$progdir/$program"; }; then
|
||||||
|
|
||||||
|
file="$$-$program"
|
||||||
|
|
||||||
|
if test ! -d "$progdir"; then
|
||||||
|
mkdir "$progdir"
|
||||||
|
else
|
||||||
|
rm -f "$progdir/$file"
|
||||||
|
fi
|
||||||
|
|
||||||
|
# relink executable if necessary
|
||||||
|
if test -n "$relink_command"; then
|
||||||
|
if relink_command_output=`eval $relink_command 2>&1`; then :
|
||||||
|
else
|
||||||
|
echo "$relink_command_output" >&2
|
||||||
|
rm -f "$progdir/$file"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
mv -f "$progdir/$file" "$progdir/$program" 2>/dev/null ||
|
||||||
|
{ rm -f "$progdir/$program";
|
||||||
|
mv -f "$progdir/$file" "$progdir/$program"; }
|
||||||
|
rm -f "$progdir/$file"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if test -f "$progdir/$program"; then
|
||||||
|
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
|
||||||
|
# Run the actual program with our arguments.
|
||||||
|
|
||||||
|
exec "$progdir/$program" ${1+"$@"}
|
||||||
|
|
||||||
|
$ECHO "$0: cannot exec $program $*" 1>&2
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
# The program doesn't exist.
|
||||||
|
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
|
||||||
|
$ECHO "This script is just a wrapper for $program." 1>&2
|
||||||
|
echo "See the libtool documentation for more information." 1>&2
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
@ -0,0 +1,295 @@
|
||||||
|
/**
|
||||||
|
* @file testGaussianBayesTree.cpp
|
||||||
|
* @brief Unit tests for GaussianBayesTree
|
||||||
|
* @author Michael Kaess
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <boost/foreach.hpp>
|
||||||
|
#include <boost/assign/std/list.hpp> // for operator +=
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include "Ordering.h"
|
||||||
|
#include "GaussianBayesNet.h"
|
||||||
|
#include "BayesTree-inl.h"
|
||||||
|
#include "GaussianBayesTree.h"
|
||||||
|
#include "smallExample.h"
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Some numbers that should be consistent among all smoother tests
|
||||||
|
|
||||||
|
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||||
|
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||||
|
|
||||||
|
/* ************************************************************************* *
|
||||||
|
Bayes tree for smoother with "natural" ordering:
|
||||||
|
C1 x6 x7
|
||||||
|
C2 x5 : x6
|
||||||
|
C3 x4 : x5
|
||||||
|
C4 x3 : x4
|
||||||
|
C5 x2 : x3
|
||||||
|
C6 x1 : x2
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, linear_smoother_shortcuts )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
Ordering ordering;
|
||||||
|
for (int t = 1; t <= 7; t++)
|
||||||
|
ordering.push_back(symbol('x', t));
|
||||||
|
|
||||||
|
// eliminate using the "natural" ordering
|
||||||
|
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||||
|
|
||||||
|
// Create the Bayes tree
|
||||||
|
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||||
|
LONGS_EQUAL(6,bayesTree.size());
|
||||||
|
|
||||||
|
// Check the conditional P(Root|Root)
|
||||||
|
GaussianBayesNet empty;
|
||||||
|
GaussianBayesTree::sharedClique R = bayesTree.root();
|
||||||
|
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(empty,actual1,1e-4));
|
||||||
|
|
||||||
|
// Check the conditional P(C2|Root)
|
||||||
|
GaussianBayesTree::sharedClique C2 = bayesTree["x5"];
|
||||||
|
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(empty,actual2,1e-4));
|
||||||
|
|
||||||
|
// Check the conditional P(C3|Root)
|
||||||
|
Vector sigma3 = repeat(2, 0.61808);
|
||||||
|
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
|
||||||
|
GaussianBayesNet expected3;
|
||||||
|
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
|
||||||
|
GaussianBayesTree::sharedClique C3 = bayesTree["x4"];
|
||||||
|
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||||
|
|
||||||
|
// Check the conditional P(C4|Root)
|
||||||
|
Vector sigma4 = repeat(2, 0.661968);
|
||||||
|
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
|
||||||
|
GaussianBayesNet expected4;
|
||||||
|
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
|
||||||
|
GaussianBayesTree::sharedClique C4 = bayesTree["x3"];
|
||||||
|
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* *
|
||||||
|
Bayes tree for smoother with "nested dissection" ordering:
|
||||||
|
|
||||||
|
Node[x1] P(x1 | x2)
|
||||||
|
Node[x3] P(x3 | x2 x4)
|
||||||
|
Node[x5] P(x5 | x4 x6)
|
||||||
|
Node[x7] P(x7 | x6)
|
||||||
|
Node[x2] P(x2 | x4)
|
||||||
|
Node[x6] P(x6 | x4)
|
||||||
|
Node[x4] P(x4)
|
||||||
|
|
||||||
|
becomes
|
||||||
|
|
||||||
|
C1 x5 x6 x4
|
||||||
|
C2 x3 x2 : x4
|
||||||
|
C3 x1 : x2
|
||||||
|
C4 x7 : x6
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, balanced_smoother_marginals )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
Ordering ordering;
|
||||||
|
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||||
|
|
||||||
|
// eliminate using a "nested dissection" ordering
|
||||||
|
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||||
|
|
||||||
|
VectorConfig expectedSolution;
|
||||||
|
BOOST_FOREACH(string key, ordering)
|
||||||
|
expectedSolution.insert(key,zero(2));
|
||||||
|
VectorConfig actualSolution = optimize(chordalBayesNet);
|
||||||
|
CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
|
||||||
|
|
||||||
|
// Create the Bayes tree
|
||||||
|
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||||
|
LONGS_EQUAL(4,bayesTree.size());
|
||||||
|
|
||||||
|
// Check marginal on x1
|
||||||
|
GaussianBayesNet expected1 = simpleGaussian("x1", zero(2), sigmax1);
|
||||||
|
GaussianBayesNet actual1 = bayesTree.marginalBayesNet<GaussianFactor>("x1");
|
||||||
|
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||||
|
|
||||||
|
// Check marginal on x2
|
||||||
|
GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2);
|
||||||
|
GaussianBayesNet actual2 = bayesTree.marginalBayesNet<GaussianFactor>("x2");
|
||||||
|
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||||
|
|
||||||
|
// Check marginal on x3
|
||||||
|
GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3);
|
||||||
|
GaussianBayesNet actual3 = bayesTree.marginalBayesNet<GaussianFactor>("x3");
|
||||||
|
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||||
|
|
||||||
|
// Check marginal on x4
|
||||||
|
GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4);
|
||||||
|
GaussianBayesNet actual4 = bayesTree.marginalBayesNet<GaussianFactor>("x4");
|
||||||
|
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||||
|
|
||||||
|
// Check marginal on x7 (should be equal to x1)
|
||||||
|
GaussianBayesNet expected7 = simpleGaussian("x7", zero(2), sigmax7);
|
||||||
|
GaussianBayesNet actual7 = bayesTree.marginalBayesNet<GaussianFactor>("x7");
|
||||||
|
CHECK(assert_equal(expected7,actual7,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, balanced_smoother_shortcuts )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
Ordering ordering;
|
||||||
|
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||||
|
|
||||||
|
// Create the Bayes tree
|
||||||
|
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||||
|
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||||
|
|
||||||
|
// Check the conditional P(Root|Root)
|
||||||
|
GaussianBayesNet empty;
|
||||||
|
GaussianBayesTree::sharedClique R = bayesTree.root();
|
||||||
|
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(empty,actual1,1e-4));
|
||||||
|
|
||||||
|
// Check the conditional P(C2|Root)
|
||||||
|
GaussianBayesTree::sharedClique C2 = bayesTree["x3"];
|
||||||
|
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(empty,actual2,1e-4));
|
||||||
|
|
||||||
|
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
|
||||||
|
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
|
||||||
|
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
|
||||||
|
GaussianBayesTree::sharedClique C3 = bayesTree["x1"];
|
||||||
|
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||||
|
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, balanced_smoother_clique_marginals )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
Ordering ordering;
|
||||||
|
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||||
|
|
||||||
|
// Create the Bayes tree
|
||||||
|
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||||
|
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||||
|
|
||||||
|
// Check the clique marginal P(C3)
|
||||||
|
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
|
||||||
|
Vector sigma = repeat(2, 0.707107);
|
||||||
|
Matrix A12 = (-0.5)*eye(2);
|
||||||
|
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
|
||||||
|
GaussianBayesTree::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||||
|
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
|
||||||
|
GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
|
||||||
|
CHECK(assert_equal(expected,actual,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, balanced_smoother_joint )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
Ordering ordering;
|
||||||
|
ordering += "x1","x3","x5","x7","x2","x6","x4";
|
||||||
|
|
||||||
|
// Create the Bayes tree
|
||||||
|
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||||
|
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||||
|
|
||||||
|
// Conditional density elements reused by both tests
|
||||||
|
Vector sigma = repeat(2, 0.786146);
|
||||||
|
Matrix I = eye(2), A = -0.00429185*I;
|
||||||
|
|
||||||
|
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
|
||||||
|
GaussianBayesNet expected1 = simpleGaussian("x7", zero(2), sigmax7);
|
||||||
|
push_front(expected1,"x1", zero(2), I, "x7", A, sigma);
|
||||||
|
GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
|
||||||
|
CHECK(assert_equal(expected1,actual1,1e-4));
|
||||||
|
|
||||||
|
// Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
|
||||||
|
GaussianBayesNet expected2 = simpleGaussian("x1", zero(2), sigmax1);
|
||||||
|
push_front(expected2,"x7", zero(2), I, "x1", A, sigma);
|
||||||
|
GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
|
||||||
|
CHECK(assert_equal(expected2,actual2,1e-4));
|
||||||
|
|
||||||
|
// Check the joint density P(x1,x4), i.e. with a root variable
|
||||||
|
GaussianBayesNet expected3 = simpleGaussian("x4", zero(2), sigmax4);
|
||||||
|
Vector sigma14 = repeat(2, 0.784465);
|
||||||
|
Matrix A14 = -0.0769231*I;
|
||||||
|
push_front(expected3,"x1", zero(2), I, "x4", A14, sigma14);
|
||||||
|
GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
|
||||||
|
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||||
|
|
||||||
|
// Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
|
||||||
|
GaussianBayesNet expected4 = simpleGaussian("x1", zero(2), sigmax1);
|
||||||
|
Vector sigma41 = repeat(2, 0.668096);
|
||||||
|
Matrix A41 = -0.055794*I;
|
||||||
|
push_front(expected4,"x4", zero(2), I, "x1", A41, sigma41);
|
||||||
|
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
|
||||||
|
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, iSAM_smoother )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
|
||||||
|
// run iSAM for every factor
|
||||||
|
GaussianBayesTree actual;
|
||||||
|
BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
|
||||||
|
GaussianFactorGraph factorGraph;
|
||||||
|
factorGraph.push_back(factor);
|
||||||
|
actual.update(factorGraph);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||||
|
Ordering ordering;
|
||||||
|
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||||
|
GaussianBayesTree expected(smoother.eliminate(ordering));
|
||||||
|
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( BayesTree, iSAM_smoother2 )
|
||||||
|
{
|
||||||
|
// Create smoother with 7 nodes
|
||||||
|
GaussianFactorGraph smoother = createSmoother(7);
|
||||||
|
|
||||||
|
// Create initial tree from first 4 timestamps in reverse order !
|
||||||
|
Ordering ord; ord += "x4","x3","x2","x1";
|
||||||
|
GaussianFactorGraph factors1;
|
||||||
|
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
||||||
|
GaussianBayesTree actual(factors1.eliminate(ord));
|
||||||
|
|
||||||
|
// run iSAM with remaining factors
|
||||||
|
GaussianFactorGraph factors2;
|
||||||
|
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
|
||||||
|
actual.update(factors2);
|
||||||
|
|
||||||
|
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||||
|
Ordering ordering;
|
||||||
|
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||||
|
GaussianBayesTree expected(smoother.eliminate(ordering));
|
||||||
|
|
||||||
|
CHECK(assert_equal(expected, actual));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue