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

Fixed remaining LinearFactor unit tests 

Added equality check for Variable
release/4.3a0
Alex Cunningham 2009-11-05 13:52:12 +00:00
parent d9289d14b3
commit 6c5603de0c
2 changed files with 170 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cpp/Factor.h
View File

@ -27,6 +27,7 @@ namespace gtsam {
bool operator< (const Variable& other) const {return key_<other.key_; } bool operator< (const Variable& other) const {return key_<other.key_; }
const std::string& key() const { return key_;} const std::string& key() const { return key_;}
std::size_t dim() const { return dim_;} std::size_t dim() const { return dim_;}
bool equals(const Variable& var) const { return key_ == var.key_ && dim_ == var.dim_;}
}; };
/** A set of variables, used to eliminate linear factor factor graphs. TODO FD: move */ /** A set of variables, used to eliminate linear factor factor graphs. TODO FD: move */

349
cpp/testLinearFactor.cpp
View File

@ -9,6 +9,7 @@
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp>
using namespace boost::assign; using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -67,9 +68,19 @@ TEST( LinearFactor, variables )
// get the factor "f2" from the small linear factor graph // get the factor "f2" from the small linear factor graph
LinearFactorGraph fg = createLinearFactorGraph(); LinearFactorGraph fg = createLinearFactorGraph();
LinearFactor::shared_ptr lf = fg[1]; LinearFactor::shared_ptr lf = fg[1];
VariableSet vs = lf->variables(); VariableSet actual = lf->variables();
//TODO: test this // create expected variable set
VariableSet expected;
Variable x1("x1", 2);
Variable x2("x2", 2);
expected += x1, x2;
// verify
CHECK(expected.size() == actual.size());
set<Variable>::const_iterator exp, act;
for (exp=expected.begin(), act=actual.begin(); exp != expected.end(); act++, exp++)
CHECK((*exp).equals(*act));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -361,133 +372,122 @@ TEST( LinearFactor, eliminate )
} }
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( LinearFactor, eliminate2 ) TEST( LinearFactor, eliminate2 )
//{ {
// cout << "TEST( LinearFactor, eliminate2 )" << endl; // sigmas
// // sigmas double sigma1 = 0.2;
// double sigma1 = 0.2; double sigma2 = 0.1;
// double sigma2 = 0.1; Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2);
// // the combined linear factor
// // the combined linear factor Matrix Ax2 = Matrix_(4,2,
// Matrix Ax2 = Matrix_(4,2, // x2
// // x2 -1., 0.,
// -1., 0., +0.,-1.,
// +0.,-1., 1., 0.,
// 1., 0., +0.,1.
// +0.,1. );
// );
// Matrix Al1x1 = Matrix_(4,4,
// Matrix Al1x1 = Matrix_(4,4, // l1 x1
// // l1 x1 1., 0., 0.00, 0., // f4
// 1., 0., 0.00, 0., // f4 0., 1., 0.00, 0., // f4
// 0., 1., 0.00, 0., // f4 0., 0., -1., 0., // f2
// 0., 0., -1., 0., // f2 0., 0., 0.00,-1. // f2
// 0., 0., 0.00,-1. // f2 );
// );
// // the RHS
// // the RHS Vector b2(4);
// Vector b2(4); b2(0) = -0.2;
// b2(0) = -1*sigma1; b2(1) = 0.3;
// b2(1) = 1.5*sigma1; b2(2) = 0.2;
// b2(2) = 2*sigma2; b2(3) = -0.1;
// b2(3) = -1*sigma2;
// vector<pair<string, Matrix> > meas;
// vector<pair<string, Matrix> > meas; meas.push_back(make_pair("x2", Ax2));
// meas.push_back(make_pair("x2", Ax2)); meas.push_back(make_pair("l1x1", Al1x1));
// meas.push_back(make_pair("l1x1", Al1x1)); LinearFactor combined(meas, b2, sigmas);
// LinearFactor combined(meas, b2, sigmas);
// // eliminate the combined factor
// // eliminate the combined factor ConditionalGaussian::shared_ptr actualCG;
// ConditionalGaussian::shared_ptr actualCG; LinearFactor::shared_ptr actualLF;
// LinearFactor::shared_ptr actualLF; boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// boost::tie(actualCG,actualLF) = combined.eliminate("x2");
// // create expected Conditional Gaussian
// // create expected Conditional Gaussian Matrix R11 = Matrix_(2,2,
// Matrix R11 = Matrix_(2,2, 1.00, 0.00,
// 11.1803, 0.00, 0.00, 1.00
// 0.00, 11.1803 );
// ); Matrix S12 = Matrix_(2,4,
// Matrix S12 = Matrix_(2,4, -0.20, 0.00,-0.80, 0.00,
// -2.23607, 0.00,-8.94427, 0.00, +0.00,-0.20,+0.00,-0.80
// +0.00,-2.23607,+0.00,-8.94427 );
// ); Vector d(2); d(0) = 0.2; d(1) = -0.14;
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
// Vector x2Sigmas(2);
// Vector sigmas(2); x2Sigmas(0) = 0.0894427;
// sigmas(0) = R11(0,0); x2Sigmas(1) = 0.0894427;
// sigmas(1) = R11(1,1);
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,x2Sigmas);
// // normalize the existing matrices
// Matrix N = eye(2,2); // the expected linear factor
// N(0,0) = 1/sigmas(0); double sigma = 0.2236;
// N(1,1) = 1/sigmas(1); Matrix Bl1x1 = Matrix_(2,4,
// S12 = N*S12; // l1 x1
// R11 = N*R11; 1.00, 0.00, -1.00, 0.00,
// 0.00, 1.00, +0.00, -1.00
// ConditionalGaussian expectedCG("x2",d,R11,"l1x1",S12,sigmas); );
//
// // the expected linear factor // the RHS
// double sigma = 0.2236; Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
// Matrix Bl1x1 = Matrix_(2,4,
// // l1 x1 LinearFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma);
// 1.00, 0.00, -1.00, 0.00,
// 0.00, 1.00, +0.00, -1.00 // check if the result matches
// ); CHECK(assert_equal(expectedCG,*actualCG,1e-4));
// CHECK(assert_equal(expectedLF,*actualLF,1e-5));
// // the RHS }
// Vector b1(2); b1(0) = 0.0; b1(1) = 0.894427;
// /* ************************************************************************* */
// LinearFactor expectedLF("l1x1", Bl1x1, b1*sigma, sigma); TEST( LinearFactor, default_error )
// {
// // check if the result matches LinearFactor f;
// CHECK(assert_equal(expectedCG,*actualCG,1e-4)); //FAILS VectorConfig c;
// CHECK(assert_equal(expectedLF,*actualLF,1e-5)); //FAILS double actual = f.error(c);
//} CHECK(actual==0.0);
// }
////* ************************************************************************* */
//TEST( LinearFactor, default_error ) //* ************************************************************************* */
//{ TEST( LinearFactor, eliminate_empty )
// cout << "TEST( LinearFactor, default_error )" << endl; {
// LinearFactor f; // create an empty factor
// VectorConfig c; LinearFactor f;
// double actual = f.error(c);
// CHECK(actual==0.0); // eliminate the empty factor
//} ConditionalGaussian::shared_ptr actualCG;
// LinearFactor::shared_ptr actualLF;
////* ************************************************************************* */ boost::tie(actualCG,actualLF) = f.eliminate("x2");
//TEST( LinearFactor, eliminate_empty )
//{ // expected Conditional Gaussian is just a parent-less node with P(x)=1
// cout << "TEST( LinearFactor, eliminate_empty )" << endl; ConditionalGaussian expectedCG("x2");
// // create an empty factor
// LinearFactor f; // expected remaining factor is still empty :-)
// LinearFactor expectedLF;
// // eliminate the empty factor
// ConditionalGaussian::shared_ptr actualCG; // check if the result matches
// LinearFactor::shared_ptr actualLF; CHECK(actualCG->equals(expectedCG));
// boost::tie(actualCG,actualLF) = f.eliminate("x2"); CHECK(actualLF->equals(expectedLF));
// }
// // expected Conditional Gaussian is just a parent-less node with P(x)=1
// ConditionalGaussian expectedCG("x2"); //* ************************************************************************* */
// TEST( LinearFactor, empty )
// // expected remaining factor is still empty :-) {
// LinearFactor expectedLF; // create an empty factor
// LinearFactor f;
// // check if the result matches CHECK(f.empty()==true);
// CHECK(actualCG->equals(expectedCG)); }
// CHECK(actualLF->equals(expectedLF));
//}
//
////* ************************************************************************* */
//TEST( LinearFactor, empty )
//{
// cout << "TEST( LinearFactor, empty )" << endl;
// // create an empty factor
// LinearFactor f;
// CHECK(f.empty()==true);
//}
//
/* ************************************************************************* */ /* ************************************************************************* */
TEST( LinearFactor, matrix ) TEST( LinearFactor, matrix )
{ {
@ -513,57 +513,46 @@ TEST( LinearFactor, matrix )
EQUALITY(b,b1); EQUALITY(b,b1);
} }
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( LinearFactor, size ) TEST( LinearFactor, size )
//{ {
// cout << "TEST( LinearFactor, size )" << endl; // create a linear factor graph
// // create a linear factor graph LinearFactorGraph fg = createLinearFactorGraph();
// LinearFactorGraph fg = createLinearFactorGraph();
// // get some factors from the graph
// // get some factors from the graph boost::shared_ptr<LinearFactor> factor1 = fg[0];
// boost::shared_ptr<LinearFactor> factor1 = fg[0]; boost::shared_ptr<LinearFactor> factor2 = fg[1];
// boost::shared_ptr<LinearFactor> factor2 = fg[1]; boost::shared_ptr<LinearFactor> factor3 = fg[2];
// boost::shared_ptr<LinearFactor> factor3 = fg[2];
// CHECK(factor1->size() == 1);
// CHECK(factor1->size() == 1); CHECK(factor2->size() == 2);
// CHECK(factor2->size() == 2); CHECK(factor3->size() == 2);
// CHECK(factor3->size() == 2); }
//}
// /* ************************************************************************* */
///* ************************************************************************* */ TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )
//TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian ) {
//{ Matrix R11 = Matrix_(2,2,
// cout << "TEST( LinearFactor, CONSTRUCTOR_ConditionalGaussian )" << endl; 1.00, 0.00,
// Matrix R11 = Matrix_(2,2, 0.00, 1.00
// 11.1803, 0.00, );
// 0.00, 11.1803 Matrix S12 = Matrix_(2,2,
// ); -0.200001, 0.00,
// Matrix S12 = Matrix_(2,2, +0.00,-0.200001
// -2.23607, 0.00, );
// +0.00,-2.23607 Vector d(2); d(0) = 2.23607; d(1) = -1.56525;
// );
// Vector d(2); d(0) = 2.23607; d(1) = -1.56525; Vector sigmas(2);
// sigmas(0) = 0.29907;
// Vector sigmas(2); sigmas(1) = 0.29907;
// sigmas(0) = R11(0,0);
// sigmas(1) = R11(1,1); ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas));
// LinearFactor actualLF(CG);
// // normalize the existing matrices // actualLF.print();
// Matrix N = eye(2,2); LinearFactor expectedLF("x2",R11,"l1x1",S12,d, sigmas(0));
// N(0,0) = 1/sigmas(0);
// N(1,1) = 1/sigmas(1); CHECK(assert_equal(expectedLF,actualLF,1e-5));
// S12 = N*S12; }
// R11 = N*R11;
//
// ConditionalGaussian::shared_ptr CG(new ConditionalGaussian("x2",d,R11,"l1x1",S12,sigmas));
// LinearFactor actualLF(CG);
// // actualLF.print();
// double precision = 11.1803;
// double sigma = 1/sqrt(precision);
// LinearFactor expectedLF("x2",R11,"l1x1",S12,d, sigma);
//
// CHECK(assert_equal(expectedLF,actualLF,1e-5)); //FAILS Precisions are incorrect
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */