Partially revert hypot arguments zero checks
Unfortunately, libc++'s 3-argument std::hypot implementation is
numerically unstable until LLVM 19.x. Therefore, checking the arguments
for zeros is insufficient since an underflow can still occur resulting
in a zero norm which requires another check. As such, division by zero
cannot be reliably avoided.
Change-Id: I189c8dc722aaec1ebc3ec8b1a177e1d8ac3b36db
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index dc04a16..261d3a0 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -320,11 +320,10 @@
const T& a2 = angle_axis[2];
T k;
+ const T theta = hypot(a0, a1, a2);
// For points not at the origin, the full conversion is numerically stable.
- if (fpclassify(a0) != FP_ZERO || fpclassify(a1) != FP_ZERO ||
- fpclassify(a2) != FP_ZERO) {
- const T theta = hypot(a0, a1, a2);
+ if (fpclassify(theta) != FP_ZERO) {
const T half_theta = theta * T(0.5);
k = sin(half_theta) / theta;
quaternion[0] = cos(half_theta);
@@ -351,12 +350,11 @@
const T& q3 = quaternion[3];
T k;
+ const T sin_theta = hypot(q1, q2, q3);
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
- if (fpclassify(q1) != FP_ZERO || fpclassify(q2) != FP_ZERO ||
- fpclassify(q3) != FP_ZERO) {
- const T sin_theta = hypot(q1, q2, q3);
+ if (fpclassify(sin_theta) != FP_ZERO) {
const T& cos_theta = quaternion[0];
// If cos_theta is negative, theta is greater than pi/2, which
diff --git a/internal/ceres/autodiff_manifold_test.cc b/internal/ceres/autodiff_manifold_test.cc
index f4b6156..c7a473d 100644
--- a/internal/ceres/autodiff_manifold_test.cc
+++ b/internal/ceres/autodiff_manifold_test.cc
@@ -143,9 +143,8 @@
template <typename T>
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
T q_delta[4];
- if (fpclassify(delta[0]) != FP_ZERO || fpclassify(delta[1]) != FP_ZERO ||
- fpclassify(delta[2]) != FP_ZERO) {
- T norm_delta = hypot(delta[0], delta[1], delta[2]);
+ T norm_delta = hypot(delta[0], delta[1], delta[2]);
+ if (fpclassify(norm_delta) != FP_ZERO) {
const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
q_delta[0] = cos(norm_delta);
q_delta[1] = sin_delta_by_delta * delta[0];
@@ -171,11 +170,9 @@
T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
T ambient_y_minus_x[4];
QuaternionProduct(y, minus_x, ambient_y_minus_x);
- if (fpclassify(ambient_y_minus_x[1]) != FP_ZERO ||
- fpclassify(ambient_y_minus_x[2]) != FP_ZERO ||
- fpclassify(ambient_y_minus_x[3]) != FP_ZERO) {
- T u_norm = hypot(
- ambient_y_minus_x[1], ambient_y_minus_x[2], ambient_y_minus_x[3]);
+ T u_norm =
+ hypot(ambient_y_minus_x[1], ambient_y_minus_x[2], ambient_y_minus_x[3]);
+ if (fpclassify(u_norm) != FP_ZERO) {
T theta = 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;
