diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index fc2f60f353..b0791b506c 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); } diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp index 66b236ac2b..294b821d38 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;