Avoid division by zero Run cleanly under -fsanitize=float-divide-by-zero. Change-Id: I99e92a50c60971c9f771774e58dbe65be7c675ec
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index 367a7d0..dc04a16 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.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 @@ -318,27 +318,28 @@ const T& a0 = angle_axis[0]; const T& a1 = angle_axis[1]; const T& a2 = angle_axis[2]; - const T theta = hypot(a0, a1, a2); + + T k; // For points not at the origin, the full conversion is numerically stable. - if (fpclassify(theta) != FP_ZERO) { + if (fpclassify(a0) != FP_ZERO || fpclassify(a1) != FP_ZERO || + fpclassify(a2) != FP_ZERO) { + const T theta = hypot(a0, a1, a2); const T half_theta = theta * T(0.5); - const T k = sin(half_theta) / theta; + k = sin(half_theta) / theta; quaternion[0] = cos(half_theta); - quaternion[1] = a0 * k; - quaternion[2] = a1 * k; - quaternion[3] = a2 * k; } 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. - const T k(0.5); + k = T(0.5); quaternion[0] = T(1.0); - quaternion[1] = a0 * k; - quaternion[2] = a1 * k; - quaternion[3] = a2 * k; } + + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; } template <typename T> @@ -348,11 +349,14 @@ const T& q1 = quaternion[1]; const T& q2 = quaternion[2]; const T& q3 = quaternion[3]; - const T sin_theta = hypot(q1, q2, q3); + + T k; // For quaternions representing non-zero rotation, the conversion // is numerically stable. - if (fpclassify(sin_theta) != FP_ZERO) { + if (fpclassify(q1) != FP_ZERO || fpclassify(q2) != FP_ZERO || + fpclassify(q3) != FP_ZERO) { + const T sin_theta = hypot(q1, q2, q3); const T& cos_theta = quaternion[0]; // If cos_theta is negative, theta is greater than pi/2, which @@ -368,23 +372,20 @@ // theta - pi = atan(sin(theta - pi), cos(theta - pi)) // = atan(-sin(theta), -cos(theta)) // - const T two_theta = - T(2.0) * ((cos_theta < T(0.0)) ? atan2(-sin_theta, -cos_theta) - : atan2(sin_theta, cos_theta)); - const T k = two_theta / sin_theta; - angle_axis[0] = q1 * k; - angle_axis[1] = q2 * k; - angle_axis[2] = q3 * k; + const T sign = copysign(T(1), cos_theta); + const T two_theta = T(2.0) * atan2(sign * sin_theta, sign * cos_theta); + k = two_theta / sin_theta; } else { // For zero rotation, 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. - const T k(2.0); - angle_axis[0] = q1 * k; - angle_axis[1] = q2 * k; - angle_axis[2] = q3 * k; + k = T(2.0); } + + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; } template <typename T>
diff --git a/internal/ceres/autodiff_manifold_test.cc b/internal/ceres/autodiff_manifold_test.cc index e691da6..f4b6156 100644 --- a/internal/ceres/autodiff_manifold_test.cc +++ b/internal/ceres/autodiff_manifold_test.cc
@@ -142,12 +142,10 @@ struct QuaternionFunctor { template <typename T> bool Plus(const T* x, const T* delta, T* x_plus_delta) const { - const T squared_norm_delta = - delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - T q_delta[4]; - if (squared_norm_delta > T(0.0)) { - T norm_delta = sqrt(squared_norm_delta); + 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]); 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]; @@ -173,10 +171,11 @@ 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); - T u_norm = 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) { + 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 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;
diff --git a/internal/ceres/is_close.cc b/internal/ceres/is_close.cc index 575918b..c652b68 100644 --- a/internal/ceres/is_close.cc +++ b/internal/ceres/is_close.cc
@@ -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 @@ -41,18 +41,19 @@ double* absolute_error) { double local_absolute_error; double local_relative_error; - if (!absolute_error) { + if (absolute_error == nullptr) { absolute_error = &local_absolute_error; } - if (!relative_error) { + if (relative_error == nullptr) { relative_error = &local_relative_error; } *absolute_error = std::fabs(x - y); - *relative_error = *absolute_error / std::max(std::fabs(x), std::fabs(y)); - if (x == 0 || y == 0) { + if (std::fpclassify(x) == FP_ZERO || std::fpclassify(y) == FP_ZERO) { // If x or y is exactly zero, then relative difference doesn't have any // meaning. Take the absolute difference instead. *relative_error = *absolute_error; + } else { + *relative_error = *absolute_error / std::max(std::fabs(x), std::fabs(y)); } return *relative_error < std::fabs(relative_precision); }
diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc index a9de40f..67e5580 100644 --- a/internal/ceres/jet_test.cc +++ b/internal/ceres/jet_test.cc
@@ -103,13 +103,15 @@ Fenv env; // Do not leak floating-point exceptions to the caller double absolute_difference = std::abs(x - y); - double relative_difference = - absolute_difference / std::max(std::abs(x), std::abs(y)); + double relative_difference; if (std::fpclassify(x) == FP_ZERO || std::fpclassify(y) == FP_ZERO) { // If x or y is exactly zero, then relative difference doesn't have any // meaning. Take the absolute difference instead. relative_difference = absolute_difference; + } else { + relative_difference = + absolute_difference / std::max(std::abs(x), std::abs(y)); } return std::islessequal(relative_difference, max_abs_relative_difference); }
diff --git a/internal/ceres/polynomial.cc b/internal/ceres/polynomial.cc index db3a404..a0596c6 100644 --- a/internal/ceres/polynomial.cc +++ b/internal/ceres/polynomial.cc
@@ -70,26 +70,30 @@ scaling_has_changed = false; for (int i = 0; i < degree; ++i) { - const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); const double col_norm = companion_matrix_offdiagonal.col(i).lpNorm<1>(); - // Decompose row_norm/col_norm into mantissa * 2^exponent, - // where 0.5 <= mantissa < 1. Discard mantissa (return value - // of frexp), as only the exponent is needed. - int exponent = 0; - std::frexp(row_norm / col_norm, &exponent); - exponent /= 2; + // Avoid division by zero + if (std::fpclassify(col_norm) != FP_ZERO) { + const double row_norm = companion_matrix_offdiagonal.row(i).lpNorm<1>(); + // Decompose row_norm/col_norm into mantissa * 2^exponent, + // where 0.5 <= mantissa < 1. Discard mantissa (return value + // of frexp), as only the exponent is needed. + int exponent = 0; + std::frexp(row_norm / col_norm, &exponent); + exponent /= 2; - if (exponent != 0) { - const double scaled_col_norm = std::ldexp(col_norm, exponent); - const double scaled_row_norm = std::ldexp(row_norm, -exponent); - if (scaled_col_norm + scaled_row_norm < gamma * (col_norm + row_norm)) { - // Accept the new scaling. (Multiplication by powers of 2 should not - // introduce rounding errors (ignoring non-normalized numbers and - // over- or underflow)) - scaling_has_changed = true; - companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); - companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); + if (exponent != 0) { + const double scaled_col_norm = std::ldexp(col_norm, exponent); + const double scaled_row_norm = std::ldexp(row_norm, -exponent); + if (scaled_col_norm + scaled_row_norm < + gamma * (col_norm + row_norm)) { + // Accept the new scaling. (Multiplication by powers of 2 should not + // introduce rounding errors (ignoring non-normalized numbers and + // over- or underflow)) + scaling_has_changed = true; + companion_matrix_offdiagonal.row(i) *= std::ldexp(1.0, -exponent); + companion_matrix_offdiagonal.col(i) *= std::ldexp(1.0, exponent); + } } } }
diff --git a/internal/ceres/test_util.cc b/internal/ceres/test_util.cc index fc73764..9ae661e 100644 --- a/internal/ceres/test_util.cc +++ b/internal/ceres/test_util.cc
@@ -64,11 +64,14 @@ } double absolute_difference = fabs(x - y); - double relative_difference = absolute_difference / std::max(fabs(x), fabs(y)); - if (x == 0 || y == 0) { + double relative_difference; + if (std::fpclassify(x) == FP_ZERO || std::fpclassify(y) == FP_ZERO) { // If x or y is exactly zero, then relative difference doesn't have any // meaning. Take the absolute difference instead. relative_difference = absolute_difference; + } else { + relative_difference = + absolute_difference / std::max(std::fabs(x), std::fabs(y)); } if (relative_difference > max_abs_relative_difference) { VLOG(1) << absl::StrFormat("x=%17g y=%17g abs=%17g rel=%17g",