From c460c1476483c2be277b1f9b81ee4b6c5aaeb23c Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 22 May 2019 11:48:43 -0700 Subject: [PATCH 1/2] unit tests --- gtsam/slam/tests/testSlamExpressions.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 66b236ac2..294b821d3 100644 --- a/gtsam/slam/tests/testSlamExpressions.cpp +++ b/gtsam/slam/tests/testSlamExpressions.cpp @@ -45,6 +45,19 @@ TEST(SlamExpressions, project2) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(point2_expr, values, 1e-7, 1e-5); } +/* ************************************************************************* */ +TEST(SlamExpressions, rotation) { + Pose3_ T_(0); + const Rot3_ R_ = rotation(T_); +} + +/* ************************************************************************* */ +TEST(SlamExpressions, unrotate) { + Rot3_ R_(0); + Point3_ p_(1); + const Point3_ q_ = unrotate(R_, p_); +} + /* ************************************************************************* */ int main() { TestResult tr; From 56229eda033ddb4588dd60c062a57b08cc391e2f Mon Sep 17 00:00:00 2001 From: Brice Rebsamen Date: Wed, 22 May 2019 11:52:14 -0700 Subject: [PATCH 2/2] add rotation wrapper --- gtsam/slam/expressions.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index fc2f60f35..b0791b506 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -40,6 +40,16 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformFrom, p); } +namespace internal { +Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) { + return pose.rotation(H); +} +} // namespace internal + +inline Rot3_ rotation(const Pose3_& pose) { + return Rot3_(internal::rotation, pose); +} + inline Point3_ rotate(const Rot3_& x, const Point3_& p) { return Point3_(x, &Rot3::rotate, p); }