diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index 41dd21daf..fd1a84acd 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -56,6 +56,22 @@ namespace gtsam { Matrix H; transform_to(pose, point, boost::none, H); return H; } + /* ************************************************************************* */ + static const Rot2 R_PI_2(0., 1.); + static Matrix DR1 = Matrix_(1, 3, 0., 0., 1.); + + Matrix Dcompose1(const Pose2& p1, const Pose2& p2) { + Matrix R2_inv = p2.r().transpose(); + Point2 Rt2_ = R_PI_2 * inverse(p2.r()) * p2.t(); + Matrix Rt2 = Matrix_(2, 1, Rt2_.x(), Rt2_.y()); + Matrix Dt1 = collect(2, &R2_inv, &Rt2); + return gtsam::stack(2, &Dt1, &DR1); + } + + Matrix Dcompose2(const Pose2& p1, const Pose2& p2) { + return eye(3); + } + /* ************************************************************************* */ Pose2 between(const Pose2& p1, const Pose2& p2, boost::optional H1, boost::optional H2) { diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 3be38d09e..53898cef7 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -76,9 +76,11 @@ namespace gtsam { return Pose2(inverse(pose.r()), pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); } - /** compose this transformation onto another (pre-multiply this*p1) */ + /** compose this transformation onto another (first p1 and then p2) */ inline Pose2 compose(const Pose2& p0, const Pose2& p1) { - return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } + return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } + Matrix Dcompose1(const Pose2& p1, const Pose2& p2); + Matrix Dcompose2(const Pose2& p1, const Pose2& p2); /** exponential and log maps around identity */ diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 35add58f8..dfa81d47a 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -120,10 +120,26 @@ TEST(Pose2, compose_a) Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5))); Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0)); + Pose2 actual = compose(pose1, pose2); + Matrix actualH1 = Dcompose1(pose1, pose2); + Matrix actualH2 = Dcompose2(pose1, pose2); + Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5))); - Pose2 actual = pose1 * pose2; CHECK(assert_equal(expected, actual)); + Matrix expectedH1 = Matrix_(3,3, + 0.0, 1.0, 0.0, + -1.0, 0.0, 2.0, + 0.0, 0.0, 1.0 + ); + Matrix expectedH2 = eye(3); + Matrix numericalH1 = numericalDerivative21(compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); + CHECK(assert_equal(expectedH1,actualH1)); + CHECK(assert_equal(numericalH1,actualH1)); + CHECK(assert_equal(expectedH2,actualH2)); + CHECK(assert_equal(numericalH2,actualH2)); + Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 expected_point(-1.0, -1.0); Point2 actual_point1 = transform_to(pose1 * pose2, point); @@ -143,6 +159,13 @@ TEST(Pose2, compose_b) Pose2 pose_actual_op = pose1 * pose2; Pose2 pose_actual_fcn = compose(pose1, pose2); + Matrix actualH1 = Dcompose1(pose1, pose2); + Matrix actualH2 = Dcompose2(pose1, pose2); + + Matrix numericalH1 = numericalDerivative21(compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); + CHECK(assert_equal(numericalH1,actualH1)); + CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -159,6 +182,13 @@ TEST(Pose2, compose_c) Pose2 pose_actual_op = pose1 * pose2; Pose2 pose_actual_fcn = compose(pose1,pose2); + Matrix actualH1 = Dcompose1(pose1, pose2); + Matrix actualH2 = Dcompose2(pose1, pose2); + + Matrix numericalH1 = numericalDerivative21(compose, pose1, pose2, 1e-5); + Matrix numericalH2 = numericalDerivative22(compose, pose1, pose2, 1e-5); + CHECK(assert_equal(numericalH1,actualH1)); + CHECK(assert_equal(numericalH2,actualH2)); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn));