From bc99c58226fb71bee35a1f922937e0028e5b8274 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <dta-huynh@irobot.com>
Date: Thu, 17 Sep 2015 17:44:00 -0400
Subject: [PATCH] fix Rot3-from-Matrix construction for Quaternion case

---
 gtsam/geometry/Pose3.cpp                      |  2 +-
 gtsam/geometry/Rot3.h                         | 12 ++++--------
 gtsam/geometry/Rot3M.cpp                      | 12 ++++++++++++
 gtsam/geometry/Rot3Q.cpp                      |  8 ++++++++
 gtsam/geometry/tests/testCalibratedCamera.cpp |  2 +-
 gtsam/geometry/tests/testSimpleCamera.cpp     |  2 +-
 gtsam/navigation/NavState.h                   |  2 +-
 gtsam/navigation/tests/testAHRSFactor.cpp     |  2 +-
 8 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 3f46df40d..f373e5ad4 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/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);
 }
diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 3e95bf04d..cc0dc309e 100644
--- a/gtsam/geometry/Rot3.h
+++ b/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
diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp
index a7b654070..324c05ae4 100644
--- a/gtsam/geometry/Rot3M.cpp
+++ b/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()) {
 }
diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index b255b082d..c97e76718 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/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) {
diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp
index 199ae30ce..e7c66c48d 100644
--- a/gtsam/geometry/tests/testCalibratedCamera.cpp
+++ b/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.
diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp
index 71979481c..3755573ac 100644
--- a/gtsam/geometry/tests/testSimpleCamera.cpp
+++ b/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.
diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h
index 9561aa77b..c6c11627c 100644
--- a/gtsam/navigation/NavState.h
+++ b/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) :
diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp
index 9e8e78efe..2121eda35 100644
--- a/gtsam/navigation/tests/testAHRSFactor.cpp
+++ b/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>