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

Fixed compile problem in toolbox for PoseTranslationPrior. Added planar tests.

release/4.3a0
Alex Cunningham 2012-08-20 14:25:09 +00:00
parent 3794c2a7b7
commit e488871ffd
2 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/slam/PoseTranslationPrior.h
View File

@ -59,7 +59,7 @@ public:
/** Constructor that pulls the translation from an incoming POSE */ /** Constructor that pulls the translation from an incoming POSE */
PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model) PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model)
: Base(key, model), measured_(pose_z.translation()) { : Base(model, key), measured_(pose_z.translation()) {
} }
virtual ~PoseTranslationPrior() {} virtual ~PoseTranslationPrior() {}

31
gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp
View File

@ -29,11 +29,20 @@ const gtsam::Key poseKey = 1;
const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x)); return LieVector(factor.evaluateError(x));
} }
/* ************************************************************************* */
LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testRelativeElevationFactor, level3_zero_error ) { TEST( testRelativeElevationFactor, level3_zero_error ) {
Pose3 pose1(rot3A, point3A); Pose3 pose1(rot3A, point3A);
@ -100,6 +109,28 @@ TEST( testRelativeElevationFactor, smallrot3_error ) {
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
/* ************************************************************************* */
TEST( testRelativeElevationFactor, level2_zero_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
TEST( testRelativeElevationFactor, level2_error ) {
Pose2 pose1(rot2A, point2A);
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal(Vector_(2,-3.0,-4.0), 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); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */