From 4200271cf4bd381b62012bb4c33e483a3f8a767d Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Wed, 9 Dec 2009 19:39:25 +0000 Subject: [PATCH] GaussianBayesTree added, testBayesTree split --- cpp/GaussianBayesTree.h | 26 +++ cpp/Makefile.am | 7 +- cpp/inference-inl.h | 2 +- cpp/testBayesTree.cpp | 274 +------------------------------ cpp/testGaussianBayesTree | 147 +++++++++++++++++ cpp/testGaussianBayesTree.cpp | 295 ++++++++++++++++++++++++++++++++++ 6 files changed, 475 insertions(+), 276 deletions(-) create mode 100644 cpp/GaussianBayesTree.h create mode 100755 cpp/testGaussianBayesTree create mode 100644 cpp/testGaussianBayesTree.cpp diff --git a/cpp/GaussianBayesTree.h b/cpp/GaussianBayesTree.h new file mode 100644 index 000000000..8ac507dca --- /dev/null +++ b/cpp/GaussianBayesTree.h @@ -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 +#include +#include +#include +#include +#include + +#include "Testable.h" +#include "BayesTree.h" +#include "GaussianConditional.h" + +namespace gtsam { + + typedef BayesTree GaussianBayesTree; + +} /// namespace gtsam diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 78d5eff29..5cba74b96 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -63,13 +63,16 @@ example = smallExample.cpp # Inference headers += inference.h inference-inl.h headers += FactorGraph.h FactorGraph-inl.h -headers += BayesNet.h BayesNet-inl.h BayesTree.h BayesTree-inl.h -check_PROGRAMS += testFactorgraph testBayesTree testInference testOrdering +headers += BayesNet.h BayesNet-inl.h +headers += BayesTree.h BayesTree-inl.h GaussianBayesTree.h +check_PROGRAMS += testFactorgraph testBayesTree testGaussianBayesTree testInference testOrdering testFactorgraph_SOURCES = testFactorgraph.cpp testBayesTree_SOURCES = $(example) testBayesTree.cpp +testGaussianBayesTree_SOURCES = $(example) testGaussianBayesTree.cpp testInference_SOURCES = $(example) testInference.cpp testFactorgraph_LDADD = libgtsam.la testBayesTree_LDADD = libgtsam.la +testGaussianBayesTree_LDADD = libgtsam.la testInference_LDADD = libgtsam.la testOrdering_SOURCES = testOrdering.cpp testOrdering_LDADD = libgtsam.la diff --git a/cpp/inference-inl.h b/cpp/inference-inl.h index b8b19cf1b..ee41e87b4 100644 --- a/cpp/inference-inl.h +++ b/cpp/inference-inl.h @@ -4,7 +4,7 @@ * @author Frank Dellaert */ -#include "inference.h" +//#include "inference.h" #include "FactorGraph-inl.h" #include "BayesNet-inl.h" diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index b0756aa5b..adc7c696b 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -12,7 +12,6 @@ using namespace boost::assign; #include #include "SymbolicBayesNet.h" -#include "GaussianBayesNet.h" #include "SymbolicFactorGraph.h" #include "Ordering.h" #include "BayesTree-inl.h" @@ -21,7 +20,7 @@ using namespace boost::assign; using namespace gtsam; typedef BayesTree SymbolicBayesTree; -typedef BayesTree GaussianBayesTree; + /* ************************************************************************* */ // SLAM example from RSS sqrtSAM paper SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), @@ -110,230 +109,6 @@ TEST( BayesTree, constructor ) 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(R); - CHECK(assert_equal(empty,actual1,1e-4)); - - // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree["x5"]; - GaussianBayesNet actual2 = C2->shortcut(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(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(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("x1"); - CHECK(assert_equal(expected1,actual1,1e-4)); - - // Check marginal on x2 - GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2); - GaussianBayesNet actual2 = bayesTree.marginalBayesNet("x2"); - CHECK(assert_equal(expected2,actual2,1e-4)); - - // Check marginal on x3 - GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3); - GaussianBayesNet actual3 = bayesTree.marginalBayesNet("x3"); - CHECK(assert_equal(expected3,actual3,1e-4)); - - // Check marginal on x4 - GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4); - GaussianBayesNet actual4 = bayesTree.marginalBayesNet("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("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(R); - CHECK(assert_equal(empty,actual1,1e-4)); - - // Check the conditional P(C2|Root) - GaussianBayesTree::sharedClique C2 = bayesTree["x3"]; - GaussianBayesNet actual2 = C2->shortcut(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(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 marginal = C3->marginal(R); - GaussianBayesNet actual = eliminate(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("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("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("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("x4","x1"); - CHECK(assert_equal(expected4,actual4,1e-4)); -} - /* ************************************************************************* * Bayes Tree for testing conversion to a forest of orphans needed for incremental. A,B @@ -555,53 +330,6 @@ TEST( BayesTree, iSAM_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 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; diff --git a/cpp/testGaussianBayesTree b/cpp/testGaussianBayesTree new file mode 100755 index 000000000..e0965de12 --- /dev/null +++ b/cpp/testGaussianBayesTree @@ -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 diff --git a/cpp/testGaussianBayesTree.cpp b/cpp/testGaussianBayesTree.cpp new file mode 100644 index 000000000..800af85cc --- /dev/null +++ b/cpp/testGaussianBayesTree.cpp @@ -0,0 +1,295 @@ +/** + * @file testGaussianBayesTree.cpp + * @brief Unit tests for GaussianBayesTree + * @author Michael Kaess + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include + +#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(R); + CHECK(assert_equal(empty,actual1,1e-4)); + + // Check the conditional P(C2|Root) + GaussianBayesTree::sharedClique C2 = bayesTree["x5"]; + GaussianBayesNet actual2 = C2->shortcut(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(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(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("x1"); + CHECK(assert_equal(expected1,actual1,1e-4)); + + // Check marginal on x2 + GaussianBayesNet expected2 = simpleGaussian("x2", zero(2), sigmax2); + GaussianBayesNet actual2 = bayesTree.marginalBayesNet("x2"); + CHECK(assert_equal(expected2,actual2,1e-4)); + + // Check marginal on x3 + GaussianBayesNet expected3 = simpleGaussian("x3", zero(2), sigmax3); + GaussianBayesNet actual3 = bayesTree.marginalBayesNet("x3"); + CHECK(assert_equal(expected3,actual3,1e-4)); + + // Check marginal on x4 + GaussianBayesNet expected4 = simpleGaussian("x4", zero(2), sigmax4); + GaussianBayesNet actual4 = bayesTree.marginalBayesNet("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("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(R); + CHECK(assert_equal(empty,actual1,1e-4)); + + // Check the conditional P(C2|Root) + GaussianBayesTree::sharedClique C2 = bayesTree["x3"]; + GaussianBayesNet actual2 = C2->shortcut(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(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 marginal = C3->marginal(R); + GaussianBayesNet actual = eliminate(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("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("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("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("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 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);} +/* ************************************************************************* */