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

Most factors now work out for sigma=1 and sigma=0.1

release/4.3a0
dellaert 2015-03-02 08:53:07 -08:00
parent 11a6ba412c
commit ca18c13cd2
1 changed files with 47 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -39,7 +39,7 @@ static const bool manageDegeneracy = true;
// Create a noise model for the pixel error // Create a noise model for the pixel error
static const double sigma = 0.1; static const double sigma = 0.1;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, sigma)); static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X; using symbol_shorthand::X;
@ -299,8 +299,10 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// After eliminating the point, A1 and A2 contain 2-rank information on cameras: // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
Matrix16 A1, A2; Matrix16 A1, A2;
A1 << -1000, 0, 0, 0, 100, 0; A1 << -10, 0, 0, 0, 1, 0;
A2 << 1000, 0, 100, 0, -100, 0; A2 << 10, 0, 1, 0, -1, 0;
A1 *= 10. / sigma;
A2 *= 10. / sigma;
Matrix expectedInformation; // filled below Matrix expectedInformation; // filled below
{ {
// createHessianFactor // createHessianFactor
@ -339,21 +341,38 @@ TEST( SmartProjectionPoseFactor, Factors ) {
F2(1, 0) = 100; F2(1, 0) = 100;
F2(1, 2) = 10; F2(1, 2) = 10;
F2(1, 4) = -10; F2(1, 4) = -10;
Matrix43 E; Matrix E(4, 3);
E.setZero(); E.setZero();
E(0, 0) = 100; E(0, 0) = 10;
E(1, 1) = 100; E(1, 1) = 10;
E(2, 0) = 100; E(2, 0) = 10;
E(2, 2) = 10; E(2, 2) = 1;
E(3, 1) = 100; E(3, 1) = 10;
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > // vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, F1))(make_pair(x2, F2)); (make_pair(x1, F1))(make_pair(x2, F2));
Matrix3 P = (E.transpose() * E).inverse(); Vector b(4);
Vector4 b;
b.setZero(); b.setZero();
// createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
Matrix3 P = (E.transpose() * E).inverse();
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0);
CHECK(actualQ);
EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8));
EXPECT(assert_equal(expectedQ, *actualQ));
// Whiten for RegularImplicitSchurFactor (does not have noise model)
model->WhitenSystem(E, b);
Matrix3 whiteP = (E.transpose() * E).inverse();
BOOST_FOREACH(SmartFactor::KeyMatrix2D& Fblock,Fblocks)
Fblock.second = model->Whiten(Fblock.second);
// createRegularImplicitSchurFactor // createRegularImplicitSchurFactor
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual = boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
@ -361,17 +380,6 @@ TEST( SmartProjectionPoseFactor, Factors ) {
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8)); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
// createJacobianQFactor
SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma);
JacobianFactorQ<6, 2> expectedQ(Fblocks, E, P, b, n);
EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-8));
boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ =
smartFactor1->createJacobianQFactor(cameras, 0.0);
CHECK(actual);
EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-8));
EXPECT(assert_equal(expectedQ, *actualQ));
} }
{ {