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

SVD and HessianFactor checked using same information

release/4.3a0
dellaert 2015-02-22 07:05:11 +01:00
parent d09c7aa105
commit 772db8850a
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -483,18 +483,22 @@ TEST( SmartProjectionPoseFactor, Factors ){
CHECK(p); CHECK(p);
EXPECT(assert_equal(landmark1,*p)); EXPECT(assert_equal(landmark1,*p));
// After eliminating the point, A1 and A2 contain 2-rank information on cameras:
Matrix16 A1, A2;
A1 << -1000, 0, 0, 0, 100, 0;
A2 << 1000, 0, 100, 0, -100, 0;
{ {
// createHessianFactor // createHessianFactor
Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; Matrix66 G11 = 0.5*A1.transpose()*A1;
Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; Matrix66 G12 = 0.5*A1.transpose()*A2;
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 = 0.5*A2.transpose()*A2;
Vector6 g1; g1.setZero(); Vector6 g1; g1.setZero();
Vector6 g2; g2.setZero(); Vector6 g2; g2.setZero();
double f = 0; double f = 0;
RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, 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);
@ -530,9 +534,6 @@ TEST( SmartProjectionPoseFactor, Factors ){
{ {
// createJacobianSVDFactor // createJacobianSVDFactor
Matrix16 A1, A2;
A1 << -1000, 0, 0, 0, 100, 0;
A2 << 1000, 0, 100, 0, -100, 0;
Vector1 b; b.setZero(); Vector1 b; b.setZero();
double s = sin(M_PI_4); double s = sin(M_PI_4);
JacobianFactor expected(x1, s*A1, x2, s*A2, b); JacobianFactor expected(x1, s*A1, x2, s*A2, b);
@ -542,7 +543,6 @@ TEST( SmartProjectionPoseFactor, Factors ){
CHECK(actual); CHECK(actual);
CHECK(assert_equal(expected, *actual)); CHECK(assert_equal(expected, *actual));
} }
} }
/* *************************************************************************/ /* *************************************************************************/