fix Rot3 constructor from a matrix. Revert to the generic template version, but provide a overload version for Matrix3 to avoid casting.

gtsam/geometry/Rot3.h

@@ -85,11 +85,33 @@ namespace gtsam {
         double R21, double R22, double R23,
         double R31, double R32, double R33);
 
-    /** constructor from a rotation matrix */
-    Rot3(const Matrix3& R);
+    /**
+     * Constructor from a rotation matrix
+     * Version for generic matrices. Need casting to Matrix3
+     * in quaternion mode, since Eigen's quaternion doesn't
+     * allow assignment/construction from a generic matrix.
+     * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
+     */
+    template<typename Derived>
+    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
+      #ifdef GTSAM_USE_QUATERNIONS
+        quaternion_=Matrix3(R);
+      #else
+        rot_ = R;
+      #endif
+    }
 
-    /** constructor from a rotation matrix */
-    Rot3(const Matrix& R);
+    /**
+     * Constructor from a rotation matrix
+     * Overload version for Matrix3 to avoid casting in quaternion mode.
+     */
+    inline Rot3(const Matrix3& 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

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

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -30,7 +30,7 @@ using namespace gtsam;
 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
 
 // Camera situated at 0.5 meters high, looking down
-static const Pose3 pose1((Matrix(3,3) <<
+static const Pose3 pose1((Matrix3() <<
               1., 0., 0.,
               0.,-1., 0.,
               0., 0.,-1.

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -28,7 +28,7 @@ using namespace gtsam;
 
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1((Matrix3)(Matrix(3,3) <<
+static const Pose3 pose1((Matrix3() <<
               1., 0., 0.,
               0.,-1., 0.,
               0., 0.,-1.

gtsam/navigation/NavState.h

@@ -61,7 +61,7 @@ public:
   }
   /// Construct from Matrix group representation (no checking)
   NavState(const Matrix7& T) :
-      R_((Matrix3)T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
+      R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
   }
   /// Construct from SO(3) and R^6
   NavState(const Matrix3& R, const Vector9 tv) :