diff --git a/cpp/Pose2.cpp b/cpp/Pose2.cpp index d16f91e46..530b8615d 100644 --- a/cpp/Pose2.cpp +++ b/cpp/Pose2.cpp @@ -13,13 +13,13 @@ namespace gtsam { /** Explicit instantiation of base class to export members */ INSTANTIATE_LIE(Pose2); - static const Matrix I3 = eye(3); + static const Matrix I3 = eye(3), Z12 = zeros(1,2); + static const Rot2 R_PI_2(0., 1.); /* ************************************************************************* */ Matrix Pose2::matrix() const { Matrix R = r_.matrix(); - Matrix Z = zeros(1,2); - R = stack(2, &R, &Z); + R = stack(2, &R, &Z12); Matrix T = Matrix_(3,1, t_.x(), t_.y(), 1.0); return collect(2, &R, &T); } @@ -34,6 +34,31 @@ namespace gtsam { return t_.equals(q.t_, tol) && r_.equals(q.r_, tol); } + /* ************************************************************************* */ + // Calculate Adjoint map + // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) + Matrix AdjointMap(const Pose2& p) { + const Rot2 R = p.r(); + const Point2 t = p.t(); + double c = R.c(), s = R.s(), x = t.x(), y = t.y(); + return Matrix_(3,3, + c, -s, y, + s, c, -x, + 0.0, 0.0, 1.0 + ); + } + + /* ************************************************************************* */ + Pose2 inverse(const Pose2& pose) { + const Rot2& R = pose.r(); + const Point2& t = pose.t(); + return Pose2(inverse(R), R.unrotate(Point2(-t.x(), -t.y()))); + } + + Matrix Dinverse(const Pose2& pose) { + return -AdjointMap(pose); + } + /* ************************************************************************* */ Point2 transform_to(const Pose2& pose, const Point2& point, boost::optional< Matrix&> H1, boost::optional H2) { @@ -56,20 +81,12 @@ 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); + return AdjointMap(inverse(p2)); } Matrix Dcompose2(const Pose2& p1, const Pose2& p2) { - return eye(3); + return I3; } /* ************************************************************************* */ @@ -97,8 +114,7 @@ namespace gtsam { double x = dt.x(), y = dt.y(); Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - // FD: I don't understand this code (a performance-driven transformation - // from Richard's heavier code) but it works. + // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; diff --git a/cpp/Pose2.h b/cpp/Pose2.h index c8688a723..4b363c0f9 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -71,10 +71,15 @@ namespace gtsam { /** return DOF, dimensionality of tangent space = 3 */ inline size_t dim(const Pose2&) { return 3; } + /** + * Calculate Adjoint map + * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) + */ + Matrix AdjointMap(const Pose2& p); + /** inverse transformation */ - inline Pose2 inverse(const Pose2& pose) { - return Pose2(inverse(pose.r()), - pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); } + Pose2 inverse(const Pose2& pose); + Matrix Dinverse(const Pose2& pose); /** compose this transformation onto another (first p1 and then p2) */ inline Pose2 compose(const Pose2& p0, const Pose2& p1) { diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index c0cf58930..47739d53d 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -148,8 +148,8 @@ TEST(Pose2, compose_a) 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); + Matrix actualDcompose1 = Dcompose1(pose1, pose2); + Matrix actualDcompose2 = Dcompose2(pose1, pose2); Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5))); CHECK(assert_equal(expected, actual)); @@ -162,10 +162,10 @@ TEST(Pose2, compose_a) 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)); + CHECK(assert_equal(expectedH1,actualDcompose1)); + CHECK(assert_equal(numericalH1,actualDcompose1)); + CHECK(assert_equal(expectedH2,actualDcompose2)); + CHECK(assert_equal(numericalH2,actualDcompose2)); Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 expected_point(-1.0, -1.0); @@ -186,13 +186,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 actualDcompose1 = Dcompose1(pose1, pose2); + Matrix actualDcompose2 = 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(numericalH1,actualDcompose1)); + CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -209,13 +209,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 actualDcompose1 = Dcompose1(pose1, pose2); + Matrix actualDcompose2 = 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(numericalH1,actualDcompose1)); + CHECK(assert_equal(numericalH2,actualDcompose2)); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -234,8 +234,12 @@ TEST(Pose2, inverse ) Point2 l(4,5), g(-4,6); CHECK(assert_equal(g,gTl*l)); CHECK(assert_equal(l,lTg*g)); -} + // Check derivative + Matrix numericalH = numericalDerivative11(inverse, lTg, 1e-5); + Matrix actualDinverse = Dinverse(lTg); + CHECK(assert_equal(numericalH,actualDinverse)); +} /* ************************************************************************* */ Vector homogeneous(const Point2& p) { @@ -304,7 +308,7 @@ TEST( Pose2, between ) Matrix actualH1,actualH2; Pose2 expected(M_PI_2, Point2(2,2)); - Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!! + Pose2 actual1 = between(gT1,gT2); Pose2 actual2 = between(gT1,gT2,actualH1,actualH2); CHECK(assert_equal(expected,actual1)); CHECK(assert_equal(expected,actual2)); @@ -317,6 +321,8 @@ TEST( Pose2, between ) Matrix numericalH1 = numericalDerivative21(between, gT1, gT2, 1e-5); CHECK(assert_equal(expectedH1,actualH1)); CHECK(assert_equal(numericalH1,actualH1)); + // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx + CHECK(assert_equal(-AdjointMap(between(gT2,gT1)),actualH1)); Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0, @@ -326,6 +332,7 @@ TEST( Pose2, between ) Matrix numericalH2 = numericalDerivative22(between, gT1, gT2, 1e-5); CHECK(assert_equal(expectedH2,actualH2)); CHECK(assert_equal(numericalH2,actualH2)); + } /* ************************************************************************* */