12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/cpp/testLinearFactor.cpp

420 lines
11 KiB
C++
Raw Blame History

/**
* @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, 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);}
/* ************************************************************************* */
Reference in New Issue View Git Blame Copy Permalink