From e488871ffdd7220d7cda7526efdaee47f4694c36 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 20 Aug 2012 14:25:09 +0000 Subject: [PATCH] Fixed compile problem in toolbox for PoseTranslationPrior. Added planar tests. --- gtsam_unstable/slam/PoseTranslationPrior.h | 2 +- .../slam/tests/testPoseTranslationPrior.cpp | 31 +++++++++++++++++++ 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/PoseTranslationPrior.h b/gtsam_unstable/slam/PoseTranslationPrior.h index 61cf77263..ba023c0e3 100644 --- a/gtsam_unstable/slam/PoseTranslationPrior.h +++ b/gtsam_unstable/slam/PoseTranslationPrior.h @@ -59,7 +59,7 @@ public: /** Constructor that pulls the translation from an incoming POSE */ 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() {} diff --git a/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp index 1db155fc1..0821e566b 100644 --- a/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam_unstable/slam/tests/testPoseTranslationPrior.cpp @@ -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 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) { return LieVector(factor.evaluateError(x)); } +/* ************************************************************************* */ +LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { + return LieVector(factor.evaluateError(x)); +} + /* ************************************************************************* */ TEST( testRelativeElevationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); @@ -100,6 +109,28 @@ TEST( testRelativeElevationFactor, smallrot3_error ) { 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( + 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( + boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + EXPECT(assert_equal(expH1, actH1, tol)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */