Add the Quaternion manifold and tests.
Change-Id: I0a396a5834168c1526d6f2b8c99a1ad3348ad657
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
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
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
