432 lines
11 KiB
C++
432 lines
11 KiB
C++
/**
|
|
* @file testLinearFactor.cpp
|
|
* @brief Unit tests for Linear Factor
|
|
* @author Christian Potthast
|
|
**/
|
|
|
|
/*STL/C++*/
|
|
#include <iostream>
|
|
using namespace std;
|
|
|
|
#include <boost/tuple/tuple.hpp>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include "Matrix.h"
|
|
#include "smallExample.h"
|
|
|
|
using namespace gtsam;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, linearFactor )
|
|
{
|
|
Matrix A1(2,2);
|
|
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
|
|
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
|
|
|
|
Matrix A2(2,2);
|
|
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
|
|
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
|
|
|
|
Vector b(2);
|
|
b(0) = 2 ; b(1) = -1;
|
|
|
|
LinearFactor expected("x1", A1, "x2", A2, b);
|
|
|
|
// create a small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// get the factor "f2" from the factor graph
|
|
LinearFactor::shared_ptr lf = fg[1];
|
|
|
|
// check if the two factors are the same
|
|
CHECK( lf->equals(expected) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, keys )
|
|
{
|
|
// get the factor "f2" from the small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
LinearFactor::shared_ptr lf = fg[1];
|
|
set<string> expected;
|
|
expected.insert("x1");
|
|
expected.insert("x2");
|
|
CHECK(lf->keys() == expected);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, variables )
|
|
{
|
|
// get the factor "f2" from the small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
LinearFactor::shared_ptr lf = fg[1];
|
|
VariableSet vs = lf->variables();
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
|
|
// NOTE: This test does not pass when running on Linux (Ubuntu 9.04, GCC 4.3.3)
|
|
// systems, and appears to be dependent on the ordering of factors in the
|
|
// set lfg. Reversing the order in which the matrices are appended will
|
|
// make this test work correctly, while the next test will fail.
|
|
TEST( LinearFactor, linearFactor2 )
|
|
{
|
|
// create a small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// get two factors from it and insert the factors into a set
|
|
std::set<LinearFactor::shared_ptr> lfg;
|
|
lfg.insert(fg[4 - 1]);
|
|
lfg.insert(fg[2 - 1]);
|
|
|
|
// combine in a factor
|
|
MutableLinearFactor combined(lfg);
|
|
|
|
// the expected combined linear factor
|
|
Matrix Ax2 = Matrix_(4, 2, // x2
|
|
-5., 0.,
|
|
+0., -5.,
|
|
10., 0.,
|
|
+0., 10.);
|
|
|
|
Matrix Al1 = Matrix_(4, 2, // l1
|
|
5., 0.,
|
|
0., 5.,
|
|
0., 0.,
|
|
0., 0.);
|
|
|
|
Matrix Ax1 = Matrix_(4, 2, // x1
|
|
0.00, 0., // f4
|
|
0.00, 0., // f4
|
|
-10., 0., // f2
|
|
0.00, -10. // f2
|
|
);
|
|
|
|
// the RHS
|
|
Vector b2(4);
|
|
b2(0) = -1;
|
|
b2(1) = 1.5;
|
|
b2(2) = 2;
|
|
b2(3) = -1;
|
|
|
|
LinearFactor expected("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
|
|
CHECK(combined.equals(expected));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, linearFactor3){
|
|
|
|
Matrix A11(2,2);
|
|
A11(0,0) = 10.4545; A11(0,1) = 0;
|
|
A11(1,0) = 0; A11(1,1) = 10.4545;
|
|
Vector b(2);
|
|
b(0) = 2 ; b(1) = -1;
|
|
LinearFactor::shared_ptr f1(new LinearFactor("x1", A11, b));
|
|
|
|
A11(0,0) = 2; A11(0,1) = 0;
|
|
A11(1,0) = 0; A11(1,1) = -2;
|
|
b(0) = 4 ; b(1) = -5;
|
|
LinearFactor::shared_ptr f2(new LinearFactor("x1", A11, b));
|
|
|
|
A11(0,0) = 4; A11(0,1) = 0;
|
|
A11(1,0) = 0; A11(1,1) = -4;
|
|
b(0) = 3 ; b(1) = -88;
|
|
LinearFactor::shared_ptr f3(new LinearFactor("x1", A11, b));
|
|
|
|
A11(0,0) = 6; A11(0,1) = 0;
|
|
A11(1,0) = 0; A11(1,1) = 7;
|
|
b(0) = 5 ; b(1) = -6;
|
|
LinearFactor::shared_ptr f4(new LinearFactor("x1", A11, b));
|
|
|
|
std::set<LinearFactor::shared_ptr> lfg;
|
|
lfg.insert(f1);
|
|
lfg.insert(f2);
|
|
lfg.insert(f3);
|
|
lfg.insert(f4);
|
|
MutableLinearFactor combined(lfg);
|
|
|
|
Matrix A22(8,2);
|
|
A22(0,0) = 10.4545; A22(0,1) = 0;
|
|
A22(1,0) = 0; A22(1,1) = 10.4545;
|
|
A22(2,0) = 2; A22(2,1) = 0;
|
|
A22(3,0) = 0; A22(3,1) = -2;
|
|
A22(4,0) = 4; A22(4,1) = 0;
|
|
A22(5,0) = 0; A22(5,1) = -4;
|
|
A22(6,0) = 6; A22(6,1) = 0;
|
|
A22(7,0) = 0; A22(7,1) = 7;
|
|
Vector exb(8);
|
|
exb(0) = 2 ; exb(1) = -1; exb(2) = 4 ; exb(3) = -5;
|
|
exb(4) = 3 ; exb(5) = -88; exb(6) = 5 ; exb(7) = -6;
|
|
LinearFactor::shared_ptr expected(new LinearFactor("x1", A22, exb));
|
|
|
|
CHECK( combined.equals(*expected) ); // currently fails on linux, see previous test's note
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, error )
|
|
{
|
|
// create a small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// get the first factor from the factor graph
|
|
LinearFactor::shared_ptr lf = fg[0];
|
|
|
|
// check the error of the first factor with nosiy config
|
|
FGConfig cfg = createZeroDelta();
|
|
|
|
// calculate the error from the factor "f1"
|
|
// note the error is the same as in testNonlinearFactor
|
|
double actual = lf->error(cfg);
|
|
DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, eliminate )
|
|
{
|
|
// the combined linear factor
|
|
Matrix Ax2 = Matrix_(4,2,
|
|
// x2
|
|
-5., 0.,
|
|
+0.,-5.,
|
|
10., 0.,
|
|
+0.,10.
|
|
);
|
|
|
|
Matrix Al1 = Matrix_(4,2,
|
|
// l1
|
|
5., 0.,
|
|
0., 5.,
|
|
0., 0.,
|
|
0., 0.
|
|
);
|
|
|
|
Matrix Ax1 = Matrix_(4,2,
|
|
// x1
|
|
0.00, 0., // f4
|
|
0.00, 0., // f4
|
|
-10., 0., // f2
|
|
0.00,-10. // f2
|
|
);
|
|
|
|
// the RHS
|
|
Vector b2(4);
|
|
b2(0) = -1;
|
|
b2(1) = 1.5;
|
|
b2(2) = 2;
|
|
b2(3) = -1;
|
|
|
|
MutableLinearFactor combined("x2", Ax2, "l1", Al1, "x1", Ax1, b2);
|
|
|
|
// eliminate the combined factor
|
|
|
|
ConditionalGaussian::shared_ptr actualCG;
|
|
LinearFactor::shared_ptr actualLF;
|
|
boost::tie(actualCG,actualLF) = combined.eliminate("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 expectedCG(d,R11,"l1",S12,"x1",S13);
|
|
|
|
// the expected linear factor
|
|
Matrix Bl1 = Matrix_(2,2,
|
|
// l1
|
|
4.47214, 0.00,
|
|
0.00, 4.47214
|
|
);
|
|
|
|
Matrix Bx1 = Matrix_(2,2,
|
|
// x1
|
|
-4.47214, 0.00,
|
|
+0.00, -4.47214
|
|
);
|
|
|
|
// the RHS
|
|
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
|
|
|
|
LinearFactor expectedLF("l1", Bl1, "x1", Bx1, b1);
|
|
|
|
// check if the result matches
|
|
CHECK(actualCG->equals(expectedCG));
|
|
CHECK(actualLF->equals(expectedLF,1e-5));
|
|
}
|
|
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, eliminate2 )
|
|
{
|
|
// the combined linear factor
|
|
Matrix Ax2 = Matrix_(4,2,
|
|
// x2
|
|
-5., 0.,
|
|
+0.,-5.,
|
|
10., 0.,
|
|
+0.,10.
|
|
);
|
|
|
|
Matrix Al1x1 = Matrix_(4,4,
|
|
// l1 x1
|
|
5., 0., 0.00, 0., // f4
|
|
0., 5., 0.00, 0., // f4
|
|
0., 0., -10., 0., // f2
|
|
0., 0., 0.00,-10. // f2
|
|
);
|
|
|
|
// the RHS
|
|
Vector b2(4);
|
|
b2(0) = -1;
|
|
b2(1) = 1.5;
|
|
b2(2) = 2;
|
|
b2(3) = -1;
|
|
|
|
MutableLinearFactor combined("x2", Ax2, "l1x1", Al1x1, b2);
|
|
|
|
// eliminate the combined factor
|
|
ConditionalGaussian::shared_ptr actualCG;
|
|
LinearFactor::shared_ptr actualLF;
|
|
boost::tie(actualCG,actualLF) = combined.eliminate("x2");
|
|
|
|
// create expected Conditional Gaussian
|
|
Matrix R11 = Matrix_(2,2,
|
|
11.1803, 0.00,
|
|
0.00, 11.1803
|
|
);
|
|
Matrix S12 = Matrix_(2,4,
|
|
-2.23607, 0.00,-8.94427, 0.00,
|
|
+0.00,-2.23607,+0.00,-8.94427
|
|
);
|
|
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
|
|
ConditionalGaussian expectedCG(d,R11,"l1x1",S12);
|
|
|
|
// the expected linear factor
|
|
Matrix Bl1x1 = Matrix_(2,4,
|
|
// l1 x1
|
|
4.47214, 0.00, -4.47214, 0.00,
|
|
0.00, 4.47214, +0.00, -4.47214
|
|
);
|
|
|
|
// the RHS
|
|
Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
|
|
|
|
LinearFactor expectedLF("l1x1", Bl1x1, b1);
|
|
|
|
// check if the result matches
|
|
CHECK(actualCG->equals(expectedCG));
|
|
CHECK(actualLF->equals(expectedLF,1e-5));
|
|
}
|
|
|
|
//* ************************************************************************* */
|
|
TEST( LinearFactor, default_error )
|
|
{
|
|
MutableLinearFactor f;
|
|
FGConfig c;
|
|
double actual = f.error(c);
|
|
CHECK(actual==0.0);
|
|
}
|
|
|
|
//* ************************************************************************* */
|
|
TEST( LinearFactor, eliminate_empty )
|
|
{
|
|
// create an empty factor
|
|
MutableLinearFactor f;
|
|
|
|
// eliminate the empty factor
|
|
ConditionalGaussian::shared_ptr actualCG;
|
|
LinearFactor::shared_ptr actualLF;
|
|
boost::tie(actualCG,actualLF) = f.eliminate("x2");
|
|
|
|
// expected Conditional Gaussian is just a parent-less node with P(x)=1
|
|
ConditionalGaussian expectedCG;
|
|
|
|
// expected remaining factor is still empty :-)
|
|
MutableLinearFactor expectedLF;
|
|
|
|
// check if the result matches
|
|
CHECK(actualCG->equals(expectedCG));
|
|
CHECK(actualLF->equals(expectedLF));
|
|
}
|
|
|
|
//* ************************************************************************* */
|
|
TEST( LinearFactor, empty )
|
|
{
|
|
// create an empty factor
|
|
MutableLinearFactor f;
|
|
CHECK(f.empty()==true);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, matrix )
|
|
{
|
|
// create a small linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// get the factor "f2" from the factor graph
|
|
LinearFactor::shared_ptr lf = fg[1];
|
|
|
|
// render with a given ordering
|
|
Ordering ord;
|
|
ord.push_back("x1");
|
|
ord.push_back("x2");
|
|
|
|
Matrix A; Vector b;
|
|
boost::tie(A,b) = lf->matrix(ord);
|
|
|
|
Matrix A1 = Matrix_(2,4,
|
|
-10.0, 0.0, 10.0, 0.0,
|
|
000.0,-10.0, 0.0, 10.0 );
|
|
Vector b1 = Vector_(2, 2.0, -1.0);
|
|
|
|
EQUALITY(A,A1);
|
|
EQUALITY(b,b1);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, size )
|
|
{
|
|
// create a linear factor graph
|
|
LinearFactorGraph fg = createLinearFactorGraph();
|
|
|
|
// get some factors from the graph
|
|
boost::shared_ptr<LinearFactor> factor1 = fg[0];
|
|
boost::shared_ptr<LinearFactor> factor2 = fg[1];
|
|
boost::shared_ptr<LinearFactor> factor3 = fg[2];
|
|
|
|
CHECK(factor1->size() == 1);
|
|
CHECK(factor2->size() == 2);
|
|
CHECK(factor3->size() == 2);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
|
|
{
|
|
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
|
|
);
|
|
Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
|
|
ConditionalGaussian::shared_ptr CG(new ConditionalGaussian(d,R11,"l1x1",S12) );
|
|
LinearFactor actualLF("x2",CG);
|
|
// actualLF.print();
|
|
LinearFactor expectedLF("x2",R11,"l1x1",S12,d);
|
|
|
|
CHECK(actualLF.equals(expectedLF));
|
|
}
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
|
/* ************************************************************************* */
|