From 205d20d4dc1061ae83a9179bcb7d9e0ff8649de5 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 21 Jul 2015 11:22:49 -0700
Subject: [PATCH] Rot3 action on Vector3, and templated constructor

---
 gtsam/geometry/Rot3.h    | 23 +++++++++++++++++++----
 gtsam/geometry/Rot3M.cpp | 12 ------------
 gtsam/geometry/Rot3Q.cpp |  8 --------
 3 files changed, 19 insertions(+), 24 deletions(-)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 608f41954..1dec6eafe 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -86,10 +86,14 @@ namespace gtsam {
         double R31, double R32, double R33);
 
     /** constructor from a rotation matrix */
-    Rot3(const Matrix3& R);
-
-    /** constructor from a rotation matrix */
-    Rot3(const Matrix& R);
+    template<typename Derived>
+    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
+      #ifdef GTSAM_USE_QUATERNIONS
+        quaternion_=R
+      #else
+        rot_ = R;
+      #endif
+    }
 
     /** Constructor from a quaternion.  This can also be called using a plain
      * Vector, due to implicit conversion from Vector to Quaternion
@@ -330,6 +334,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;
 
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index 324c05ae4..a7b654070 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/gtsam/geometry/Rot3M.cpp
@@ -50,18 +50,6 @@ Rot3::Rot3(double R11, double R12, double R13,
         R31, R32, R33;
 }
 
-/* ************************************************************************* */
-Rot3::Rot3(const Matrix3& R) {
-  rot_ = R;
-}
-
-/* ************************************************************************* */
-Rot3::Rot3(const Matrix& R) {
-  if (R.rows()!=3 || R.cols()!=3)
-    throw invalid_argument("Rot3 constructor expects 3*3 matrix");
-  rot_ = R;
-}
-
 /* ************************************************************************* */
 Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
 }
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index c97e76718..b255b082d 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -48,14 +48,6 @@ namespace gtsam {
             R21, R22, R23,
             R31, R32, R33).finished()) {}
 
-  /* ************************************************************************* */
-  Rot3::Rot3(const Matrix3& R) :
-      quaternion_(R) {}
-
-  /* ************************************************************************* */
-  Rot3::Rot3(const Matrix& R) :
-      quaternion_(Matrix3(R)) {}
-
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) :
       quaternion_(q) {