From 7bba8c42e446aa91ee95405aebd903dfc70189e1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 21 Dec 2014 14:59:59 +0100
Subject: [PATCH] Fixed testQuaternion

---
 gtsam/geometry/Quaternion.h             | 36 +++++++++++++------
 gtsam/geometry/tests/testQuaternion.cpp | 48 +++++++++++++++++++++----
 2 files changed, 68 insertions(+), 16 deletions(-)

diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h
index cb1a3a0df..31bfd9271 100644
--- a/gtsam/geometry/Quaternion.h
+++ b/gtsam/geometry/Quaternion.h
@@ -31,7 +31,27 @@ struct traits_x<QUATERNION_TYPE> {
   typedef lie_group_tag structure_category;
   typedef multiplicative_group_tag group_flavor;
 
-  /// @name Basic Manifold traits
+  /// @name Group traits
+  /// @{
+  static Q Identity() {
+    return Q::Identity();
+  }
+
+  static Q Compose(const Q &g, const Q & h) {
+    return g * h;
+  }
+
+  static Q Between(const Q &g, const Q & h) {
+    Q d = g.inverse() * h;
+    return d;
+  }
+
+  static Q Inverse(const Q &g) {
+    return g.inverse();
+  }
+
+  /// @}
+  /// @name Basic manifold traits
   /// @{
   enum {
     dimension = 3
@@ -42,12 +62,8 @@ struct traits_x<QUATERNION_TYPE> {
   /// @}
   /// @name Lie group traits
   /// @{
-  static Q Identity() {
-    return Q::Identity();
-  }
-
-  static Q Compose(const Q &g, const Q & h, ChartJacobian Hg = boost::none,
-      ChartJacobian Hh = boost::none) {
+  static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
+      boost::none) {
     if (Hg)
       *Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
     if (Hh)
@@ -55,8 +71,8 @@ struct traits_x<QUATERNION_TYPE> {
     return g * h;
   }
 
-  static Q Between(const Q &g, const Q & h, ChartJacobian Hg = boost::none,
-      ChartJacobian Hh = boost::none) {
+  static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
+      boost::none) {
     Q d = g.inverse() * h;
     if (Hg)
       *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
@@ -65,7 +81,7 @@ struct traits_x<QUATERNION_TYPE> {
     return d;
   }
 
-  static Q Inverse(const Q &g, ChartJacobian H = boost::none) {
+  static Q Inverse(const Q &g, ChartJacobian H) {
     if (H)
       *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
     return g.inverse();
diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp
index 19a0338fd..2eff36bb6 100644
--- a/gtsam/geometry/tests/testQuaternion.cpp
+++ b/gtsam/geometry/tests/testQuaternion.cpp
@@ -16,6 +16,7 @@
  **/
 
 #include <gtsam/geometry/Quaternion.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -40,7 +41,7 @@ TEST(Quaternion , Constructor) {
 TEST(Quaternion , Invariants) {
   Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
   Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
-  check_group_invariants(q1,q2);
+  check_group_invariants(q1, q2);
 }
 
 //******************************************************************************
@@ -48,7 +49,7 @@ TEST(Quaternion , Local) {
   Vector3 z_axis(0, 0, 1);
   Q q1(Eigen::AngleAxisd(0, z_axis));
   Q q2(Eigen::AngleAxisd(0.1, z_axis));
-  QuaternionJacobian H1,H2;
+  QuaternionJacobian H1, H2;
   Vector3 expected(0, 0, 0.1);
   Vector3 actual = traits_x<Q>::Local(q1, q2, H1, H2);
   EXPECT(assert_equal((Vector)expected,actual));
@@ -60,24 +61,59 @@ TEST(Quaternion , Retract) {
   Q q(Eigen::AngleAxisd(0, z_axis));
   Q expected(Eigen::AngleAxisd(0.1, z_axis));
   Vector3 v(0, 0, 0.1);
-  QuaternionJacobian Hq,Hv;
+  QuaternionJacobian Hq, Hv;
   Q actual = traits_x<Q>::Retract(q, v, Hq, Hv);
   EXPECT(actual.isApprox(expected));
 }
 
 //******************************************************************************
 TEST(Quaternion , Compose) {
-  EXPECT(false);
+  Vector3 z_axis(0, 0, 1);
+  Q q1(Eigen::AngleAxisd(0.2, z_axis));
+  Q q2(Eigen::AngleAxisd(0.1, z_axis));
+
+  Q expected = q1 * q2;
+  Matrix actualH1, actualH2;
+  Q actual = traits_x<Q>::Compose(q1, q2, actualH1, actualH2);
+  EXPECT(traits_x<Q>::Equals(expected,actual));
+
+  Matrix numericalH1 = numericalDerivative21(traits_x<Q>::Compose, q1, q2);
+  EXPECT(assert_equal(numericalH1,actualH1));
+
+  Matrix numericalH2 = numericalDerivative22(traits_x<Q>::Compose, q1, q2);
+  EXPECT(assert_equal(numericalH2,actualH2));
 }
 
 //******************************************************************************
 TEST(Quaternion , Between) {
-  EXPECT(false);
+  Vector3 z_axis(0, 0, 1);
+  Q q1(Eigen::AngleAxisd(0.2, z_axis));
+  Q q2(Eigen::AngleAxisd(0.1, z_axis));
+
+  Q expected = q1.inverse() * q2;
+  Matrix actualH1, actualH2;
+  Q actual = traits_x<Q>::Between(q1, q2, actualH1, actualH2);
+  EXPECT(traits_x<Q>::Equals(expected,actual));
+
+  Matrix numericalH1 = numericalDerivative21(traits_x<Q>::Between, q1, q2);
+  EXPECT(assert_equal(numericalH1,actualH1));
+
+  Matrix numericalH2 = numericalDerivative22(traits_x<Q>::Between, q1, q2);
+  EXPECT(assert_equal(numericalH2,actualH2));
 }
 
 //******************************************************************************
 TEST(Quaternion , Inverse) {
-  EXPECT(false);
+  Vector3 z_axis(0, 0, 1);
+  Q q1(Eigen::AngleAxisd(0.1, z_axis));
+  Q expected(Eigen::AngleAxisd(-0.1, z_axis));
+
+  Matrix actualH;
+  Q actual = traits_x<Q>::Inverse(q1, actualH);
+  EXPECT(traits_x<Q>::Equals(expected,actual));
+
+  Matrix numericalH = numericalDerivative11(traits_x<Q>::Inverse, q1);
+  EXPECT(assert_equal(numericalH,actualH));
 }
 
 //******************************************************************************