From 74f5ae0d1d8a984fe3d8ef7db566b80142047a19 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 3 Oct 2011 18:28:55 +0000
Subject: [PATCH] Merged Paul's quaternion conversions, added documentation and
 unit tests

---
 gtsam/geometry/Rot3.h             | 24 ++++++++++++++++++++
 gtsam/geometry/tests/testRot3.cpp | 37 +++++++++++++++++++++++++++++++
 2 files changed, 61 insertions(+)

diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h
index 40ab945ef..5270de9c8 100644
--- a/gtsam/geometry/Rot3.h
+++ b/gtsam/geometry/Rot3.h
@@ -22,9 +22,12 @@
 #pragma once
 
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
 
 namespace gtsam {
 
+typedef Eigen::Quaterniond Quaternion;
+
   /**
    * @brief 3D Rotations represented as rotation matrices
    * @ingroup geometry
@@ -63,6 +66,17 @@ namespace gtsam {
       r2_(Point3(R(0,1), R(1,1), R(2,1))),
       r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
 
+    /** Constructor from a quaternion.  This can also be called using a plain
+     * Vector, due to implicit conversion from Vector to Quaternion
+     * @param q The quaternion
+     */
+    Rot3(const Quaternion& q) {
+      Eigen::Matrix3d R = q.toRotationMatrix();
+      r1_ = Point3(R.col(0));
+      r2_ = Point3(R.col(1));
+      r3_ = Point3(R.col(2));
+    }
+
     /** Static member function to generate some well known rotations */
 
     /**
@@ -149,6 +163,16 @@ namespace gtsam {
      */
     Vector rpy() const;
 
+    /** Compute the quaternion representation of this rotation.
+     * @return The quaternion
+     */
+    Quaternion toQuaternion() const {
+      return Quaternion((Eigen::Matrix3d() <<
+          r1_.x(), r2_.x(), r3_.x(),
+          r1_.y(), r2_.y(), r3_.y(),
+          r1_.z(), r2_.z(), r3_.z()).finished());
+    }
+
     /** dimension of the variable - used to autodetect sizes */
     inline static size_t Dim() { return dimension; }
 
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 5421351c0..ec7a60f31 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -431,6 +431,43 @@ TEST( Rot3, logmapStability ) {
   CHECK(assert_equal(w, actualw, 1e-15));
 }
 
+/* ************************************************************************* */
+TEST(Rot3, quaternion) {
+  // NOTE: This is also verifying the ability to convert Vector to Quaternion
+  Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
+  Rot3 R1 = Rot3(Matrix_(3,3,
+      0.271018623057411,   0.278786459830371,   0.921318086098018,
+      0.578529366719085,   0.717799701969298,  -0.387385285854279,
+     -0.769319620053772,   0.637998195662053,   0.033250932803219));
+
+  Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
+  Rot3 R2 = Rot3(Matrix_(3,3,
+      -0.207341903877828,   0.250149415542075,   0.945745528564780,
+       0.881304914479026,  -0.371869043667957,   0.291573424846290,
+       0.424630407073532,   0.893945571198514,  -0.143353873763946));
+
+  // Check creating Rot3 from quaternion
+  EXPECT(assert_equal(R1, Rot3(q1)));
+  EXPECT(assert_equal(R2, Rot3(q2)));
+
+  // Check converting Rot3 to quaterion
+  EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
+  EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
+
+  // Check that quaternion and Rot3 represent the same rotation
+  Point3 p1(1.0, 2.0, 3.0);
+  Point3 p2(8.0, 7.0, 9.0);
+
+  Point3 expected1 = R1*p1;
+  Point3 expected2 = R2*p2;
+
+  Point3 actual1 = Point3(q1*p1.vector());
+  Point3 actual2 = Point3(q2*p2.vector());
+
+  EXPECT(assert_equal(expected1, actual1));
+  EXPECT(assert_equal(expected2, actual2));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;