534 lines
14 KiB
C++
534 lines
14 KiB
C++
/**
|
|
* @file testLinearFactorGraph.cpp
|
|
* @brief Unit tests for Linear Factor Graph
|
|
* @author Christian Potthast
|
|
**/
|
|
|
|
/*STL/C++*/
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
#include <string.h>
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include "Matrix.h"
|
|
#include "smallExample.h"
|
|
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
/* unit test for equals (LinearFactorGraph1 == LinearFactorGraph2) */
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, equals ){
|
|
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
LinearFactorGraph fg2 = createLinearFactorGraph();
|
|
CHECK( fg.equals(fg2) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, error )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
FGConfig cfg = createZeroDelta();
|
|
|
|
// note the error is the same as in testNonlinearFactorGraph as a
|
|
// zero delta config in the linear graph is equivalent to noisy in
|
|
// non-linear, which is really linear under the hood
|
|
double actual = fg.error(cfg);
|
|
DOUBLES_EQUAL( 5.625, actual, 1e-9 );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
/* unit test for find factors */
|
|
/* ************************************************************************* */
|
|
/*
|
|
TEST( LinearFactorGraph, find_factors )
|
|
{
|
|
int checksum = 0;
|
|
int expected = 16;
|
|
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// create a shared pointer object to prevent memory leaks
|
|
set<shared_ptr<LinearFactor> > factors = fg.find_factors_and_remove("x2");
|
|
|
|
// CHECK whether find the right two factors
|
|
|
|
// odometry between x1 and x2
|
|
Matrix A21(2,2);
|
|
A21(0,0) = -10.0 ; A21(0,1) = 0.0;
|
|
A21(1,0) = 0.0 ; A21(1,1) = -10.0;
|
|
|
|
Matrix A22(2,2);
|
|
A22(0,0) = 10.0 ; A22(0,1) = 0.0;
|
|
A22(1,0) = 0.0 ; A22(1,1) = 10.0;
|
|
|
|
Vector b(2);
|
|
b(0) = 2 ; b(1) = -1;
|
|
|
|
LinearFactor f2 = LinearFactor("x1", A21, "x2", A22, b);
|
|
|
|
// measurement between x2 and l1
|
|
Matrix A41(2,2);
|
|
A41(0,0) = -5 ; A41(0,1) = 0.0;
|
|
A41(1,0) = 0.0 ; A41(1,1) = -5;
|
|
|
|
Matrix A42(2,2);
|
|
A42(0,0) = 5 ; A42(0,1) = 0;
|
|
A42(1,0) = 0 ; A42(1,1) = 5;
|
|
|
|
b(0)= -1 ; b(1) = 1.5;
|
|
|
|
LinearFactor f4 = LinearFactor("x2", A41, "l1", A42, b);
|
|
|
|
set<shared_ptr<LinearFactor> >::iterator it;
|
|
for( it = factors.begin() ; it != factors.end() ; it++ ){
|
|
if( strcmp( it->get()->name_.c_str(), "f2") == 0){
|
|
if(it->get()->equals(f2)){
|
|
checksum = checksum + 2;
|
|
}
|
|
}else if( strcmp( it->get()->name_.c_str(), "f4") == 0){
|
|
if(it->get()->equals(f4)){
|
|
checksum = checksum + 4;
|
|
}else{
|
|
}
|
|
}
|
|
}
|
|
|
|
// CHECK if the factors are deleted from the factor graph
|
|
|
|
// Create
|
|
LinearFactorGraph fg2;
|
|
|
|
// prior on x1
|
|
Matrix A11(2,2);
|
|
A11(0,0) = 10 ; A11(0,1) = 0;
|
|
A11(1,0) = 0 ; A11(1,1) = 10;
|
|
|
|
b(0) = -1 ; b(1) = -1;
|
|
|
|
LinearFactor *f1 = new LinearFactor("x1", A11, b);
|
|
fg2.push_back(f1);
|
|
|
|
// measurement between x1 and l1
|
|
Matrix A31(2,2);
|
|
A31(0,0) = -5 ; A31(0,1) = 0;
|
|
A31(1,0) = 0 ; A31(1,1) = -5;
|
|
|
|
Matrix A32(2,2);
|
|
A32(0,0) = 5 ; A32(0,1) = 0;
|
|
A32(1,0) = 0 ; A32(1,1) = 5;
|
|
|
|
b(0) = 0 ; b(1) = 1;
|
|
|
|
LinearFactor *f3 = new LinearFactor("x1", A31, "l1", A32, b);
|
|
fg2.push_back(f3);
|
|
|
|
if( fg.equals(fg2) ){
|
|
checksum = checksum + 10;
|
|
}
|
|
|
|
CHECK( checksum == expected );
|
|
}
|
|
*/
|
|
|
|
/* ************************************************************************* */
|
|
/* unit test for find seperator */
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, find_separator )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
set<string> separator = fg.find_separator("x2");
|
|
set<string> expected;
|
|
expected.insert("x1");
|
|
expected.insert("l1");
|
|
|
|
CHECK(separator.size()==expected.size());
|
|
set<string>::iterator it1 = separator.begin(), it2 = expected.begin();
|
|
for(; it1!=separator.end(); it1++, it2++)
|
|
CHECK(*it1 == *it2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Note: This test does not pass when running on Linux(Ubuntu 9.04, GCC 4.3.3)
|
|
// systems.
|
|
TEST( LinearFactorGraph, combine_factors_x1 )
|
|
{
|
|
// create a small example for a linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// combine all factors
|
|
LinearFactor::shared_ptr actual = fg.combine_factors("x1");
|
|
|
|
// the expected linear factor
|
|
Matrix Al1 = Matrix_(6,2,
|
|
0., 0.,
|
|
0., 0.,
|
|
0., 0.,
|
|
0., 0.,
|
|
5., 0.,
|
|
0., 5.
|
|
);
|
|
|
|
Matrix Ax1 = Matrix_(6,2,
|
|
10., 0.,
|
|
0.00, 10.,
|
|
-10., 0.,
|
|
0.00,-10.,
|
|
-5., 0.,
|
|
00., -5.
|
|
);
|
|
|
|
Matrix Ax2 = Matrix_(6,2,
|
|
0., 0.,
|
|
0., 0.,
|
|
10., 0.,
|
|
+0.,10.,
|
|
0., 0.,
|
|
0., 0.
|
|
);
|
|
|
|
// the expected RHS vector
|
|
Vector b(6);
|
|
b(0) = -1;
|
|
b(1) = -1;
|
|
b(2) = 2;
|
|
b(3) = -1;
|
|
b(4) = 0;
|
|
b(5) = 1;
|
|
|
|
LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
|
|
|
|
// check if the two factors are the same
|
|
CHECK(actual->equals(expected)); //currently fails
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, combine_factors_x2 )
|
|
{
|
|
// create a small example for a linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// combine all factors
|
|
LinearFactor::shared_ptr actual = fg.combine_factors("x2");
|
|
|
|
// the expected linear factor
|
|
Matrix Al1 = Matrix_(4,2,
|
|
// l1
|
|
0., 0.,
|
|
0., 0.,
|
|
5., 0.,
|
|
0., 5.
|
|
);
|
|
|
|
Matrix Ax1 = Matrix_(4,2,
|
|
// x1
|
|
-10., 0., // f2
|
|
0.00,-10., // f2
|
|
0.00, 0., // f4
|
|
0.00, 0. // f4
|
|
);
|
|
|
|
Matrix Ax2 = Matrix_(4,2,
|
|
// x2
|
|
10., 0.,
|
|
+0.,10.,
|
|
-5., 0.,
|
|
+0.,-5.
|
|
);
|
|
|
|
// the expected RHS vector
|
|
Vector b(4);
|
|
b(0) = 2;
|
|
b(1) = -1;
|
|
b(2) = -1;
|
|
b(3) = 1.5;
|
|
|
|
LinearFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b);
|
|
|
|
// check if the two factors are the same
|
|
CHECK(actual->equals(expected)); // currently fails - ordering is different
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( LinearFactorGraph, eliminate_one_x1 )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
ConditionalGaussian::shared_ptr actual = fg.eliminate_one("x1");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix R11 = Matrix_(2,2,
|
|
15.0, 00.0,
|
|
00.0, 15.0
|
|
);
|
|
Matrix S12 = Matrix_(2,2,
|
|
-1.66667, 0.00,
|
|
+0.00,-1.66667
|
|
);
|
|
Matrix S13 = Matrix_(2,2,
|
|
-6.66667, 0.00,
|
|
+0.00,-6.66667
|
|
);
|
|
Vector d(2); d(0) = -2; d(1) = -1.0/3.0;
|
|
ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13);
|
|
|
|
CHECK( actual->equals(expected) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
TEST( LinearFactorGraph, eliminate_one_x2 )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
ConditionalGaussian::shared_ptr actual = fg.eliminate_one("x2");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix R11 = Matrix_(2,2,
|
|
11.1803, 0.00,
|
|
0.00, 11.1803
|
|
);
|
|
Matrix S12 = Matrix_(2,2,
|
|
-2.23607, 0.00,
|
|
+0.00,-2.23607
|
|
);
|
|
Matrix S13 = Matrix_(2,2,
|
|
-8.94427, 0.00,
|
|
+0.00,-8.94427
|
|
);
|
|
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
|
|
ConditionalGaussian expected(d,R11,"l1",S12,"x1",S13);
|
|
|
|
CHECK( actual->equals(expected) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, eliminate_one_l1 )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
ConditionalGaussian::shared_ptr actual = fg.eliminate_one("l1");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix R11 = Matrix_(2,2,
|
|
7.07107, 0.00,
|
|
0.00, 7.07107
|
|
);
|
|
Matrix S12 = Matrix_(2,2,
|
|
-3.53553, 0.00,
|
|
+0.00,-3.53553
|
|
);
|
|
Matrix S13 = Matrix_(2,2,
|
|
-3.53553, 0.00,
|
|
+0.00,-3.53553
|
|
);
|
|
Vector d(2); d(0) = -0.707107; d(1) = 1.76777;
|
|
ConditionalGaussian expected(d,R11,"x1",S12,"x2",S13);
|
|
|
|
CHECK( actual->equals(expected) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, eliminateAll )
|
|
{
|
|
// create expected Chordal bayes Net
|
|
double data1[] = { 10, 0.0,
|
|
0.0, 10};
|
|
Matrix R1 = Matrix_(2,2, data1);
|
|
Vector d1(2); d1(0) = -1; d1(1) = -1;
|
|
ConditionalGaussian::shared_ptr cg1(new ConditionalGaussian(d1, R1));
|
|
|
|
double data21[] = { 6.7082, 0.0,
|
|
0.0, 6.7082};
|
|
Matrix R2 = Matrix_(2,2, data21);
|
|
double data22[] = { -6.7082, 0.0,
|
|
0.0, -6.7082};
|
|
Matrix A1 = Matrix_(2,2, data22);
|
|
Vector d2(2); d2(0) = 0.0; d2(1) = 1.34164;
|
|
ConditionalGaussian::shared_ptr cg2(new ConditionalGaussian(d2, R2, "x1", A1));
|
|
|
|
double data31[] = { 11.1803, 0.0,
|
|
0.0, 11.1803};
|
|
Matrix R3 = Matrix_(2,2, data31);
|
|
double data32[] = { -2.23607, 0.0,
|
|
0.0, -2.23607};
|
|
Matrix A21 = Matrix_(2,2, data32);
|
|
double data33[] = { -8.94427, 0.0,
|
|
0.0, -8.94427};
|
|
Matrix A22 = Matrix_(2,2, data33);
|
|
|
|
Vector d3(2); d3(0) = 2.23607; d3(1) = -1.56525;
|
|
ConditionalGaussian::shared_ptr cg3(new ConditionalGaussian(d3, R3, "l1", A21, "x1", A22));
|
|
|
|
ChordalBayesNet expected;
|
|
expected.insert("x1", cg1);
|
|
expected.insert("l1", cg2);
|
|
expected.insert("x2", cg3);
|
|
|
|
// Check one ordering
|
|
LinearFactorGraph fg1 = createLinearFactorGraph();
|
|
Ordering ord1;
|
|
ord1.push_back("x2");
|
|
ord1.push_back("l1");
|
|
ord1.push_back("x1");
|
|
ChordalBayesNet::shared_ptr actual1 = fg1.eliminate(ord1);
|
|
CHECK(actual1->equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, add_priors )
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
LinearFactorGraph actual = fg.add_priors(3);
|
|
LinearFactorGraph expected = createLinearFactorGraph();
|
|
Matrix A = 3*eye(2);
|
|
Vector b = zero(2);
|
|
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("l1",A,b)));
|
|
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x1",A,b)));
|
|
expected.push_back(LinearFactor::shared_ptr(new LinearFactor("x2",A,b)));
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, copying )
|
|
{
|
|
// Create a graph
|
|
LinearFactorGraph actual = createLinearFactorGraph();
|
|
|
|
// Copy the graph !
|
|
LinearFactorGraph copy = actual;
|
|
|
|
// now eliminate the copy
|
|
Ordering ord1;
|
|
ord1.push_back("x2");
|
|
ord1.push_back("l1");
|
|
ord1.push_back("x1");
|
|
ChordalBayesNet::shared_ptr actual1 = copy.eliminate(ord1);
|
|
|
|
// Create the same graph, but not by copying
|
|
LinearFactorGraph expected = createLinearFactorGraph();
|
|
|
|
// and check that original is still the same graph
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, matrix )
|
|
{
|
|
// Create a graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord.push_back("x2");
|
|
ord.push_back("l1");
|
|
ord.push_back("x1");
|
|
|
|
Matrix A; Vector b;
|
|
boost::tie(A,b) = fg.matrix(ord);
|
|
|
|
Matrix A1 = Matrix_(2*4,3*2,
|
|
00.0, 0.0, 0.0, 0.0, 10.0, 0.0,
|
|
00.0, 0.0, 0.0, 0.0, 0.0, 10.0,
|
|
10.0, 0.0, 0.0, 0.0,-10.0, 0.0,
|
|
00.0, 10.0, 0.0, 0.0, 0.0,-10.0,
|
|
00.0, 0.0, 5.0, 0.0, -5.0, 0.0,
|
|
00.0, 0.0, 0.0, 5.0, 0.0, -5.0,
|
|
-5.0, 0.0, 5.0, 0.0, 0.0, 0.0,
|
|
00.0, -5.0, 0.0, 5.0, 0.0, 0.0
|
|
);
|
|
Vector b1 = Vector_(8,-1.0, -1.0, 2.0, -1.0, 0.0, 1.0, -1.0, 1.5);
|
|
|
|
EQUALITY(A,A1); // currently fails
|
|
CHECK(b==b1); // currently fails
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, CONSTRUCTOR_ChordalBayesNet )
|
|
{
|
|
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord.push_back("x2");
|
|
ord.push_back("l1");
|
|
ord.push_back("x1");
|
|
|
|
ChordalBayesNet::shared_ptr CBN = fg.eliminate(ord);
|
|
LinearFactorGraph fg2(*CBN);
|
|
ChordalBayesNet::shared_ptr CBN2 = fg2.eliminate(ord);
|
|
|
|
CHECK(CBN->equals(*CBN2));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, GET_ORDERING)
|
|
{
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
Ordering ord = fg.getOrdering();
|
|
CHECK(ord[0] == string("l1"));
|
|
CHECK(ord[1] == string("x1"));
|
|
CHECK(ord[2] == string("x2"));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, OPTIMIZE )
|
|
{
|
|
// create a graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// create an ordering
|
|
Ordering ord = fg.getOrdering();
|
|
|
|
// optimize the graph
|
|
FGConfig actual = fg.optimize(ord);
|
|
|
|
// verify
|
|
FGConfig expected = createCorrectDelta();
|
|
|
|
CHECK(actual.equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, COMBINE_GRAPHS_INPLACE)
|
|
{
|
|
// create a test graph
|
|
LinearFactorGraph fg1 = createLinearFactorGraph();
|
|
|
|
// create another factor graph
|
|
LinearFactorGraph fg2 = createLinearFactorGraph();
|
|
|
|
// get sizes
|
|
int size1 = fg1.size();
|
|
int size2 = fg2.size();
|
|
|
|
// combine them
|
|
fg1.combine(fg2);
|
|
|
|
CHECK(size1+size2 == fg1.size());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactorGraph, COMBINE_GRAPHS)
|
|
{
|
|
// create a test graph
|
|
LinearFactorGraph fg1 = createLinearFactorGraph();
|
|
|
|
// create another factor graph
|
|
LinearFactorGraph fg2 = createLinearFactorGraph();
|
|
|
|
// get sizes
|
|
int size1 = fg1.size();
|
|
int size2 = fg2.size();
|
|
|
|
// combine them
|
|
LinearFactorGraph fg3 = LinearFactorGraph::combine2(fg1, fg2);
|
|
|
|
CHECK(size1+size2 == fg3.size());
|
|
}
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|