Add the Quaternion manifold and tests.

Change-Id: I0a396a5834168c1526d6f2b8c99a1ad3348ad657

include/ceres/manifold.h

diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h
index 30ecae7..0867997 100644
--- a/include/ceres/manifold.h
+++ b/include/ceres/manifold.h
@@ -339,6 +339,48 @@
   int buffer_size_ = 0;
 };
 
+// Implements the manifold for a Hamilton quaternion as defined in
+// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as unit
+// norm 4-vectors, i.e.
+//
+// q = [q0; q1; q2; q3], |q| = 1
+//
+// is the ambient space representation.
+//
+//   q0  scalar part.
+//   q1  coefficient of i.
+//   q2  coefficient of j.
+//   q3  coefficient of k.
+//
+// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
+//
+// The tangent space is R^3, which relates to the ambient space through
+// the Plus and Minus operations defined as:
+//
+// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x
+//    Minus(y, x) = to_delta(y * x^{-1})
+//
+// where "*" is the quaternion product and because q is a unit quaternion
+// (|q|=1), q^-1 = [q0; -q1; -q2; -q3]
+//
+// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0)
+class CERES_EXPORT Quaternion : public Manifold {
+ public:
+  Quaternion() = default;
+  virtual ~Quaternion() = default;
+  int AmbientSize() const override { return 4; }
+  int TangentSize() const override { return 3; }
+
+  bool Plus(const double* x,
+            const double* delta,
+            double* x_plus_delta) const override;
+  bool PlusJacobian(const double* x, double* jacobian) const override;
+  bool Minus(const double* y,
+             const double* x,
+             double* y_minus_x) const override;
+  bool MinusJacobian(const double* x, double* jacobian) const override;
+};
+
 }  // namespace ceres
 
 // clang-format off

internal/ceres/manifold.cc

diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
index b0fead2..70b904c 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -1,5 +1,7 @@
 #include "ceres/manifold.h"
 
+#include <cmath>
+
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/fixed_array.h"
 #include "glog/logging.h"
@@ -272,4 +274,82 @@
   return true;
 }
 
+bool Quaternion::Plus(const double* x,
+                      const double* delta,
+                      double* x_plus_delta) const {
+  // x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the
+  // quaternion constructed from delta.
+  const double norm_delta = std::sqrt(
+      delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
+  if (norm_delta > 0.0) {
+    const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta);
+    double q_delta[4];
+    q_delta[0] = std::cos(norm_delta);
+    q_delta[1] = sin_delta_by_delta * delta[0];
+    q_delta[2] = sin_delta_by_delta * delta[1];
+    q_delta[3] = sin_delta_by_delta * delta[2];
+
+    // clang-format off
+    x_plus_delta[0] = q_delta[0] * x[0] - q_delta[1] * x[1] - q_delta[2] * x[2] - q_delta[3] * x[3];
+    x_plus_delta[1] = q_delta[0] * x[1] + q_delta[1] * x[0] + q_delta[2] * x[3] - q_delta[3] * x[2];
+    x_plus_delta[2] = q_delta[0] * x[2] - q_delta[1] * x[3] + q_delta[2] * x[0] + q_delta[3] * x[1];
+    x_plus_delta[3] = q_delta[0] * x[3] + q_delta[1] * x[2] - q_delta[2] * x[1] + q_delta[3] * x[0];
+    // clang-format on
+  } else {
+    for (int i = 0; i < 4; ++i) {
+      x_plus_delta[i] = x[i];
+    }
+  }
+  return true;
+}
+
+bool Quaternion::PlusJacobian(const double* x, double* jacobian) const {
+  // clang-format off
+  jacobian[0] = -x[1];  jacobian[1]  = -x[2];   jacobian[2]  = -x[3];
+  jacobian[3] =  x[0];  jacobian[4]  =  x[3];   jacobian[5]  = -x[2];
+  jacobian[6] = -x[3];  jacobian[7]  =  x[0];   jacobian[8]  =  x[1];
+  jacobian[9] =  x[2];  jacobian[10] = -x[1];   jacobian[11] =  x[0];
+  // clang-format on
+  return true;
+}
+
+bool Quaternion::Minus(const double* y,
+                       const double* x,
+                       double* y_minus_x) const {
+  // ambient_y_minus_x = QuaternionProduct(y, -x) where -x is the conjugate of
+  // x.
+  double ambient_y_minus_x[4];
+
+  // clang-format off
+  ambient_y_minus_x[0] =  y[0] * x[0] + y[1] * x[1] + y[2] * x[2] + y[3] * x[3];
+  ambient_y_minus_x[1] = -y[0] * x[1] + y[1] * x[0] - y[2] * x[3] + y[3] * x[2];
+  ambient_y_minus_x[2] = -y[0] * x[2] + y[1] * x[3] + y[2] * x[0] - y[3] * x[1];
+  ambient_y_minus_x[3] = -y[0] * x[3] - y[1] * x[2] + y[2] * x[1] + y[3] * x[0];
+  // clang-format on
+
+  const double u_norm = std::sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
+                                  ambient_y_minus_x[2] * ambient_y_minus_x[2] +
+                                  ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+  if (u_norm > 0.0) {
+    const double theta = std::atan2(u_norm, ambient_y_minus_x[0]);
+    y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+    y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+    y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+  } else {
+    y_minus_x[0] = 0.0;
+    y_minus_x[1] = 0.0;
+    y_minus_x[2] = 0.0;
+  }
+  return true;
+}
+
+bool Quaternion::MinusJacobian(const double* x, double* jacobian) const {
+  // clang-format off
+  jacobian[0] = - x[1]; jacobian[1]=  x[0]; jacobian[2]  = -x[3]; jacobian[3]  =   x[2];
+  jacobian[4] = - x[2]; jacobian[5]=  x[3]; jacobian[6]  =  x[0]; jacobian[7]  =  -x[1];
+  jacobian[8] = - x[3]; jacobian[9]= -x[2]; jacobian[10] =  x[1]; jacobian[11] =   x[0];
+  // clang-format on
+  return true;
+}
+
 }  // namespace ceres

internal/ceres/manifold_test.cc

diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index a3777ae..6c95fa6 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -548,5 +548,111 @@
   }
 }
 
+TEST(Quaternion, PlusPiBy2) {
+  Quaternion manifold;
+  Vector x = Vector::Zero(4);
+  x[0] = 1.0;
+
+  for (int i = 0; i < 3; ++i) {
+    Vector delta = Vector::Zero(3);
+    delta[i] = M_PI / 2;
+    Vector x_plus_delta = Vector::Zero(4);
+    EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
+
+    // Expect that the element corresponding to pi/2 is +/- 1. All other
+    // elements should be zero.
+    for (int j = 0; j < 4; ++j) {
+      if (i == (j - 1)) {
+        EXPECT_LT(std::abs(x_plus_delta[j]) - 1,
+                  std::numeric_limits<double>::epsilon())
+            << "\ndelta = " << delta.transpose()
+            << "\nx_plus_delta = " << x_plus_delta.transpose()
+            << "\n expected the " << j
+            << "th element of x_plus_delta to be +/- 1.";
+      } else {
+        EXPECT_LT(std::abs(x_plus_delta[j]),
+                  std::numeric_limits<double>::epsilon())
+            << "\ndelta = " << delta.transpose()
+            << "\nx_plus_delta = " << x_plus_delta.transpose()
+            << "\n expected the " << j << "th element of x_plus_delta to be 0.";
+      }
+    }
+    EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, x_plus_delta);
+  }
+}
+
+// Compute the expected value of Quaternion::Plus via functions in rotation.h
+// and compares it to the one computed by Quaternion::Plus.
+MATCHER_P2(QuaternionPlusIsCorrectAt, x, delta, "") {
+  // This multiplication by 2 is needed because AngleAxisToQuaternion uses
+  // |delta|/2 as the angle of rotation where as in the implementation of
+  // Quaternion for historical reasons we use |delta|.
+  const Vector two_delta = delta * 2;
+  Vector delta_q(4);
+  AngleAxisToQuaternion(two_delta.data(), delta_q.data());
+
+  Vector expected(4);
+  QuaternionProduct(delta_q.data(), x.data(), expected.data());
+  Vector actual(4);
+  EXPECT_TRUE(arg.Plus(x.data(), delta.data(), actual.data()));
+
+  const double n = (actual - expected).norm();
+  const double d = expected.norm();
+  const double diffnorm = n / d;
+  if (diffnorm > kEpsilon) {
+    *result_listener << "\nx: " << x.transpose()
+                     << "\ndelta: " << delta.transpose()
+                     << "\nexpected: " << expected.transpose()
+                     << "\nactual: " << actual.transpose()
+                     << "\ndiff: " << (expected - actual).transpose()
+                     << "\ndiffnorm : " << diffnorm;
+    return false;
+  }
+  return true;
+}
+
+Vector RandomQuaternion() {
+  Vector x = Vector::Random(4);
+  x.normalize();
+  return x;
+}
+
+TEST(Quaternion, GenericDelta) {
+  Quaternion manifold;
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    const Vector x = RandomQuaternion();
+    const Vector y = RandomQuaternion();
+    Vector delta = Vector::Random(3);
+    EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+    EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+  }
+}
+
+TEST(Quaternion, SmallDelta) {
+  Quaternion manifold;
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    const Vector x = RandomQuaternion();
+    const Vector y = RandomQuaternion();
+    Vector delta = Vector::Random(3);
+    delta.normalize();
+    delta *= 1e-6;
+    EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+    EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+  }
+}
+
+TEST(Quaternion, DeltaJustBelowPi) {
+  Quaternion manifold;
+  for (int trial = 0; trial < kNumTrials; ++trial) {
+    const Vector x = RandomQuaternion();
+    const Vector y = RandomQuaternion();
+    Vector delta = Vector::Random(3);
+    delta.normalize();
+    delta *= (M_PI - 1e-6);
+    EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+    EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+  }
+}
+
 }  // namespace internal
 }  // namespace ceres