Add EigenQuaternion manifold and tests for it.
Change-Id: I579a45a2921424a07d03b3b4d9cfcbc0ab529f9b
diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h
index 0867997..c5687c8 100644
--- a/include/ceres/manifold.h
+++ b/include/ceres/manifold.h
@@ -381,6 +381,32 @@
bool MinusJacobian(const double* x, double* jacobian) const override;
};
+// Implements the quaternion manifold for Eigen's representation of the Hamilton
+// quaternion. Geometrically it is exactly the same as the Quaternion manifold
+// defined above. However, Eigen uses a different internal memory layout for the
+// elements of the quaternion than what is commonly used. It stores the
+// quaternion in memory as [q1, q2, q3, q0] or [x, y, z, w] where the
+// real (scalar) part is last.
+//
+// Since Ceres operates on parameter blocks which are raw double pointers
+// this difference is important and requires a different manifold.
+class CERES_EXPORT EigenQuaternion : public Manifold {
+ public:
+ EigenQuaternion() = default;
+ virtual ~EigenQuaternion() = 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 70b904c..8163d2c 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -7,6 +7,134 @@
#include "glog/logging.h"
namespace ceres {
+namespace {
+
+struct CeresQuaternionOrder {
+ static constexpr int kW = 0;
+ static constexpr int kX = 1;
+ static constexpr int kY = 2;
+ static constexpr int kZ = 3;
+};
+
+struct EigenQuaternionOrder {
+ static constexpr int kW = 3;
+ static constexpr int kX = 0;
+ static constexpr int kY = 1;
+ static constexpr int kZ = 2;
+};
+
+template <typename Order>
+inline void QuaternionPlusImpl(const double* x,
+ const double* delta,
+ double* x_plus_delta) {
+ // 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) {
+ for (int i = 0; i < 4; ++i) {
+ x_plus_delta[i] = x[i];
+ }
+ return;
+ }
+
+ const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta);
+ double q_delta[4];
+ q_delta[Order::kW] = std::cos(norm_delta);
+ q_delta[Order::kX] = sin_delta_by_delta * delta[0];
+ q_delta[Order::kY] = sin_delta_by_delta * delta[1];
+ q_delta[Order::kZ] = sin_delta_by_delta * delta[2];
+
+ x_plus_delta[Order::kW] =
+ q_delta[Order::kW] * x[Order::kW] - q_delta[Order::kX] * x[Order::kX] -
+ q_delta[Order::kY] * x[Order::kY] - q_delta[Order::kZ] * x[Order::kZ];
+ x_plus_delta[Order::kX] =
+ q_delta[Order::kW] * x[Order::kX] + q_delta[Order::kX] * x[Order::kW] +
+ q_delta[Order::kY] * x[Order::kZ] - q_delta[Order::kZ] * x[Order::kY];
+ x_plus_delta[Order::kY] =
+ q_delta[Order::kW] * x[Order::kY] - q_delta[Order::kX] * x[Order::kZ] +
+ q_delta[Order::kY] * x[Order::kW] + q_delta[Order::kZ] * x[Order::kX];
+ x_plus_delta[Order::kZ] =
+ q_delta[Order::kW] * x[Order::kZ] + q_delta[Order::kX] * x[Order::kY] -
+ q_delta[Order::kY] * x[Order::kX] + q_delta[Order::kZ] * x[Order::kW];
+}
+
+template <typename Order>
+inline void QuaternionPlusJacobianImpl(const double* x, double* jacobian_ptr) {
+ Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> jacobian(
+ jacobian_ptr);
+
+ jacobian(Order::kW, 0) = -x[Order::kX];
+ jacobian(Order::kW, 1) = -x[Order::kY];
+ jacobian(Order::kW, 2) = -x[Order::kZ];
+ jacobian(Order::kX, 0) = x[Order::kW];
+ jacobian(Order::kX, 1) = x[Order::kZ];
+ jacobian(Order::kX, 2) = -x[Order::kY];
+ jacobian(Order::kY, 0) = -x[Order::kZ];
+ jacobian(Order::kY, 1) = x[Order::kW];
+ jacobian(Order::kY, 2) = x[Order::kX];
+ jacobian(Order::kZ, 0) = x[Order::kY];
+ jacobian(Order::kZ, 1) = -x[Order::kX];
+ jacobian(Order::kZ, 2) = x[Order::kW];
+}
+
+template <typename Order>
+inline void QuaternionMinusImpl(const double* y,
+ const double* x,
+ double* y_minus_x) {
+ // ambient_y_minus_x = QuaternionProduct(y, -x) where -x is the conjugate of
+ // x.
+ double ambient_y_minus_x[4];
+ ambient_y_minus_x[Order::kW] =
+ y[Order::kW] * x[Order::kW] + y[Order::kX] * x[Order::kX] +
+ y[Order::kY] * x[Order::kY] + y[Order::kZ] * x[Order::kZ];
+ ambient_y_minus_x[Order::kX] =
+ -y[Order::kW] * x[Order::kX] + y[Order::kX] * x[Order::kW] -
+ y[Order::kY] * x[Order::kZ] + y[Order::kZ] * x[Order::kY];
+ ambient_y_minus_x[Order::kY] =
+ -y[Order::kW] * x[Order::kY] + y[Order::kX] * x[Order::kZ] +
+ y[Order::kY] * x[Order::kW] - y[Order::kZ] * x[Order::kX];
+ ambient_y_minus_x[Order::kZ] =
+ -y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] +
+ y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW];
+
+ const double u_norm =
+ std::sqrt(ambient_y_minus_x[Order::kX] * ambient_y_minus_x[Order::kX] +
+ ambient_y_minus_x[Order::kY] * ambient_y_minus_x[Order::kY] +
+ ambient_y_minus_x[Order::kZ] * ambient_y_minus_x[Order::kZ]);
+ if (u_norm > 0.0) {
+ const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
+ y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm;
+ y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm;
+ y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm;
+ } else {
+ y_minus_x[0] = 0.0;
+ y_minus_x[1] = 0.0;
+ y_minus_x[2] = 0.0;
+ }
+}
+
+template <typename Order>
+inline void QuaternionMinusJacobianImpl(const double* x, double* jacobian_ptr) {
+ Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian(
+ jacobian_ptr);
+
+ jacobian(0, Order::kW) = -x[Order::kX];
+ jacobian(0, Order::kX) = x[Order::kW];
+ jacobian(0, Order::kY) = -x[Order::kZ];
+ jacobian(0, Order::kZ) = x[Order::kY];
+ jacobian(1, Order::kW) = -x[Order::kY];
+ jacobian(1, Order::kX) = x[Order::kZ];
+ jacobian(1, Order::kY) = x[Order::kW];
+ jacobian(1, Order::kZ) = -x[Order::kX];
+ jacobian(2, Order::kW) = -x[Order::kZ];
+ jacobian(2, Order::kX) = -x[Order::kY];
+ jacobian(2, Order::kY) = x[Order::kX];
+ jacobian(2, Order::kZ) = x[Order::kW];
+}
+
+} // namespace
bool Manifold::RightMultiplyByPlusJacobian(const double* x,
const int num_rows,
@@ -277,78 +405,48 @@
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];
- }
- }
+ QuaternionPlusImpl<CeresQuaternionOrder>(x, delta, x_plus_delta);
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
+ QuaternionPlusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
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;
- }
+ QuaternionMinusImpl<CeresQuaternionOrder>(y, x, y_minus_x);
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
+ QuaternionMinusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
+ return true;
+}
+
+bool EigenQuaternion::Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const {
+ QuaternionPlusImpl<EigenQuaternionOrder>(x, delta, x_plus_delta);
+ return true;
+}
+
+bool EigenQuaternion::PlusJacobian(const double* x, double* jacobian) const {
+ QuaternionPlusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
+ return true;
+}
+
+bool EigenQuaternion::Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const {
+ QuaternionMinusImpl<EigenQuaternionOrder>(y, x, y_minus_x);
+ return true;
+}
+
+bool EigenQuaternion::MinusJacobian(const double* x, double* jacobian) const {
+ QuaternionMinusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
return true;
}
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index 6c95fa6..f626430 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -654,5 +654,76 @@
}
}
+// Compute the expected value of EigenQuaternion::Plus using Eigen and compares
+// it to the one computed by Quaternion::Plus.
+MATCHER_P2(EigenQuaternionPlusIsCorrectAt, 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());
+ Eigen::Quaterniond delta_eigen_q(
+ delta_q[0], delta_q[1], delta_q[2], delta_q[3]);
+
+ Eigen::Map<const Eigen::Quaterniond> x_eigen_q(x.data());
+
+ Eigen::Quaterniond expected = delta_eigen_q * x_eigen_q;
+ double actual[4];
+ EXPECT_TRUE(arg.Plus(x.data(), delta.data(), actual));
+ Eigen::Map<Eigen::Quaterniond> actual_eigen_q(actual);
+
+ const double n = (actual_eigen_q.coeffs() - expected.coeffs()).norm();
+ const double d = expected.norm();
+ const double diffnorm = n / d;
+ if (diffnorm > kEpsilon) {
+ *result_listener
+ << "\nx: " << x.transpose() << "\ndelta: " << delta.transpose()
+ << "\nexpected: " << expected.coeffs().transpose()
+ << "\nactual: " << actual_eigen_q.coeffs().transpose() << "\ndiff: "
+ << (expected.coeffs() - actual_eigen_q.coeffs()).transpose()
+ << "\ndiffnorm : " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+TEST(EigenQuaternion, GenericDelta) {
+ EigenQuaternion manifold;
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = RandomQuaternion();
+ const Vector y = RandomQuaternion();
+ Vector delta = Vector::Random(3);
+ EXPECT_THAT(manifold, EigenQuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+ }
+}
+
+TEST(EigenQuaternion, SmallDelta) {
+ EigenQuaternion 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, EigenQuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+ }
+}
+
+TEST(EigenQuaternion, DeltaJustBelowPi) {
+ EigenQuaternion 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, EigenQuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y);
+ }
+}
+
} // namespace internal
} // namespace ceres
