From a5877a96b89e949615a15af4a0fc72d63189b9f9 Mon Sep 17 00:00:00 2001
From: nsrinivasan7 <natesh.rvce@gmail.com>
Date: Fri, 5 Dec 2014 16:01:10 -0500
Subject: [PATCH] @dellaert. Works with quaternion switch on in gcc 4.7, 4.8
 and MSVC 2012. Pls Merge if compiles on MAC

---
 gtsam/geometry/Rot3Q.cpp | 21 ++++-----------------
 1 file changed, 4 insertions(+), 17 deletions(-)

diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index 19de92ca2..26ca25bf2 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -87,22 +87,9 @@ namespace gtsam {
   Rot3 Rot3::rodriguez(const Vector& w, double theta) {
     return Quaternion(Eigen::AngleAxisd(theta, w)); }
 
-  /* ************************************************************************* */
-  Rot3 Rot3::compose(const Rot3& R2) const {
-    return Rot3(quaternion_ * R2.quaternion_);
-  }
-
   /* ************************************************************************* */
   Rot3 Rot3::compose(const Rot3& R2,
-  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
-    if (H1) *H1 = R2.transpose();
-    if (H2) *H2 = I3;
-    return Rot3(quaternion_ * R2.quaternion_);
-  }
-
-  /* ************************************************************************* */
-  Rot3 Rot3::compose(const Rot3& R2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
     if (H1) *H1 = R2.transpose();
     if (H2) *H2 = I3;
     return Rot3(quaternion_ * R2.quaternion_);
@@ -114,7 +101,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
+  Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
     if (H1) *H1 = -matrix();
     return Rot3(quaternion_.inverse());
   }
@@ -129,7 +116,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Rot3 Rot3::between(const Rot3& R2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
     if (H1) *H1 = -(R2.transpose()*matrix());
     if (H2) *H2 = I3;
     return between_default(*this, R2);
@@ -137,7 +124,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Point3 Rot3::rotate(const Point3& p,
-        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
+        OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
     Matrix R = matrix();
     if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
     if (H2) *H2 = R;