69 lines
1.9 KiB
C++
69 lines
1.9 KiB
C++
/**
|
|
* @file testBayesNetConditioner.cpp
|
|
* @brief Unit tests for BayesNetConditioner
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include <boost/foreach.hpp>
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#define GTSAM_MAGIC_KEY
|
|
|
|
#include "Ordering.h"
|
|
#include "BayesNetPreconditioner.h"
|
|
#include "iterative-inl.h"
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( BayesNetPreconditioner, operators )
|
|
{
|
|
// Build a simple Bayes net
|
|
// small Bayes Net x <- y, x=2D, y=1D
|
|
// 1 2 3 x1 0
|
|
// 0 1 2 * x2 = 0
|
|
// 0 0 1 x3 1
|
|
|
|
// Create a scalar Gaussian on y
|
|
GaussianBayesNet bn = scalarGaussian("y", 1, 0.1);
|
|
|
|
// Add a conditional node with one parent |Rx+Sy-d|
|
|
Matrix R11 = Matrix_(2, 2, 1.0, 2.0, 0.0, 1.0), S12 = Matrix_(2, 1, 3.0, 2.0);
|
|
Vector d = zero(2);
|
|
Vector sigmas = Vector_(2, 0.1, 0.1);
|
|
push_front(bn, "x", d, R11, "y", S12, sigmas);
|
|
|
|
// Create Precondioner class
|
|
GaussianFactorGraph dummy;
|
|
BayesNetPreconditioner P(dummy,bn);
|
|
|
|
// inv(R1)*d should equal solution [1;-2;1]
|
|
VectorConfig D;
|
|
D.insert("x", d);
|
|
D.insert("y", Vector_(1, 1.0 / 0.1)); // corrected by sigma
|
|
VectorConfig expected1;
|
|
expected1.insert("x", Vector_(2, 1.0, -2.0));
|
|
expected1.insert("y", Vector_(1, 1.0));
|
|
VectorConfig actual1 = P.backSubstitute(D);
|
|
CHECK(assert_equal(expected1,actual1));
|
|
|
|
// inv(R1')*ones should equal ?
|
|
VectorConfig ones;
|
|
ones.insert("x", Vector_(2, 1.0, 1.0));
|
|
ones.insert("y", Vector_(1, 1.0));
|
|
VectorConfig expected2;
|
|
expected2.insert("x", Vector_(2, 0.1, -0.1));
|
|
expected2.insert("y", Vector_(1, 0.0));
|
|
VectorConfig actual2 = P.backSubstituteTranspose(ones);
|
|
CHECK(assert_equal(expected2,actual2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() {
|
|
TestResult tr;
|
|
return TestRegistry::runAllTests(tr);
|
|
}
|
|
/* ************************************************************************* */
|