fix Rot3-from-Matrix construction for Quaternion case

gtsam/geometry/Pose3.cpp

@@ -387,7 +387,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   Matrix3 UVtranspose = U * V.transpose();
   Matrix3 detWeighting = I_3x3;
   detWeighting(2, 2) = UVtranspose.determinant();
-  Rot3 R(Matrix(V * detWeighting * U.transpose()));
+  Rot3 R(Matrix3(V * detWeighting * U.transpose()));
   Point3 t = Point3(cq) - R * Point3(cp);
   return Pose3(R, t);
 }

gtsam/geometry/Rot3.h

@@ -86,14 +86,10 @@ namespace gtsam {
         double R31, double R32, double R33);
 
     /** constructor from a rotation matrix */
-    template<typename Derived>
-    inline Rot3(const Eigen::MatrixBase<Derived>& R) {
-      #ifdef GTSAM_USE_QUATERNIONS
-        quaternion_=R
-      #else
-        rot_ = R;
-      #endif
-    }
+    Rot3(const Matrix3& R);
+
+    /** constructor from a rotation matrix */
+    Rot3(const Matrix& R);
 
     /** 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,6 +50,18 @@ 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,6 +48,14 @@ 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)(Matrix(3,3) <<
+static const Pose3 pose1((Matrix(3,3) <<
               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((Matrix)(Matrix(3,3) <<
+static const Pose3 pose1((Matrix3)(Matrix(3,3) <<
               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_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
+      R_((Matrix3)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) :

gtsam/navigation/tests/testAHRSFactor.cpp

@@ -435,7 +435,7 @@ TEST (AHRSFactor, predictTest) {
   // Actual Jacobians
   Matrix H;
   (void) pim.predict(bias,H);
-  EXPECT(assert_equal(expectedH, H));
+  EXPECT(assert_equal(expectedH, H, 1e-8));
 }
 //******************************************************************************
 #include <gtsam/linear/GaussianFactorGraph.h>