Rot3 action on Vector3, and templated constructor

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);
+    template<typename Derived>
-
+    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
-    /** constructor from a rotation matrix */
+      #ifdef GTSAM_USE_QUATERNIONS
-    Rot3(const Matrix& R);
+        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;
 

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()) {
 }

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) {