From 6baa7e102b29ae5009f0b9fb4f63f5c8ae972b8e Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Mon, 27 Mar 2023 17:28:45 -0400 Subject: [PATCH 1/4] Adds jacobians for Pose2 component extraction Adds jacobians to translation() and rotation() for Pose2 to bring it into spec with Pose3's equilivent functions. Also adds tests. --- gtsam/geometry/Pose2.cpp | 15 +++++++++++++++ gtsam/geometry/Pose2.h | 8 ++++---- gtsam/geometry/tests/testPose2.cpp | 27 +++++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0d9f1bc01..4892af60a 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -32,6 +32,21 @@ GTSAM_CONCEPT_POSE_INST(Pose2) static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); +/* ************************************************************************* */ +const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const { + if (Hself) { + *Hself = Matrix::Zero(2, 3); + (*Hself).block<2, 2>(0, 0) = rotation().matrix(); + } + return t_; +} + +/* ************************************************************************* */ +const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const { + if (Hself) *Hself << 0, 0, 1; + return r_; +} + /* ************************************************************************* */ Matrix3 Pose2::matrix() const { Matrix2 R = r_.matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index c3f588dcc..fa148b1c5 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -252,16 +252,16 @@ public: inline double theta() const { return r_.theta(); } /// translation - inline const Point2& t() const { return t_; } + inline const Point2& t() const { return translation(); } /// rotation - inline const Rot2& r() const { return r_; } + inline const Rot2& r() const { return rotation(); } /// translation - inline const Point2& translation() const { return t_; } + const Point2& translation(OptionalJacobian<2, 3> Hself={}) const; /// rotation - inline const Rot2& rotation() const { return r_; } + const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const; //// return transformation matrix GTSAM_EXPORT Matrix3 matrix() const; diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 8c1bdccc0..b52a6e590 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix ) EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT } + + +/* ************************************************************************* */ +TEST( Pose2, translation ) { + Pose2 pose(3.5, -8.2, 4.2); + + Matrix actualH; + EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8)); + + std::function f = [](const Pose2& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +TEST( Pose2, rotation ) { + Pose2 pose(3.5, -8.2, 4.2); + + Matrix actualH(4, 3); + EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8)); + + std::function f = [](const Pose2& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + + /* ************************************************************************* */ TEST( Pose2, between ) { From d6a24847f182e76f9140f8e0365fee3de8410c2f Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Mon, 27 Mar 2023 18:05:18 -0400 Subject: [PATCH 2/4] Extends python iface Pose2 component jacobians --- gtsam/geometry/geometry.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index e9929227a..4ea322fa7 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -424,7 +424,9 @@ class Pose2 { gtsam::Rot2 bearing(const gtsam::Point2& point) const; double range(const gtsam::Point2& point) const; gtsam::Point2 translation() const; + gtsam::Point2 translation(Eigen::Ref Hself) const; gtsam::Rot2 rotation() const; + gtsam::Rot2 rotation(Eigen::Ref Hself) const; Matrix matrix() const; // enabling serialization functionality From 7e30cc7c9d4ec295ea0a08171ad238681b52ce08 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Sun, 9 Apr 2023 11:38:40 -0400 Subject: [PATCH 3/4] Convert Pose2 r/t accessors to inline --- gtsam/geometry/Pose2.cpp | 15 --------------- gtsam/geometry/Pose2.h | 13 +++++++++++-- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 4892af60a..0d9f1bc01 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -32,21 +32,6 @@ GTSAM_CONCEPT_POSE_INST(Pose2) static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); -/* ************************************************************************* */ -const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const { - if (Hself) { - *Hself = Matrix::Zero(2, 3); - (*Hself).block<2, 2>(0, 0) = rotation().matrix(); - } - return t_; -} - -/* ************************************************************************* */ -const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const { - if (Hself) *Hself << 0, 0, 1; - return r_; -} - /* ************************************************************************* */ Matrix3 Pose2::matrix() const { Matrix2 R = r_.matrix(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index fa148b1c5..80418745e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -258,10 +258,19 @@ public: inline const Rot2& r() const { return rotation(); } /// translation - const Point2& translation(OptionalJacobian<2, 3> Hself={}) const; + inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const { + if (Hself) { + *Hself = Matrix::Zero(2, 3); + (*Hself).block<2, 2>(0, 0) = rotation().matrix(); + } + return t_; + } /// rotation - const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const; + inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const { + if (Hself) *Hself << 0, 0, 1; + return r_; + } //// return transformation matrix GTSAM_EXPORT Matrix3 matrix() const; From 2175f804ddb46d9e0731e2b92899e7f226a8f089 Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Sun, 9 Apr 2023 14:18:53 -0400 Subject: [PATCH 4/4] Revert changes to Pose2 r(), t() --- gtsam/geometry/Pose2.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 80418745e..f1b38c5a6 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -252,10 +252,10 @@ public: inline double theta() const { return r_.theta(); } /// translation - inline const Point2& t() const { return translation(); } + inline const Point2& t() const { return t_; } /// rotation - inline const Rot2& r() const { return rotation(); } + inline const Rot2& r() const { return r_; } /// translation inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {