ISAM2 (nonlinear ISAM) partially there, unit test currently disabled
Michael Kaess
parent
8d4a029665
commit
75ab62a729
|
|
@ -633,6 +633,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianISAM2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianISAM2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
|
|
|
|||
|
|
@ -400,7 +400,6 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: add to factors and orphans
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file GaussianISAM
|
||||
* @brief
|
||||
* @brief Linear ISAM only
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file GaussianISAM
|
||||
* @brief
|
||||
* @brief Linear ISAM only
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,42 @@
|
|||
/**
|
||||
* @file GaussianISAM2
|
||||
* @brief Full non-linear ISAM
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#include "GaussianISAM2.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
#include "ISAM2-inl.h"
|
||||
template class ISAM2<GaussianConditional, VectorConfig>;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
GaussianISAM2::Clique::const_reverse_iterator it;
|
||||
for (it = clique->rbegin(); it!=clique->rend(); it++) {
|
||||
GaussianConditional::shared_ptr cg = *it;
|
||||
Vector x = cg->solve(result); // Solve for that variable
|
||||
result.insert(cg->key(), x); // store result in partial solution
|
||||
}
|
||||
BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) {
|
||||
// list<GaussianISAM2::Clique::shared_ptr>::const_iterator child;
|
||||
// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
|
||||
optimize2(child, result);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig optimize2(const GaussianISAM2& bayesTree) {
|
||||
VectorConfig result;
|
||||
// starting from the root, call optimize on each conditional
|
||||
optimize2(bayesTree.root(), result);
|
||||
return result;
|
||||
}
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
/**
|
||||
* @file GaussianISAM
|
||||
* @brief Full non-linear ISAM.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ISAM2.h"
|
||||
#include "GaussianConditional.h"
|
||||
#include "GaussianFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef ISAM2<GaussianConditional, VectorConfig> GaussianISAM2;
|
||||
|
||||
// recursively optimize this conditional and all subtrees
|
||||
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result);
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
VectorConfig optimize2(const GaussianISAM2& bayesTree);
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/**
|
||||
* @file ISAM2-inl.h
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "GaussianFactor.h"
|
||||
#include "VectorConfig.h"
|
||||
|
||||
#include "Conditional.h"
|
||||
#include "BayesTree-inl.h"
|
||||
#include "ISAM2.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, Cliques& orphans) {
|
||||
|
||||
Config xxx;
|
||||
FactorGraph<GaussianFactor> newFactors; //todo = newFactorsXXX.linearize(xxx);
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<GaussianFactor> factors;
|
||||
boost::tie(factors, orphans) = this->removeTop(newFactors);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
||||
// create an ordering for the new and contaminated factors
|
||||
Ordering ordering;
|
||||
if (true) {
|
||||
ordering = factors.getOrdering();
|
||||
} else {
|
||||
list<string> keys = factors.keys();
|
||||
keys.sort(); // todo: correct sorting order?
|
||||
ordering = keys;
|
||||
}
|
||||
|
||||
// eliminate into a Bayes net
|
||||
BayesNet<Conditional> bayesNet = eliminate<GaussianFactor, Conditional>(factors,ordering);
|
||||
|
||||
// insert conditionals back in, straight into the topless bayesTree
|
||||
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
this->insert(*rit);
|
||||
|
||||
int count = 0;
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
|
||||
string key = orphan->separator_.front();
|
||||
sharedClique parent = (*this)[key];
|
||||
|
||||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors) {
|
||||
Cliques orphans;
|
||||
this->update_internal(newFactors, orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
/**
|
||||
* @file ISAM2.h
|
||||
* @brief Incremental update functionality (ISAM2) for BayesTree.
|
||||
* @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 "FactorGraph.h"
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "BayesNet.h"
|
||||
#include "BayesTree.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class Conditional, class Config>
|
||||
class ISAM2: public BayesTree<Conditional> {
|
||||
|
||||
NonlinearFactorGraph<Config> nonlinearFactors_;
|
||||
|
||||
public:
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM2();
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
ISAM2(const BayesNet<Conditional>& bayesNet);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~ISAM2() {
|
||||
}
|
||||
|
||||
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
||||
|
||||
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
||||
|
||||
/**
|
||||
* ISAM2. (update_internal provides access to list of orphans for drawing purposes)
|
||||
*/
|
||||
void update_internal(const NonlinearFactorGraph<Config>& newFactors, Cliques& orphans);
|
||||
void update(const NonlinearFactorGraph<Config>& newFactors);
|
||||
|
||||
}; // ISAM2
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -77,9 +77,10 @@ headers += FactorGraph.h FactorGraph-inl.h
|
|||
headers += BayesNet.h BayesNet-inl.h
|
||||
headers += BayesTree.h BayesTree-inl.h
|
||||
headers += ISAM.h ISAM-inl.h GaussianISAM.h
|
||||
sources += GaussianISAM.cpp
|
||||
headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h
|
||||
sources += GaussianISAM.cpp GaussianISAM2.cpp
|
||||
check_PROGRAMS += testFactorgraph testInference testOrdering
|
||||
check_PROGRAMS += testBayesTree testISAM testGaussianISAM
|
||||
check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2
|
||||
testFactorgraph_SOURCES = testFactorgraph.cpp
|
||||
testInference_SOURCES = $(example) testInference.cpp
|
||||
testFactorgraph_LDADD = libgtsam.la
|
||||
|
|
@ -90,6 +91,8 @@ testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
|||
testBayesTree_LDADD = libgtsam.la
|
||||
testGaussianISAM_SOURCES = $(example) testGaussianISAM.cpp
|
||||
testGaussianISAM_LDADD = libgtsam.la
|
||||
testGaussianISAM2_SOURCES = $(example) testGaussianISAM2.cpp
|
||||
testGaussianISAM2_LDADD = libgtsam.la
|
||||
testISAM_SOURCES = $(example) testISAM.cpp
|
||||
testISAM_LDADD = libgtsam.la
|
||||
|
||||
|
|
|
|||
|
|
@ -192,7 +192,7 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph createSmoother(int T) {
|
||||
pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
|
||||
|
||||
// noise on measurements and odometry, respectively
|
||||
double sigma1 = 1, sigma2 = 1;
|
||||
|
|
@ -224,6 +224,15 @@ GaussianFactorGraph createSmoother(int T) {
|
|||
poses.insert(key, xt);
|
||||
}
|
||||
|
||||
return make_pair(nlfg, poses);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph createSmoother(int T) {
|
||||
ExampleNonlinearFactorGraph nlfg;
|
||||
VectorConfig poses;
|
||||
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
|
||||
|
||||
GaussianFactorGraph lfg = nlfg.linearize(poses);
|
||||
return lfg;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,6 +62,12 @@ namespace gtsam {
|
|||
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
|
||||
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
|
||||
|
||||
/**
|
||||
* Create a full nonlinear smoother
|
||||
* @param T number of time-steps
|
||||
*/
|
||||
std::pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T);
|
||||
|
||||
/**
|
||||
* Create a Kalman smoother by linearizing a non-linear factor graph
|
||||
* @param T number of time-steps
|
||||
|
|
|
|||
|
|
@ -0,0 +1,308 @@
|
|||
/**
|
||||
* @file testGaussianISAM2.cpp
|
||||
* @brief Unit tests for GaussianISAM2
|
||||
* @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 "ISAM2-inl.h"
|
||||
#include "GaussianISAM2.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;
|
||||
#if 0
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM2, ISAM2_smoother )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
ExampleNonlinearFactorGraph smoother;
|
||||
VectorConfig poses;
|
||||
boost::tie(smoother, poses) = createNonlinearSmoother(7);
|
||||
|
||||
// run ISAM2 for every factor
|
||||
GaussianISAM2 actual;
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) {
|
||||
ExampleNonlinearFactorGraph 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);
|
||||
GaussianISAM2 expected(smoother.linearize(poses).eliminate(ordering));
|
||||
|
||||
// Check whether BayesTree is correct
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// obtain solution
|
||||
VectorConfig e; // expected solution
|
||||
Vector v = Vector_(2, 0., 0.);
|
||||
for (int i=1; i<=7; i++)
|
||||
e.insert(symbol('x', i), v);
|
||||
VectorConfig optimized = optimize2(actual); // actual solution
|
||||
CHECK(assert_equal(e, optimized));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM2, ISAM2_smoother2 )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
ExampleNonlinearFactorGraph smoother;
|
||||
VectorConfig poses;
|
||||
boost::tie(smoother, poses) = createNonlinearSmoother(7);
|
||||
|
||||
// Create initial tree from first 4 timestamps in reverse order !
|
||||
Ordering ord; ord += "x4","x3","x2","x1";
|
||||
ExampleNonlinearFactorGraph factors1;
|
||||
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
||||
GaussianISAM2 actual(factors1.linearize(poses).eliminate(ord)); // todo: subset of poses?
|
||||
|
||||
// run ISAM2 with remaining factors
|
||||
ExampleNonlinearFactorGraph 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);
|
||||
GaussianISAM2 expected(smoother.linearize(poses).eliminate(ordering));
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
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
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
LONGS_EQUAL(6,bayesTree.size());
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianISAM2::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianISAM2::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);
|
||||
GaussianISAM2::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);
|
||||
GaussianISAM2::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 = optimize2(chordalBayesNet);
|
||||
CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianISAM2 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);
|
||||
GaussianISAM2 bayesTree(chordalBayesNet);
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianISAM2::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianISAM2::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);
|
||||
GaussianISAM2::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);
|
||||
GaussianISAM2 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);
|
||||
GaussianISAM2::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);
|
||||
GaussianISAM2 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));
|
||||
}
|
||||
#endif
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue