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
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
using symbol_shorthand::X;
@ -299,8 +299,10 @@ TEST( SmartProjectionPoseFactor, Factors ) {
// 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;
A1 << -10, 0, 0, 0, 1, 0;
A2 << 10, 0, 1, 0, -1, 0;
A1 *= 10. / sigma;
A2 *= 10. / sigma;
Matrix expectedInformation; // filled below
{
// createHessianFactor
@ -339,21 +341,38 @@ TEST( SmartProjectionPoseFactor, Factors ) {
F2(1, 0) = 100;
F2(1, 2) = 10;
F2(1, 4) = -10;
Matrix43 E;
Matrix E(4, 3);
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> > //
E(0, 0) = 10;
E(1, 1) = 10;
E(2, 0) = 10;
E(2, 2) = 1;
E(3, 1) = 10;
vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(x1, F1))(make_pair(x2, F2));
Matrix3 P = (E.transpose() * E).inverse();
Vector4 b;
Vector b(4);
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
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b);
RegularImplicitSchurFactor<6> expected(Fblocks, E, whiteP, b);
boost::shared_ptr<RegularImplicitSchurFactor<6> > actual =
smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0);
@ -361,17 +380,6 @@ TEST( SmartProjectionPoseFactor, Factors ) {
EXPECT(assert_equal(expectedInformation, expected.information(), 1e-8));
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-8));
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));
}
{