From 56229eda033ddb4588dd60c062a57b08cc391e2f Mon Sep 17 00:00:00 2001
From: Brice Rebsamen <brice.rebsamen@gmail.com>
Date: Wed, 22 May 2019 11:52:14 -0700
Subject: [PATCH] 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);
 }