From 34c6348d456295292f82d8c12c1e114eec92daa4 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 30 May 2019 10:51:16 -0400
Subject: [PATCH] Added some more overloads, including if Point3 != Vector3

---
 gtsam/slam/expressions.h | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
index b0791b506..17a8d849f 100644
--- a/gtsam/slam/expressions.h
+++ b/gtsam/slam/expressions.h
@@ -41,6 +41,7 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
 }
 
 namespace internal {
+// define getter that returns value rather than reference
 Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
   return pose.rotation(H);
 }
@@ -54,10 +55,42 @@ inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
   return Point3_(x, &Rot3::rotate, p);
 }
 
+inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
+  return Unit3_(x, &Rot3::rotate, p);
+}
+
 inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
   return Point3_(x, &Rot3::unrotate, p);
 }
 
+inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
+  return Unit3_(x, &Rot3::unrotate, p);
+}
+
+#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+namespace internal {
+// define a rotate and unrotate for Vector3
+Vector3 rotate(const Rot3& R, const Vector3& v,
+               OptionalJacobian<3, 3> H1 = boost::none,
+               OptionalJacobian<3, 3> H2 = boost::none) {
+  return R.rotate(v, H1, H2);
+}
+Vector3 unrotate(const Rot3& R, const Vector3& v,
+                 OptionalJacobian<3, 3> H1 = boost::none,
+                 OptionalJacobian<3, 3> H2 = boost::none) {
+  return R.unrotate(v, H1, H2);
+}
+}  // namespace internal
+inline Expression<Vector3> rotate(const Rot3_& R,
+                                  const Expression<Vector3>& v) {
+  return Expression<Vector3>(internal::rotate, R, v);
+}
+inline Expression<Vector3> unrotate(const Rot3_& R,
+                                    const Expression<Vector3>& v) {
+  return Expression<Vector3>(internal::unrotate, R, v);
+}
+#endif
+
 // Projection
 
 typedef Expression<Cal3_S2> Cal3_S2_;