Reuse rotation functionality in (Eigen)QuaternionManifold Allow to specify the memory layout of quaternion coefficients using a template parameter which defaults to Ceres coefficients order. The changes are, for the most part, backwards compatible unless the floating-point type is explicitly specified, e.g., as &ceres::QuaternionToAngleAxis<double> to obtain a pointer to the corresponding function. In such rare use cases, the coefficients order must be given explicitly first as ceres::QuaternionToAngleAxis<ceres::CeresQuaternionOrder>. In normal situations, however, this should not be needed. Change-Id: I05dd80f0593672dec656cc785cf06fe5268aee74
diff --git a/include/ceres/manifold_test_utils.h b/include/ceres/manifold_test_utils.h index 37841f9..4872e66 100644 --- a/include/ceres/manifold_test_utils.h +++ b/include/ceres/manifold_test_utils.h
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2023 Google Inc. All rights reserved. +// Copyright 2025 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,11 +31,13 @@ #include <cmath> #include <limits> #include <memory> +#include <type_traits> #include "ceres/dynamic_numeric_diff_cost_function.h" #include "ceres/internal/eigen.h" #include "ceres/manifold.h" #include "ceres/numeric_diff_options.h" +#include "ceres/rotation.h" #include "ceres/types.h" #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -214,16 +216,53 @@ Vector actual = Vector::Zero(ambient_size); EXPECT_TRUE(arg.Plus(x.data(), y_minus_x.data(), actual.data())); - const double n = (actual - Vector{y}).norm(); - const double d = y.norm(); - const double diffnorm = (d == 0.0) ? n : (n / d); - if (diffnorm > tolerance) { - *result_listener << "\nx: " << x.transpose() - << "\nexpected: " << y.transpose() - << "\nactual:" << actual.transpose() - << "\ndiff:" << (y - actual).transpose() - << "\ndiffnorm: " << diffnorm; - return false; + using ManifoldType = std::decay_t<decltype(arg)>; + + constexpr bool kIsCeresQuaternion = + std::is_same_v<ManifoldType, ceres::QuaternionManifold>; + constexpr bool kIsEigenQuaternion = + std::is_same_v<ManifoldType, ceres::EigenQuaternionManifold>; + + if constexpr (!(kIsEigenQuaternion || kIsCeresQuaternion)) { + const double n = (actual - Vector{y}).norm(); + const double d = y.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() + << "\nexpected: " << y.transpose() + << "\nactual: " << actual.transpose() + << "\ndiff: " << (y - actual).transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + } else { + // Quaternions are equivalent up to a sign change. Compute the algebraic + // distance by taking the minimum of |x+y| and |x-y| where |∘| is the L² + // norm. For more details, refer to + // + // Hartley, R., Trumpf, J., Dai, Y., & Li, H. (2013). Rotation Averaging. + // International Journal of Computer Vision, 103(3), 267–305. + // https://doi.org/10.1007/s11263-012-0601-0 + // + // The computation here is closely related to the IsNearQuaternion matcher + // use in rotation_test.cc. The matcher, however, does not compute the + // distance but only compares the coefficients. + const Eigen::Vector4d plus = actual + Vector{y}; + const Eigen::Vector4d minus = actual - Vector{y}; + + const double plus_norm = plus.norm(); + const double minus_norm = minus.norm(); + const double diffnorm = std::min(plus_norm, minus_norm); + + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() + << "\nexpected: " << y.transpose() + << "\nactual: " << actual.transpose() + << "\ndiff +: " << plus.transpose() + << "\ndiff -: " << minus.transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } } return true; }
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index c04278e..addd598 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.h
@@ -79,13 +79,29 @@ template <typename T> MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer); +// (Default) Ceres quaternion coefficients order (w, x, y, z). +struct CeresQuaternionOrder { + static constexpr int kW = 0; + static constexpr int kX = 1; + static constexpr int kY = 2; + static constexpr int kZ = 3; +}; + +// Eigen quaternion coefficients order (x, y, z, w). +struct EigenQuaternionOrder { + static constexpr int kW = 3; + static constexpr int kX = 0; + static constexpr int kY = 1; + static constexpr int kZ = 2; +}; + // Convert a value in combined axis-angle representation to a quaternion. // The value angle_axis is a triple whose norm is an angle in radians, // and whose direction is aligned with the axis of rotation, // and quaternion is a 4-tuple that will contain the resulting quaternion. // The implementation may be used with auto-differentiation up to the first // derivative, higher derivatives may have unexpected results near the origin. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); // Convert a quaternion to the equivalent combined axis-angle representation. @@ -94,16 +110,19 @@ // rotation in radians, and whose direction is the axis of rotation. // The implementation may be used with auto-differentiation up to the first // derivative, higher derivatives may have unexpected results near the origin. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); // Conversions between 3x3 rotation matrix (in column major order) and // quaternion rotation representations. Templated for use with // autodifferentiation. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> void RotationMatrixToQuaternion(const T* R, T* quaternion); -template <typename T, int row_stride, int col_stride> +template <typename Order = CeresQuaternionOrder, + typename T, + int row_stride, + int col_stride> void RotationMatrixToQuaternion( const MatrixAdapter<const T, row_stride, col_stride>& R, T* quaternion); @@ -221,10 +240,13 @@ // such that det(Q) = 1 and Q*Q' = I // // WARNING: The rotation matrix is ROW MAJOR -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); -template <typename T, int row_stride, int col_stride> +template <typename Order = CeresQuaternionOrder, + typename T, + int row_stride, + int col_stride> inline void QuaternionToScaledRotation( const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R); @@ -232,10 +254,13 @@ // Frobenius norm, so that R * R' = I (and det(R) = 1). // // WARNING: The rotation matrix is ROW MAJOR -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> inline void QuaternionToRotation(const T q[4], T R[3 * 3]); -template <typename T, int row_stride, int col_stride> +template <typename Order = CeresQuaternionOrder, + typename T, + int row_stride, + int col_stride> inline void QuaternionToRotation( const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R); @@ -250,7 +275,7 @@ // // Inplace rotation is not supported. pt and result must point to different // memory locations, otherwise the result will be undefined. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // With this function you do not need to assume that q has unit norm. @@ -258,7 +283,7 @@ // // Inplace rotation is not supported. pt and result must point to different // memory locations, otherwise the result will be undefined. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); // zw = z * w, where * is the Quaternion product between 4 vectors. @@ -266,9 +291,15 @@ // Inplace quaternion product is not supported. The resulting quaternion zw must // not share the memory with the input quaternion z and w, otherwise the result // will be undefined. -template <typename T> +template <typename Order = CeresQuaternionOrder, typename T> inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]); +// Compute the conjugate of the quaternion q and store it in z. +// +// Inplace conjugation is supported. +template <typename Order = CeresQuaternionOrder, typename T> +inline void QuaternionConjugate(const T q[4], T z[4]); + // xy = x cross y; // // Inplace cross product is not supported. The resulting vector x_cross_y must @@ -310,7 +341,7 @@ return MatrixAdapter<T, 3, 1>(pointer); } -template <typename T> +template <typename Order, typename T> inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { using std::fpclassify; using std::hypot; @@ -325,28 +356,28 @@ if (fpclassify(theta) != FP_ZERO) { const T half_theta = theta * T(0.5); k = sin(half_theta) / theta; - quaternion[0] = cos(half_theta); + quaternion[Order::kW] = cos(half_theta); } else { // At the origin, sqrt() will produce NaN in the derivative since // the argument is zero. By approximating with a Taylor series, // and truncating at one term, the value and first derivatives will be // computed correctly when Jets are used. k = T(0.5); - quaternion[0] = T(1.0); + quaternion[Order::kW] = T(1.0); } - quaternion[1] = a0 * k; - quaternion[2] = a1 * k; - quaternion[3] = a2 * k; + quaternion[Order::kX] = a0 * k; + quaternion[Order::kY] = a1 * k; + quaternion[Order::kZ] = a2 * k; } -template <typename T> +template <typename Order, typename T> inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { using std::fpclassify; using std::hypot; - const T& q1 = quaternion[1]; - const T& q2 = quaternion[2]; - const T& q3 = quaternion[3]; + const T& q1 = quaternion[Order::kX]; + const T& q2 = quaternion[Order::kY]; + const T& q3 = quaternion[Order::kZ]; T k; const T sin_theta = hypot(q1, q2, q3); @@ -354,7 +385,7 @@ // For quaternions representing non-zero rotation, the conversion // is numerically stable. if (fpclassify(sin_theta) != FP_ZERO) { - const T& cos_theta = quaternion[0]; + const T& cos_theta = quaternion[Order::kW]; // If cos_theta is negative, theta is greater than pi/2, which // means that angle for the angle_axis vector which is 2 * theta @@ -385,32 +416,32 @@ angle_axis[2] = q3 * k; } -template <typename T> +template <typename Order, typename T> void RotationMatrixToQuaternion(const T* R, T* quaternion) { - RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), quaternion); + RotationMatrixToQuaternion<Order>(ColumnMajorAdapter3x3(R), quaternion); } // This algorithm comes from "Quaternion Calculus and Fast Animation", // Ken Shoemake, 1987 SIGGRAPH course notes -template <typename T, int row_stride, int col_stride> +template <typename Order, typename T, int row_stride, int col_stride> void RotationMatrixToQuaternion( const MatrixAdapter<const T, row_stride, col_stride>& R, T* quaternion) { const T trace = R(0, 0) + R(1, 1) + R(2, 2); if (trace >= 0.0) { T t = sqrt(trace + T(1.0)); - quaternion[0] = T(0.5) * t; + quaternion[Order::kW] = T(0.5) * t; t = T(0.5) / t; - quaternion[1] = (R(2, 1) - R(1, 2)) * t; - quaternion[2] = (R(0, 2) - R(2, 0)) * t; - quaternion[3] = (R(1, 0) - R(0, 1)) * t; + quaternion[Order::kX] = (R(2, 1) - R(1, 2)) * t; + quaternion[Order::kY] = (R(0, 2) - R(2, 0)) * t; + quaternion[Order::kZ] = (R(1, 0) - R(0, 1)) * t; } else { - int i = 0; + int i = Order::kW; if (R(1, 1) > R(0, 0)) { - i = 1; + i = Order::kX; } if (R(2, 2) > R(i, i)) { - i = 2; + i = Order::kY; } const int j = (i + 1) % 3; @@ -418,7 +449,7 @@ T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0)); quaternion[i + 1] = T(0.5) * t; t = T(0.5) / t; - quaternion[0] = (R(k, j) - R(j, k)) * t; + quaternion[Order::kW] = (R(k, j) - R(j, k)) * t; quaternion[j + 1] = (R(j, i) + R(i, j)) * t; quaternion[k + 1] = (R(k, i) + R(i, k)) * t; } @@ -663,19 +694,19 @@ R(2, 2) = c2 * c3; } -template <typename T> +template <typename Order, typename T> inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { - QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); + QuaternionToScaledRotation<Order>(q, RowMajorAdapter3x3(R)); } -template <typename T, int row_stride, int col_stride> +template <typename Order, typename T, int row_stride, int col_stride> inline void QuaternionToScaledRotation( const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) { // Make convenient names for elements of q. - T a = q[0]; - T b = q[1]; - T c = q[2]; - T d = q[3]; + T a = q[Order::kW]; + T b = q[Order::kX]; + T c = q[Order::kY]; + T d = q[Order::kZ]; // This is not to eliminate common sub-expression, but to // make the lines shorter so that they fit in 80 columns! T aa = a * a; @@ -696,17 +727,18 @@ // clang-format on } -template <typename T> +template <typename Order, typename T> inline void QuaternionToRotation(const T q[4], T R[3 * 3]) { - QuaternionToRotation(q, RowMajorAdapter3x3(R)); + QuaternionToRotation<Order>(q, RowMajorAdapter3x3(R)); } -template <typename T, int row_stride, int col_stride> +template <typename Order, typename T, int row_stride, int col_stride> inline void QuaternionToRotation( const T q[4], const MatrixAdapter<T, row_stride, col_stride>& R) { - QuaternionToScaledRotation(q, R); + QuaternionToScaledRotation<Order>(q, R); - T normalizer = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; + T normalizer = q[Order::kW] * q[Order::kW] + q[Order::kX] * q[Order::kX] + + q[Order::kY] * q[Order::kY] + q[Order::kZ] * q[Order::kZ]; normalizer = T(1) / normalizer; for (int i = 0; i < 3; ++i) { @@ -716,60 +748,69 @@ } } -template <typename T> +template <typename Order, typename T> inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { DCHECK_NE(pt, result) << "Inplace rotation is not supported."; // clang-format off - T uv0 = q[2] * pt[2] - q[3] * pt[1]; - T uv1 = q[3] * pt[0] - q[1] * pt[2]; - T uv2 = q[1] * pt[1] - q[2] * pt[0]; + T uv0 = q[Order::kY] * pt[2] - q[Order::kZ] * pt[1]; + T uv1 = q[Order::kZ] * pt[0] - q[Order::kX] * pt[2]; + T uv2 = q[Order::kX] * pt[1] - q[Order::kY] * pt[0]; uv0 += uv0; uv1 += uv1; uv2 += uv2; - result[0] = pt[0] + q[0] * uv0; - result[1] = pt[1] + q[0] * uv1; - result[2] = pt[2] + q[0] * uv2; - result[0] += q[2] * uv2 - q[3] * uv1; - result[1] += q[3] * uv0 - q[1] * uv2; - result[2] += q[1] * uv1 - q[2] * uv0; + result[0] = pt[0] + q[Order::kW] * uv0; + result[1] = pt[1] + q[Order::kW] * uv1; + result[2] = pt[2] + q[Order::kW] * uv2; + result[0] += q[Order::kY] * uv2 - q[Order::kZ] * uv1; + result[1] += q[Order::kZ] * uv0 - q[Order::kX] * uv2; + result[2] += q[Order::kX] * uv1 - q[Order::kY] * uv0; // clang-format on } -template <typename T> +template <typename Order, typename T> inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { DCHECK_NE(pt, result) << "Inplace rotation is not supported."; // 'scale' is 1 / norm(q). const T scale = - T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + T(1) / sqrt(q[Order::kW] * q[Order::kW] + q[Order::kX] * q[Order::kX] + + q[Order::kY] * q[Order::kY] + q[Order::kZ] * q[Order::kZ]); // Make unit-norm version of q. const T unit[4] = { - scale * q[0], - scale * q[1], - scale * q[2], - scale * q[3], + scale * q[Order::kW], + scale * q[Order::kX], + scale * q[Order::kY], + scale * q[Order::kZ], }; - UnitQuaternionRotatePoint(unit, pt, result); + UnitQuaternionRotatePoint<Order>(unit, pt, result); } -template <typename T> +template <typename Order, typename T> inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { DCHECK_NE(z, zw) << "Inplace quaternion product is not supported."; DCHECK_NE(w, zw) << "Inplace quaternion product is not supported."; // clang-format off - zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; - zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; - zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; - zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; + zw[Order::kW] = z[Order::kW] * w[Order::kW] - z[Order::kX] * w[Order::kX] - z[Order::kY] * w[Order::kY] - z[Order::kZ] * w[Order::kZ]; + zw[Order::kX] = z[Order::kW] * w[Order::kX] + z[Order::kX] * w[Order::kW] + z[Order::kY] * w[Order::kZ] - z[Order::kZ] * w[Order::kY]; + zw[Order::kY] = z[Order::kW] * w[Order::kY] - z[Order::kX] * w[Order::kZ] + z[Order::kY] * w[Order::kW] + z[Order::kZ] * w[Order::kX]; + zw[Order::kZ] = z[Order::kW] * w[Order::kZ] + z[Order::kX] * w[Order::kY] - z[Order::kY] * w[Order::kX] + z[Order::kZ] * w[Order::kW]; // clang-format on } +template <typename Order, typename T> +inline void QuaternionConjugate(const T q[4], T w[4]) { + w[Order::kW] = q[Order::kW]; + w[Order::kX] = -q[Order::kX]; + w[Order::kY] = -q[Order::kY]; + w[Order::kZ] = -q[Order::kZ]; +} + // xy = x cross y; template <typename T> inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc index f2cd378..9830803 100644 --- a/internal/ceres/manifold.cc +++ b/internal/ceres/manifold.cc
@@ -5,59 +5,20 @@ #include "absl/log/check.h" #include "ceres/internal/eigen.h" +#include "ceres/rotation.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::hypot(delta[0], delta[1], delta[2]); - - if (std::fpclassify(norm_delta) == FP_ZERO) { - // No change in rotation: return the quaternion as is. - std::copy_n(x, 4, x_plus_delta); - return; - } - - const double half_norm_delta = norm_delta / 2; - const double sin_half_delta_by_delta = - (std::sin(half_norm_delta) / norm_delta); double q_delta[4]; - q_delta[Order::kW] = std::cos(half_norm_delta); - q_delta[Order::kX] = sin_half_delta_by_delta * delta[0]; - q_delta[Order::kY] = sin_half_delta_by_delta * delta[1]; - q_delta[Order::kZ] = sin_half_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]; + AngleAxisToQuaternion<Order>(delta, q_delta); + QuaternionProduct<Order>(q_delta, x, x_plus_delta); } template <typename Order> @@ -86,31 +47,12 @@ 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]; + double x_conj[4]; + QuaternionConjugate<Order>(x, x_conj); - const double u_norm = std::hypot(ambient_y_minus_x[Order::kX], - ambient_y_minus_x[Order::kY], - ambient_y_minus_x[Order::kZ]); - if (std::fpclassify(u_norm) != FP_ZERO) { - const double theta = 2 * 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 { - std::fill_n(y_minus_x, 3, 0.0); - } + double ambient_y_minus_x[4]; + QuaternionProduct<Order>(y, x_conj, ambient_y_minus_x); + QuaternionToAngleAxis<Order>(ambient_y_minus_x, y_minus_x); } template <typename Order>
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index 85c8b78..28b34ef 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc
@@ -36,6 +36,7 @@ #include <limits> #include <random> #include <string> +#include <type_traits> #include <utility> #include "absl/log/log.h" @@ -201,111 +202,149 @@ return true; } +template <typename Order, typename T> +constexpr auto MakeQuaternion(T w, T x, T y, T z) + -> std::enable_if_t<std::is_same_v<Order, CeresQuaternionOrder>, + std::array<T, 4>> { + return {w, x, y, z}; +} + +template <typename Order, typename T> +constexpr auto MakeQuaternion(T w, T x, T y, T z) + -> std::enable_if_t<std::is_same_v<Order, EigenQuaternionOrder>, + std::array<T, 4>> { + return {x, y, z, w}; +} + +template <typename T> +class QuaternionTest : public testing::Test {}; + +using QuaternionOrderTypes = + testing::Types<ceres::CeresQuaternionOrder, ceres::EigenQuaternionOrder>; + +TYPED_TEST_SUITE(QuaternionTest, QuaternionOrderTypes); + // Transforms a zero axis/angle to a quaternion. -TEST(Rotation, ZeroAngleAxisToQuaternion) { +TYPED_TEST(QuaternionTest, ZeroAngleAxisToQuaternion) { + using Order = TypeParam; double axis_angle[3] = {0, 0, 0}; double quaternion[4]; - double expected[4] = {1, 0, 0, 0}; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<double, 4> expected = + MakeQuaternion<Order>(1.0, 0.0, 0.0, 0.0); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, IsNormalizedQuaternion()); EXPECT_THAT(quaternion, IsNearQuaternion(expected)); } // Test that exact conversion works for small angles. -TEST(Rotation, SmallAngleAxisToQuaternion) { +TYPED_TEST(QuaternionTest, SmallAngleAxisToQuaternion) { + using Order = TypeParam; // Small, finite value to test. double theta = 1.0e-2; double axis_angle[3] = {theta, 0, 0}; double quaternion[4]; - double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<double, 4> expected = + MakeQuaternion<Order>(cos(theta / 2.0), sin(theta / 2.0), 0.0, 0.0); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, IsNormalizedQuaternion()); EXPECT_THAT(quaternion, IsNearQuaternion(expected)); } // Test that approximate conversion works for very small angles. -TEST(Rotation, TinyAngleAxisToQuaternion) { +TYPED_TEST(QuaternionTest, TinyAngleAxisToQuaternion) { + using Order = TypeParam; // Very small value that could potentially cause underflow. double theta = pow(std::numeric_limits<double>::min(), 0.75); double axis_angle[3] = {theta, 0, 0}; double quaternion[4]; - double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<double, 4> expected = + MakeQuaternion<Order>(cos(theta / 2.0), sin(theta / 2.0), 0.0, 0.0); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, IsNormalizedQuaternion()); EXPECT_THAT(quaternion, IsNearQuaternion(expected)); } // Transforms a rotation by pi/2 around X to a quaternion. -TEST(Rotation, XRotationToQuaternion) { +TYPED_TEST(QuaternionTest, XRotationToQuaternion) { + using Order = TypeParam; double axis_angle[3] = {kPi / 2, 0, 0}; double quaternion[4]; - double expected[4] = {kHalfSqrt2, kHalfSqrt2, 0, 0}; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<double, 4> expected = + MakeQuaternion<Order>(kHalfSqrt2, kHalfSqrt2, 0.0, 0.0); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, IsNormalizedQuaternion()); EXPECT_THAT(quaternion, IsNearQuaternion(expected)); } // Transforms a unit quaternion to an axis angle. -TEST(Rotation, UnitQuaternionToAngleAxis) { - double quaternion[4] = {1, 0, 0, 0}; +TYPED_TEST(QuaternionTest, UnitQuaternionToAngleAxis) { + using Order = TypeParam; + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(1.0, 0.0, 0.0, 0.0); double axis_angle[3]; double expected[3] = {0, 0, 0}; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); } // Transforms a quaternion that rotates by pi about the Y axis to an axis angle. -TEST(Rotation, YRotationQuaternionToAngleAxis) { - double quaternion[4] = {0, 0, 1, 0}; +TYPED_TEST(QuaternionTest, YRotationQuaternionToAngleAxis) { + using Order = TypeParam; + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(0.0, 0.0, 1.0, 0.0); double axis_angle[3]; double expected[3] = {0, kPi, 0}; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); } // Transforms a quaternion that rotates by pi/3 about the Z axis to an axis // angle. -TEST(Rotation, ZRotationQuaternionToAngleAxis) { - double quaternion[4] = {sqrt(3) / 2, 0, 0, 0.5}; +TYPED_TEST(QuaternionTest, ZRotationQuaternionToAngleAxis) { + using Order = TypeParam; + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(sqrt(3) / 2, 0.0, 0.0, 0.5); double axis_angle[3]; double expected[3] = {0, 0, kPi / 3}; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); } // Test that exact conversion works for small angles. -TEST(Rotation, SmallQuaternionToAngleAxis) { +TYPED_TEST(QuaternionTest, SmallQuaternionToAngleAxis) { + using Order = TypeParam; // Small, finite value to test. double theta = 1.0e-2; - double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(cos(theta / 2.0), sin(theta / 2.0), 0.0, 0.0); double axis_angle[3]; double expected[3] = {theta, 0, 0}; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); } // Test that approximate conversion works for very small angles. -TEST(Rotation, TinyQuaternionToAngleAxis) { +TYPED_TEST(QuaternionTest, TinyQuaternionToAngleAxis) { + using Order = TypeParam; // Very small value that could potentially cause underflow. double theta = pow(std::numeric_limits<double>::min(), 0.75); - double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(cos(theta / 2.0), sin(theta / 2.0), 0.0, 0.0); double axis_angle[3]; double expected[3] = {theta, 0, 0}; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); } -TEST(Rotation, QuaternionToAngleAxisAngleIsLessThanPi) { - double quaternion[4]; +TYPED_TEST(QuaternionTest, QuaternionToAngleAxisAngleIsLessThanPi) { + using Order = TypeParam; double angle_axis[3]; const double half_theta = 0.75 * kPi; - quaternion[0] = cos(half_theta); - quaternion[1] = 1.0 * sin(half_theta); - quaternion[2] = 0.0; - quaternion[3] = 0.0; - QuaternionToAngleAxis(quaternion, angle_axis); + const std::array<double, 4> quaternion = + MakeQuaternion<Order>(cos(half_theta), 1.0 * sin(half_theta), 0.0, 0.0); + QuaternionToAngleAxis<Order>(quaternion.data(), angle_axis); const double angle = std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]); EXPECT_LE(angle, kPi); } @@ -314,7 +353,8 @@ // Takes a bunch of random axis/angle values, converts them to quaternions, // and back again. -TEST(Rotation, AngleAxisToQuaterionAndBack) { +TYPED_TEST(QuaternionTest, AngleAxisToQuaterionAndBack) { + using Order = TypeParam; std::mt19937 prng; std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; for (int i = 0; i < kNumTrials; i++) { @@ -340,16 +380,17 @@ // We use ASSERTs here because if there's one failure, there are // probably many and spewing a million failures doesn't make anyone's // day. - AngleAxisToQuaternion(axis_angle, quaternion); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); ASSERT_THAT(quaternion, IsNormalizedQuaternion()); - QuaternionToAngleAxis(quaternion, round_trip); + QuaternionToAngleAxis<Order>(quaternion, round_trip); ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle)); } } // Takes a bunch of random quaternions, converts them to axis/angle, // and back again. -TEST(Rotation, QuaterionToAngleAxisAndBack) { +TYPED_TEST(QuaternionTest, QuaterionToAngleAxisAndBack) { + using Order = TypeParam; std::mt19937 prng; std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; for (int i = 0; i < kNumTrials; i++) { @@ -368,8 +409,8 @@ double axis_angle[3]; double round_trip[4]; - QuaternionToAngleAxis(quaternion, axis_angle); - AngleAxisToQuaternion(axis_angle, round_trip); + QuaternionToAngleAxis<Order>(quaternion, axis_angle); + AngleAxisToQuaternion<Order>(axis_angle, round_trip); ASSERT_THAT(round_trip, IsNormalizedQuaternion()); ASSERT_THAT(round_trip, IsNearQuaternion(quaternion)); } @@ -1004,26 +1045,27 @@ static_cast<int>(1 + log(std::numeric_limits<double>::min()) / log(10.0)); // Test that exact conversion works for small angles when jets are used. -TEST(Rotation, SmallAngleAxisToQuaternionForJets) { +TYPED_TEST(QuaternionTest, SmallAngleAxisToQuaternionForJets) { + using Order = TypeParam; // Examine small x rotations that are still large enough // to be well within the range represented by doubles. for (int i = -2; i >= kSmallTinyCutoff; i--) { double theta = pow(10.0, i); J3 axis_angle[3] = {J3(theta, 0), J3(0, 1), J3(0, 2)}; J3 quaternion[4]; - J3 expected[4] = { - MakeJ3(cos(theta / 2), -sin(theta / 2) / 2, 0, 0), - MakeJ3(sin(theta / 2), cos(theta / 2) / 2, 0, 0), - MakeJ3(0, 0, sin(theta / 2) / theta, 0), - MakeJ3(0, 0, 0, sin(theta / 2) / theta), - }; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<J3, 4> expected = + MakeQuaternion<Order>(MakeJ3(cos(theta / 2), -sin(theta / 2) / 2, 0, 0), + MakeJ3(sin(theta / 2), cos(theta / 2) / 2, 0, 0), + MakeJ3(0, 0, sin(theta / 2) / theta, 0), + MakeJ3(0, 0, 0, sin(theta / 2) / theta)); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); } } // Test that conversion works for very small angles when jets are used. -TEST(Rotation, TinyAngleAxisToQuaternionForJets) { +TYPED_TEST(QuaternionTest, TinyAngleAxisToQuaternionForJets) { + using Order = TypeParam; // Examine tiny x rotations that extend all the way to where // underflow occurs. for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { @@ -1033,40 +1075,41 @@ // To avoid loss of precision in the test itself, // a finite expansion is used here, which will // be exact up to machine precision for the test values used. - J3 expected[4] = { - MakeJ3(1.0, 0, 0, 0), - MakeJ3(0, 0.5, 0, 0), - MakeJ3(0, 0, 0.5, 0), - MakeJ3(0, 0, 0, 0.5), - }; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<J3, 4> expected = + MakeQuaternion<Order>(MakeJ3(1.0, 0, 0, 0), + MakeJ3(0, 0.5, 0, 0), + MakeJ3(0, 0, 0.5, 0), + MakeJ3(0, 0, 0, 0.5)); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); } } // Test that derivatives are correct for zero rotation. -TEST(Rotation, ZeroAngleAxisToQuaternionForJets) { +TYPED_TEST(QuaternionTest, ZeroAngleAxisToQuaternionForJets) { + using Order = TypeParam; J3 axis_angle[3] = {J3(0, 0), J3(0, 1), J3(0, 2)}; J3 quaternion[4]; - J3 expected[4] = { - MakeJ3(1.0, 0, 0, 0), - MakeJ3(0, 0.5, 0, 0), - MakeJ3(0, 0, 0.5, 0), - MakeJ3(0, 0, 0, 0.5), - }; - AngleAxisToQuaternion(axis_angle, quaternion); + const std::array<J3, 4> expected = + MakeQuaternion<Order>(MakeJ3(1.0, 0, 0, 0), + MakeJ3(0, 0.5, 0, 0), + MakeJ3(0, 0, 0.5, 0), + MakeJ3(0, 0, 0, 0.5)); + AngleAxisToQuaternion<Order>(axis_angle, quaternion); EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); } // Test that exact conversion works for small angles. -TEST(Rotation, SmallQuaternionToAngleAxisForJets) { +TYPED_TEST(QuaternionTest, SmallQuaternionToAngleAxisForJets) { + using Order = TypeParam; // Examine small x rotations that are still large enough // to be well within the range represented by doubles. for (int i = -2; i >= kSmallTinyCutoff; i--) { double theta = pow(10.0, i); double s = sin(theta); double c = cos(theta); - J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)}; + const std::array<J4, 4> quaternion = + MakeQuaternion<Order>(J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)); J4 axis_angle[3]; // clang-format off J4 expected[3] = { @@ -1075,20 +1118,22 @@ MakeJ4(0, 0, 0, 0, 2*theta/s), }; // clang-format on - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); } } // Test that conversion works for very small angles. -TEST(Rotation, TinyQuaternionToAngleAxisForJets) { +TYPED_TEST(QuaternionTest, TinyQuaternionToAngleAxisForJets) { + using Order = TypeParam; // Examine tiny x rotations that extend all the way to where // underflow occurs. for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { double theta = pow(10.0, i); double s = sin(theta); double c = cos(theta); - J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)}; + const std::array<J4, 4> quaternion = + MakeQuaternion<Order>(J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)); J4 axis_angle[3]; // To avoid loss of precision in the test itself, // a finite expansion is used here, which will @@ -1100,21 +1145,23 @@ MakeJ4(0, 0, 0, 0, 2.0), }; // clang-format on - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); } } // Test that conversion works for no rotation. -TEST(Rotation, ZeroQuaternionToAngleAxisForJets) { - J4 quaternion[4] = {J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3)}; +TYPED_TEST(QuaternionTest, ZeroQuaternionToAngleAxisForJets) { + using Order = TypeParam; + const std::array<J4, 4> quaternion = + MakeQuaternion<Order>(J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3)); J4 axis_angle[3]; J4 expected[3] = { MakeJ4(0, 0, 2.0, 0, 0), MakeJ4(0, 0, 0, 2.0, 0), MakeJ4(0, 0, 0, 0, 2.0), }; - QuaternionToAngleAxis(quaternion, axis_angle); + QuaternionToAngleAxis<Order>(quaternion.data(), axis_angle); EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); } @@ -1619,14 +1666,13 @@ } } -TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) { +TYPED_TEST(QuaternionTest, RotatePointGivesSameAnswerAsRotationByMatrixCanned) { + using Order = TypeParam; // Canned data generated in octave. - double const q[4] = { - +0.1956830471754074, - -0.0150618562474847, - +0.7634572982788086, - -0.3019454777240753, - }; + const std::array<double, 4> q = MakeQuaternion<Order>(+0.1956830471754074, + -0.0150618562474847, + +0.7634572982788086, + -0.3019454777240753); double const Q[3][3] = { // Scaled rotation matrix. {-0.6355194033477252, +0.0951730541682254, +0.3078870197911186}, @@ -1642,22 +1688,21 @@ // Compute R from q and compare to known answer. double Rq[3][3]; - QuaternionToScaledRotation<double>(q, Rq[0]); + QuaternionToScaledRotation<Order>(q.data(), Rq[0]); ExpectArraysClose(9, Q[0], Rq[0], kTolerance); // Now do the same but compute R with normalization. - QuaternionToRotation<double>(q, Rq[0]); + QuaternionToRotation<Order>(q.data(), Rq[0]); ExpectArraysClose(9, R[0], Rq[0], kTolerance); } -TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) { +TYPED_TEST(QuaternionTest, RotatePointGivesSameAnswerAsRotationByMatrix) { + using Order = TypeParam; // Rotation defined by a unit quaternion. - double const q[4] = { - +0.2318160216097109, - -0.0178430356832060, - +0.9044300776717159, - -0.3576998641394597, - }; + const std::array<double, 4> q = MakeQuaternion<Order>(+0.2318160216097109, + -0.0178430356832060, + +0.9044300776717159, + -0.3576998641394597); double const p[3] = { +0.11, -13.15, @@ -1665,10 +1710,10 @@ }; double R[3 * 3]; - QuaternionToRotation(q, R); + QuaternionToRotation<Order>(q.data(), R); double result1[3]; - UnitQuaternionRotatePoint(q, p, result1); + UnitQuaternionRotatePoint<Order>(q.data(), p, result1); double result2[3]; VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3) * ConstVectorRef(p, 3); @@ -1676,7 +1721,8 @@ } // Verify that (a * b) * c == a * (b * c). -TEST(Quaternion, MultiplicationIsAssociative) { +TYPED_TEST(QuaternionTest, MultiplicationIsAssociative) { + using Order = TypeParam; std::mt19937 prng; std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; double a[4]; @@ -1690,18 +1736,38 @@ double ab[4]; double ab_c[4]; - QuaternionProduct(a, b, ab); - QuaternionProduct(ab, c, ab_c); + QuaternionProduct<Order>(a, b, ab); + QuaternionProduct<Order>(ab, c, ab_c); double bc[4]; double a_bc[4]; - QuaternionProduct(b, c, bc); - QuaternionProduct(a, bc, a_bc); + QuaternionProduct<Order>(b, c, bc); + QuaternionProduct<Order>(a, bc, a_bc); - ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance); - ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance); - ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance); - ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance); + ASSERT_NEAR(ab_c[Order::kW], a_bc[Order::kW], kTolerance); + ASSERT_NEAR(ab_c[Order::kX], a_bc[Order::kX], kTolerance); + ASSERT_NEAR(ab_c[Order::kY], a_bc[Order::kY], kTolerance); + ASSERT_NEAR(ab_c[Order::kZ], a_bc[Order::kZ], kTolerance); +} + +TYPED_TEST(QuaternionTest, UnitConjugationIdentity) { + using Order = TypeParam; + std::mt19937 prng; + std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; + double a[4]; + for (int i = 0; i < 4; ++i) { + a[i] = uniform_distribution(prng); + } + Eigen::Map<Eigen::Vector4d>{a}.normalize(); + double b[4]; + QuaternionConjugate<Order>(a, b); + double c[4]; + QuaternionProduct<Order>(a, b, c); + + EXPECT_NEAR(c[Order::kW], 1, kTolerance); + EXPECT_NEAR(c[Order::kX], 0, kTolerance); + EXPECT_NEAR(c[Order::kY], 0, kTolerance); + EXPECT_NEAR(c[Order::kZ], 0, kTolerance); } TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { @@ -1753,13 +1819,16 @@ } } -TEST(Quaternion, UnitQuaternion) { +TYPED_TEST(QuaternionTest, UnitQuaternion) { + using Order = TypeParam; using Jet = ceres::Jet<double, 4>; - std::array<Jet, 4> quaternion = { - Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)}; + const std::array<Jet, 4> quaternion = + MakeQuaternion<Order>(Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)); + std::array<Jet, 3> point = {Jet(0.0), Jet(0.0), Jet(0.0)}; std::array<Jet, 3> rotated_point; - QuaternionRotatePoint(quaternion.data(), point.data(), rotated_point.data()); + QuaternionRotatePoint<Order>( + quaternion.data(), point.data(), rotated_point.data()); for (int i = 0; i < 3; ++i) { EXPECT_EQ(rotated_point[i], point[i]); EXPECT_FALSE(rotated_point[i].v.array().isNaN().any());