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

Update tests for testPoseRotationPrior 

Adds new test for wraparound handling in Rot2.
Removes tests that are not quite equal for Rot3.
release/4.3a0
Lachlan Toohey 2014-10-09 14:13:24 +11:00
parent 2000dbb868
commit 3c42fbacb9
1 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3)
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01);
/* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
@ -61,11 +62,17 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
EXPECT(assert_equal((Vector(3) << 0.1,0.2,0.3), factor.evaluateError(pose1, actH1)));
// the derivative is more complex, but is close to the identity for Rot3 around the origin
/*
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
boost::bind(evalFactorError3, factor, _1), pose1, 1e-2);
EXPECT(assert_equal(expH1, actH1, tol));*/
#else
// If not using true expmap will be close, but not exact around the origin
#endif
}
/* ************************************************************************* */
@ -90,6 +97,17 @@ TEST( testPoseRotationFactor, level2_error ) {
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2 pose1(rot2C, point2A);
Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */