From 84628cd511de83bd4ef724967172525b89579cb8 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Mon, 21 Dec 2015 15:11:35 -0800
Subject: [PATCH] Added Vector3 methods from develop

---
 gtsam/geometry/Rot3.h | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 608f41954..cc0dc309e 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -330,6 +330,17 @@ namespace gtsam {
     Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
         OptionalJacobian<3,3> H2 = boost::none) const;
 
+    /// operator* for Vector3
+    inline Vector3 operator*(const Vector3& v) const {
+      return rotate(Point3(v)).vector();
+    }
+
+    /// rotate for Vector3
+    Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
+        OptionalJacobian<3, 3> H2 = boost::none) const {
+      return rotate(Point3(v), H1, H2).vector();
+    }
+
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
@@ -337,6 +348,12 @@ namespace gtsam {
     Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
         OptionalJacobian<3,3> H2=boost::none) const;
 
+    /// unrotate for Vector3
+    Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
+        OptionalJacobian<3, 3> H2 = boost::none) const {
+      return unrotate(Point3(v), H1, H2).vector();
+    }
+
     /// @}
     /// @name Group Action on Unit3
     /// @{