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());
