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

Test createJacobianSVDFactor

release/4.3a0
dellaert 2015-02-22 06:48:34 +01:00
parent 8b914189cb
commit d09c7aa105
1 changed files with 32 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -460,8 +460,6 @@ TEST( SmartProjectionPoseFactor, Factors ){
vector<Point2> measurements_cam1; vector<Point2> measurements_cam1;
// Project 2 landmarks into 2 cameras // Project 2 landmarks into 2 cameras
cout << cam1.project(landmark1) << endl;
cout << cam2.project(landmark1) << endl;
measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam1.project(landmark1));
measurements_cam1.push_back(cam2.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1));
@ -485,17 +483,8 @@ TEST( SmartProjectionPoseFactor, Factors ){
CHECK(p); CHECK(p);
EXPECT(assert_equal(landmark1,*p)); EXPECT(assert_equal(landmark1,*p));
Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10;
Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10;
Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100;
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, 10*F1))(make_pair(x2, 10*F2));
Matrix3 P = (E.transpose() * E).inverse();
Vector4 b; b.setZero();
{ {
// RegularHessianFactor<6> // createHessianFactor
// TODO, calculate G from F
Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1;
Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1;
Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1;
@ -508,27 +497,48 @@ TEST( SmartProjectionPoseFactor, Factors ){
RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f);
boost::shared_ptr<RegularHessianFactor<6> > actual = boost::shared_ptr<RegularHessianFactor<6> > actual =
smartFactor1->createHessianFactor(cameras, 0.0); smartFactor1->createHessianFactor(cameras, 0.0);
CHECK(assert_equal(expected.information(),actual->information(),1e-8)); CHECK(assert_equal(expected.information(), actual->information(), 1e-8));
CHECK(assert_equal(expected,*actual,1e-8)); CHECK(assert_equal(expected, *actual, 1e-8));
} }
{ {
// RegularImplicitSchurFactor<6> Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10;
Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10;
Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100;
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, 10*F1))(make_pair(x2, 10*F2));
Matrix3 P = (E.transpose() * E).inverse();
Vector4 b; b.setZero();
// createRegularImplicitSchurFactor
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual = boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
CHECK(actual); CHECK(actual);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected, *actual));
// createJacobianQFactor
JacobianFactorQ < 6, 2 > expectedQ(Fblocks, E, P, b);
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0);
CHECK(actual);
CHECK(assert_equal(expectedQ, *actualQ));
} }
{ {
// createJacobianQFactor<6,2> // createJacobianSVDFactor
JacobianFactorQ<6, 2> expected(Fblocks, E, P, b); Matrix16 A1, A2;
A1 << -1000, 0, 0, 0, 100, 0;
A2 << 1000, 0, 100, 0, -100, 0;
Vector1 b; b.setZero();
double s = sin(M_PI_4);
JacobianFactor expected(x1, s*A1, x2, s*A2, b);
boost::shared_ptr<JacobianFactorQ<6, 2> > actual = boost::shared_ptr<JacobianFactor> actual =
smartFactor1->createJacobianQFactor(cameras, 0.0); smartFactor1->createJacobianSVDFactor(cameras, 0.0);
CHECK(actual); CHECK(actual);
CHECK(assert_equal(expected, *actual)); CHECK(assert_equal(expected, *actual));
} }