Add AccurateNorm Composing two-argument functions for variadic Jet inputs can produce invalid derivatives because intermediate results do not preserve the full derivative relationship. Introduce overflow and underflow safe Euclidean norm with variadic overloads. The Jet overloads compute derivatives from all arguments using the complete chain rule. Change-Id: I1bcb9874416623460e6b663415714a51d4dee2c5
diff --git a/BUILD b/BUILD index 7811d93..ce36537 100644 --- a/BUILD +++ b/BUILD
@@ -60,6 +60,7 @@ ) CERES_TESTS = [ + "accurate_norm", "array_utils", "autodiff_cost_function", "autodiff_manifold",
diff --git a/docs/source/version_history.rst b/docs/source/version_history.rst index 32d6c89..29cd06c 100644 --- a/docs/source/version_history.rst +++ b/docs/source/version_history.rst
@@ -34,6 +34,13 @@ are removed in favor of standard CMake variables. CMake 3.22 or later is now required. See :ref:`options-controlling-ceres-configuration`. +New Features +------------ + +#. Added ``AccurateNorm`` for overflow and underflow safe variadic norms + accurate to nearly full working precision. Its Jet derivatives are defined + wherever the norm is nonzero, unlike those of nested two-argument norms. + 2.2.0 =====
diff --git a/include/ceres/accurate_norm.h b/include/ceres/accurate_norm.h new file mode 100644 index 0000000..2ace148 --- /dev/null +++ b/include/ceres/accurate_norm.h
@@ -0,0 +1,314 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2026 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// +// This header implements functions for accurately computing the Euclidean norm +// of two or more arguments while avoiding underflow and overflow. +// +// The functions accumulate the squares of the arguments as an unevaluated sum +// of the rounded sum and its rounding error, and correct the square root of +// the rounded sum by a single Newton step that accounts for the rounding +// errors. They share the same implementation for any number of arguments: the +// result is computed without rescaling if the largest magnitude is within a +// range where rescaling cannot change the result, and otherwise after +// rescaling all arguments by a fixed radix power. +// +// Unlike the 2-argument algorithm in [1], the functions do not return the +// larger argument if the smaller one is negligible. Neglecting arguments does +// not generalize to more arguments since the errors of several neglected +// squares accumulate. +// +// The implementation is derived from the following paper: +// +// [1] Borges, C. F. (2021). Algorithm 1014: An Improved Algorithm for +// hypot(x,y). ACM Transactions on Mathematical Software, 47(1), 1–12. +// https://doi.org/10.1145/3428446 + +#ifndef CERES_PUBLIC_ACCURATE_NORM_H_ +#define CERES_PUBLIC_ACCURATE_NORM_H_ + +#include <algorithm> +#include <cmath> +#include <limits> +#include <type_traits> +#include <utility> + +#include "ceres/internal/compensated_math.h" + +namespace ceres { + +namespace internal { + +// Helper trait to promote integral types to double and keep floating-point +// types unchanged. +template <typename T, typename Enable = void> +struct Promote {}; + +template <typename T> +struct Promote<T, std::enable_if_t<std::is_integral_v<T>>> { + // The canonical floating-point type for integral inputs. + using type = double; +}; + +template <typename T> +struct Promote<T, std::enable_if_t<std::is_floating_point_v<T>>> { + // Identity mapping. + using type = T; +}; + +// The type of the sum of the promoted arguments, e.g., double if any argument +// is integral and float if all arguments are float. References and +// cv-qualifiers of the argument types are ignored. +template <typename... Ts> +using Promote_t = + decltype((typename Promote<std::decay_t<Ts>>::type(0) + ... + 0)); + +// Computes 2^exponent exactly. Unlike std::scalbn, the function can be +// evaluated in constant expressions which avoids runtime library calls for +// compilers that do not fold std::scalbn. +template <typename T> +constexpr T PowerOfTwo(int exponent) noexcept { + T base = exponent < 0 ? T{0.5} : T{2}; + int n = exponent < 0 ? -exponent : exponent; + T result{1}; + + while (n > 0) { + if (n % 2 == 1) { + result *= base; + } + + n /= 2; + + // Avoid squaring the base beyond the representable range once all bits of + // the exponent have been consumed. + if (n > 0) { + base *= base; + } + } + + return result; +} + +// Computes ⌈log₂(n)⌉ for a positive n. +constexpr int CeilLog2(int n) noexcept { + int result = 0; + + while ((1 << result) < n) { + ++result; + } + + return result; +} + +// The second template parameter allows this trait to be customized using +// SFINAE. +// +// In the following, p denotes the precision of T, and e_min and e_max denote +// its minimum and maximum exponent as defined by IEEE 754, i.e., +// std::numeric_limits<T>::min_exponent − 1 and max_exponent − 1, respectively. +template <typename T, typename Enable = void> +struct AccurateNormTraits { + // Smallest magnitude x whose square has an exactly representable rounding + // error. The error is a multiple of ulp(x)² = 𝛽^(2(e−p+1)) for + // x ∈ [𝛽^e, 𝛽^(e+1)) which must not fall below the smallest subnormal + // 𝛽^(e_min−p+1), i.e., e ≥ ⌈(e_min+p−1)/2⌉. Integer division truncates the + // negative numerator toward zero which yields the ceiling. + static constexpr T Tiny() noexcept { + constexpr int e_min = std::numeric_limits<T>::min_exponent - 1; + return PowerOfTwo<T>((e_min + std::numeric_limits<T>::digits - 1) / 2); + } + + // The norm is computed without rescaling if the largest magnitude among the + // arguments lies within [UnscaledMinimum(), UnscaledMaximum(num_arguments)]. + // Otherwise, all arguments are rescaled by a power of the radix. + // + // Lower bound of the range. Every argument whose square has an inexact + // rounding error is then smaller than 𝛽^(-p) times the largest magnitude and + // cannot affect the result. + static constexpr T UnscaledMinimum() noexcept { + return Tiny() * PowerOfTwo<T>(std::numeric_limits<T>::digits); + } + + // Upper bound of the range for num_arguments arguments. Each square is then + // at most 𝛽^(e_max − ⌈log₂(num_arguments)⌉) such that the sum of the squares + // cannot overflow. + static constexpr T UnscaledMaximum(int num_arguments) noexcept { + constexpr int e_max = std::numeric_limits<T>::max_exponent - 1; + return PowerOfTwo<T>((e_max - CeilLog2(num_arguments)) / 2); + } + + // Exponent of ulp(√F_min) = 𝛽^(e_min/2−p+1) where F_min = 𝛽^e_min is the + // smallest normal value. + static constexpr int ScaleExponent() noexcept { + return (std::numeric_limits<T>::min_exponent - 1) / 2 - + std::numeric_limits<T>::digits + 1; + } +}; + +// Determines the largest magnitude of the arguments. An infinite argument +// yields positive infinity, even if another argument is NaN. Otherwise, a NaN +// argument yields a NaN with its payload preserved. +template <typename T, typename... Args> +inline T MaximumMagnitude(T x, Args... args) noexcept { + using std::fabs; + using std::fmax; + using std::isinf; + using std::isnan; + + // Fold expressions instead of a loop over the arguments allow compilers to + // generate branch-free code for the common case of finite arguments. + if ((isinf(x) || ... || isinf(args))) { + return std::numeric_limits<T>::infinity(); + } + + if ((isnan(x) || ... || isnan(args))) { + // Arithmetic operations propagate the payload of a NaN operand. + return (fabs(x) + ... + fabs(args)); + } + + T maximum = fabs(x); + ((maximum = fmax(maximum, T(fabs(args)))), ...); + return maximum; +} + +// Computes the sum of squares x^2 + y^2 + ... as the rounded sum and its +// rounding error. +template <typename T, typename... Args> +inline std::pair<T, T> UnscaledAccurateSquaredNormWithError(T x, + T y, + Args... args) { + using std::fma; + + // Use 2MultFMA to recover the rounding error from squaring x. + T sum_of_squares = x * x; + T sum_of_squares_error = fma(x, x, -sum_of_squares); + + // Add the remaining squares using a radix-independent error-free transform. + // The rounding errors are small compared to the sum. Accumulating them using + // plain additions is therefore sufficient. + const auto accumulate = [&sum_of_squares, &sum_of_squares_error](T value) { + const T value_sq = value * value; + const auto [sum, sum_error] = TwoSum(sum_of_squares, value_sq); + sum_of_squares = sum; + sum_of_squares_error += sum_error + fma(value, value, -value_sq); + }; + + accumulate(y); + (accumulate(args), ...); + + return std::make_pair(sum_of_squares, sum_of_squares_error); +} + +// Computes sqrt(x^2 + y^2 + ...) without checking the arguments. The arguments +// must be finite and scaled such that the sum of their squares does not +// overflow and the rounding errors of all squares that affect the result are +// exact. Not intended to be invoked by users. +template <typename T, typename... Args> +inline T UnscaledAccurateNorm(T x, T y, Args... args) { + using std::fma; + using std::sqrt; + + // In the following, σ denotes the sum of squares and σ_e its rounding error. + const auto [sum_of_squares, sum_of_squares_error] = + UnscaledAccurateSquaredNormWithError(x, y, args...); + const T h = sqrt(sum_of_squares); + const T tau = sum_of_squares_error + fma(-h, h, sum_of_squares); + // To solve h² = σ, Newton's correction term f/df is + // + // h² − σ + // δ_h = ────── . + // 2⋅h + // + // The update h − δ_h adds its negation, (σ − h²) / (2⋅h). + return fma(tau / h, T(0.5), h); +} + +} // namespace internal + +// Computes the Euclidean norm of two or more floating-point values of the same +// type while avoiding intermediate underflow and overflow. An infinite argument +// produces positive infinity, even if another argument is NaN. Otherwise, a NaN +// argument produces a NaN with its payload preserved. When all arguments are +// zero, the result is zero. +template <typename T, typename... Args> +inline auto AccurateNorm(T a, T b, Args... args) + -> std::enable_if_t<std::is_floating_point_v<T> && + (std::is_same_v<T, Args> && ...), + T> { + using std::fpclassify; + using std::isfinite; + + const T maximum = internal::MaximumMagnitude(a, b, args...); + + if (!isfinite(maximum)) { + return maximum; + } + + if (fpclassify(maximum) == FP_ZERO) { + return 0; + } + + using internal::AccurateNormTraits; + using internal::PowerOfTwo; + using internal::UnscaledAccurateNorm; + + constexpr int num_arguments = 2 + sizeof...(Args); + // Multiplying by a radix power rounds exactly as std::scalbn does. + constexpr int exponent = AccurateNormTraits<T>::ScaleExponent(); + constexpr T down = PowerOfTwo<T>(exponent); + constexpr T up = PowerOfTwo<T>(-exponent); + + // Rescale only if the largest magnitude is outside the range where rescaling + // cannot change the result. Rescaling moves the largest magnitude into that + // range. Arguments that underflow or whose squares have inexact rounding + // errors after scaling down are negligible compared to the largest one. + // Scaling up makes the rounding errors of all squares exact, including those + // of subnormal arguments. + if (maximum > AccurateNormTraits<T>::UnscaledMaximum(num_arguments)) { + return UnscaledAccurateNorm(a * down, b * down, args * down...) * up; + } + + if (maximum < AccurateNormTraits<T>::UnscaledMinimum()) { + return UnscaledAccurateNorm(a * up, b * up, args * up...) * down; + } + + return UnscaledAccurateNorm(a, b, args...); +} + +// Computes the Euclidean norm of two or more arithmetic values after promoting +// all arguments to a common floating-point type. +template <typename T, typename U, typename... Args> +inline internal::Promote_t<T, U, Args...> AccurateNorm(T a, U b, Args... args) { + using PromotedType = internal::Promote_t<T, U, Args...>; + return AccurateNorm(PromotedType(a), PromotedType(b), PromotedType(args)...); +} + +} // namespace ceres + +#endif // CERES_PUBLIC_ACCURATE_NORM_H_
diff --git a/include/ceres/constants.h b/include/ceres/constants.h index 584b669..e3c0680 100644 --- a/include/ceres/constants.h +++ b/include/ceres/constants.h
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2023 Google Inc. All rights reserved. +// Copyright 2026 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -33,10 +33,26 @@ // TODO(HSHelson): This header should no longer be necessary once C++20's // <numbers> (e.g. std::numbers::pi_v) becomes usable + +// The constants are computed using mpmath by the following shell command: +// +// python3 - <<EOF +// from mpmath import mp +// mp.dps = 64 +// print(mp.pi) +// print(mp.sqrt(3)) +// EOF namespace ceres::constants { +// π template <typename T> -inline constexpr T pi_v(3.141592653589793238462643383279502884); +inline constexpr T pi_v( + 3.141592653589793238462643383279502884197169399375105820974944592L); inline constexpr double pi = pi_v<double>; +// √3 +template <typename T> +inline constexpr T sqrt_3_v( + 1.732050807568877293527446341505872366942805253810380628055806979L); +inline constexpr double sqrt_3 = sqrt_3_v<double>; } // namespace ceres::constants #endif // CERES_PUBLIC_CONSTANTS_H_
diff --git a/include/ceres/internal/compensated_math.h b/include/ceres/internal/compensated_math.h new file mode 100644 index 0000000..8efc7e5 --- /dev/null +++ b/include/ceres/internal/compensated_math.h
@@ -0,0 +1,54 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2026 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) + +#ifndef CERES_PUBLIC_INTERNAL_COMPENSATED_MATH_H_ +#define CERES_PUBLIC_INTERNAL_COMPENSATED_MATH_H_ + +#include <type_traits> +#include <utility> + +namespace ceres::internal { + +// Similar to Fast2Sum, but without requiring ordering or a specific radix. +template <typename T> +constexpr auto TwoSum(T a, T b) + -> std::enable_if_t<std::is_floating_point_v<T>, std::pair<T, T>> { + const T s = a + b; + const T a_prime = s - b; + const T b_prime = s - a_prime; + const T delta_a = a - a_prime; + const T delta_b = b - b_prime; + const T t = delta_a + delta_b; + return std::make_pair(s, t); +} + +} // namespace ceres::internal + +#endif // CERES_PUBLIC_INTERNAL_COMPENSATED_MATH_H_
diff --git a/include/ceres/jet.h b/include/ceres/jet.h index 3672011..bc4d080 100644 --- a/include/ceres/jet.h +++ b/include/ceres/jet.h
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2024 Google Inc. All rights reserved. +// Copyright 2026 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -167,6 +167,7 @@ #include <type_traits> #include "Eigen/Core" +#include "ceres/accurate_norm.h" #include "ceres/internal/jet_traits.h" #include "ceres/internal/port.h" #include "ceres/jet_fwd.h" @@ -528,6 +529,36 @@ return Jet<T, N>(abs(f.a), copysign(T(1), f.a) * f.v); } +// AccurateNorm(x, y, ...) computes the Euclidean norm of its arguments while +// avoiding underflow and overflow. Therefore, the function can be formally +// defined as +// +// AccurateNorm(x, y, ...) = sqrt(x^2 + y^2 + ...) +// +// where +// +// d/dx sqrt(x^2 + y^2 + ...) = x / sqrt(x^2 + y^2 + ...) +// d/dy sqrt(x^2 + y^2 + ...) = y / sqrt(x^2 + y^2 + ...) +// ... +// +// with the dual representation given by +// +// AccurateNorm(x + dx, y + dy, ...) ~= +// sqrt(x^2 + y^2 + ...) + +// (x dx + y dy + ...) / sqrt(x^2 + y^2 + ...) +// +// The derivatives are undefined when all arguments are zero. +template <typename T, int N, typename... Args> +inline auto AccurateNorm(const Jet<T, N>& x, + const Jet<T, N>& y, + const Args&... args) + -> std::enable_if_t<(std::is_same_v<Args, Jet<T, N>> && ...), Jet<T, N>> { + const T tmp = AccurateNorm(x.a, y.a, args.a...); + return Jet<T, N>( + tmp, + ((x.a / tmp * x.v + y.a / tmp * y.v) + ... + (args.a / tmp * args.v))); +} + // copysign(a, b) composes a float with the magnitude of a and the sign of b. // Therefore, the function can be formally defined as //
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index 6571a68..96a4aac 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 2025 Google Inc. All rights reserved. +// Copyright 2026 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -51,6 +51,7 @@ #include <type_traits> #include "absl/log/check.h" +#include "ceres/accurate_norm.h" #include "ceres/constants.h" #include "ceres/internal/euler_angles.h" @@ -376,19 +377,29 @@ return MatrixAdapter<T, 3, 1>(pointer); } +namespace internal { + +// Determine whether any of the values is not exactly a binary floating-point +// zero. +template <typename... T> +constexpr bool AnyExactlyNonZero(const T&... values) noexcept { + using std::fpclassify; + return ((fpclassify(values) != FP_ZERO) || ...); +} + +} // namespace internal + template <typename Order, typename T> inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { - using std::fpclassify; - using std::hypot; const T& a0 = angle_axis[0]; const T& a1 = angle_axis[1]; 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(theta) != FP_ZERO) { + if (internal::AnyExactlyNonZero(a0, a1, a2)) { + const T theta = AccurateNorm(a0, a1, a2); const T half_theta = theta * T(0.5); k = sin(half_theta) / theta; quaternion[Order::kW] = cos(half_theta); @@ -408,18 +419,16 @@ template <typename Order, typename T> inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { - using std::fpclassify; - using std::hypot; 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); // For quaternions representing non-zero rotation, the conversion // is numerically stable. - if (fpclassify(sin_theta) != FP_ZERO) { + if (internal::AnyExactlyNonZero(q1, q2, q3)) { + const T sin_theta = AccurateNorm(q1, q2, q3); const T& cos_theta = quaternion[Order::kW]; // If cos_theta is negative, theta is greater than pi/2, which @@ -517,14 +526,13 @@ template <typename T, int row_stride, int col_stride> void AngleAxisToRotationMatrix( const T* angle_axis, const MatrixAdapter<T, row_stride, col_stride>& R) { - using std::fpclassify; - using std::hypot; static const T kOne = T(1.0); - const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]); - if (fpclassify(theta) != FP_ZERO) { + if (internal::AnyExactlyNonZero( + angle_axis[0], angle_axis[1], angle_axis[2])) { // We want to be careful to only evaluate the square root if the // norm of the angle_axis vector is greater than zero. Otherwise // we get a division by zero. + const T theta = AccurateNorm(angle_axis[0], angle_axis[1], angle_axis[2]); const T wx = angle_axis[0] / theta; const T wy = angle_axis[1] / theta; const T wz = angle_axis[2] / theta; @@ -627,15 +635,13 @@ void RotationMatrixToEulerAngles( const MatrixAdapter<const T, row_stride, col_stride>& R, T* euler) { using std::atan2; - using std::fpclassify; - using std::hypot; const auto [i, j, k] = EulerSystem::kAxes; T ea[3]; if constexpr (EulerSystem::kIsProperEuler) { - if (fpclassify(R(i, j)) != FP_ZERO || fpclassify(R(i, k)) != FP_ZERO) { - const T sy = hypot(R(i, j), R(i, k)); + if (internal::AnyExactlyNonZero(R(i, j), R(i, k))) { + const T sy = AccurateNorm(R(i, j), R(i, k)); ea[0] = atan2(R(i, j), R(i, k)); ea[1] = atan2(sy, R(i, i)); ea[2] = atan2(R(j, i), -R(k, i)); @@ -648,8 +654,8 @@ ea[2] = T(0.0); } } else { - if (fpclassify(R(i, i)) != FP_ZERO || fpclassify(R(j, i)) != FP_ZERO) { - const T cy = hypot(R(i, i), R(j, i)); + if (internal::AnyExactlyNonZero(R(i, i), R(j, i))) { + const T cy = AccurateNorm(R(i, i), R(j, i)); ea[0] = atan2(R(k, j), R(k, k)); ea[1] = atan2(-R(k, i), cy); ea[2] = atan2(R(j, i), R(i, i)); @@ -871,13 +877,10 @@ const T pt[3], T result[3]) { using std::cos; - using std::fpclassify; - using std::hypot; using std::sin; - const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]); - - if (fpclassify(theta) != FP_ZERO) { + if (internal::AnyExactlyNonZero( + angle_axis[0], angle_axis[1], angle_axis[2])) { // Away from zero, use the rodriguez formula // // result = pt costheta + @@ -888,6 +891,7 @@ // norm of the angle_axis vector is greater than zero. Otherwise // we get a division by zero. // + const T theta = AccurateNorm(angle_axis[0], angle_axis[1], angle_axis[2]); const T costheta = cos(theta); const T sintheta = sin(theta); const T theta_inverse = T(1.0) / theta;
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 9a20047..4b28ae1 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -384,6 +384,7 @@ set_tests_properties(${NAME}_test PROPERTIES ENVIRONMENT TEST_SRCDIR=${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) endmacro (CERES_TEST) + ceres_test(accurate_norm) ceres_test(array_utils) ceres_test(array_selector) ceres_test(autodiff)
diff --git a/internal/ceres/accurate_norm_test.cc b/internal/ceres/accurate_norm_test.cc new file mode 100644 index 0000000..8345230 --- /dev/null +++ b/internal/ceres/accurate_norm_test.cc
@@ -0,0 +1,403 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2026 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) + +#include "ceres/accurate_norm.h" + +#include <cfloat> +#include <cmath> +#include <cstring> +#include <limits> +#include <type_traits> + +#include "ceres/constants.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +// Matches if the argument is at most n floating-point values away from the +// expected value. +MATCHER_P2(MaxNumUlp, expected, n, "") { + using Scalar = std::decay_t<decltype(arg)>; + const Scalar target = static_cast<Scalar>(expected); + Scalar value = arg; + + for (int distance = 0; value != target && distance < n; ++distance) { + value = std::nextafter(value, target); + } + + *result_listener << "actual " << arg << " is not within " << n + << " ULP of expected " << target; + return value == target; +} + +TEST(AccurateNorm, Promote) { + static_assert(std::is_same_v<ceres::internal::Promote_t<int>, double>, + "Promotion of an int must be a double"); + static_assert(std::is_same_v<ceres::internal::Promote_t<int, int>, double>, + "Promotion of multuple ints must be a double"); + static_assert(std::is_same_v<ceres::internal::Promote_t<unsigned>, double>, + "Promotion of an unsigned int must be double"); + static_assert(std::is_same_v<ceres::internal::Promote_t<long>, double>, + "Promotion of a long must be double"); + static_assert( + std::is_same_v<ceres::internal::Promote_t<int, long, float>, double>, + "Promotion of arithmetic types must be double"); +} + +TEST(AccurateNorm, PromotesLvalueArguments) { + int integer = 3; + double floating_point = 4.0; + double zero = 0.0; + + EXPECT_EQ(ceres::AccurateNorm(integer, floating_point, zero), 5.0); +} + +TEST(AccurateNorm, PromotesIntegralArgumentsOfTheSameType) { + static_assert(std::is_same_v<decltype(ceres::AccurateNorm(3, 4)), double>); + static_assert(std::is_same_v<decltype(ceres::AccurateNorm(3, 4, 0)), double>); + static_assert( + std::is_same_v<decltype(ceres::AccurateNorm(3.0F, 4.0F, 0.0F)), float>); + + EXPECT_EQ(ceres::AccurateNorm(3, 4), 5.0); + EXPECT_EQ(ceres::AccurateNorm(3, 4, 0), 5.0); +} + +#if GTEST_HAS_TYPED_TEST + +template <typename T> +class AccurateNormTest : public testing::Test { + public: + static constexpr auto kTiny = std::numeric_limits<T>::min(); + static constexpr auto kHuge = std::numeric_limits<T>::max(); +}; + +using Types = testing::Types<float, double, long double>; + +TYPED_TEST_SUITE(AccurateNormTest, Types); + +TEST(AccurateNorm, ScaleIsRadixExponent) { + using Traits = ceres::internal::AccurateNormTraits<double>; + + // ulp(√F_min) = ulp(2^-511) = 2^-563 for double + static_assert(std::is_same_v<decltype(Traits::ScaleExponent()), int>); + static_assert(Traits::ScaleExponent() == -563); +} + +TYPED_TEST(AccurateNormTest, Norm) { + using Scalar = TypeParam; + + EXPECT_THAT(ceres::AccurateNorm(this->kTiny, Scalar{0}), + MaxNumUlp(this->kTiny, 0)); + EXPECT_THAT(ceres::AccurateNorm(this->kTiny, Scalar{0}, Scalar{0}), + MaxNumUlp(this->kTiny, 0)); + EXPECT_THAT(ceres::AccurateNorm(Scalar{0}, this->kTiny), + MaxNumUlp(this->kTiny, 0)); + EXPECT_THAT(ceres::AccurateNorm(Scalar{0}, Scalar{0}, this->kTiny), + MaxNumUlp(this->kTiny, 0)); + + EXPECT_THAT(ceres::AccurateNorm(this->kHuge, Scalar{0}), + MaxNumUlp(this->kHuge, 0)); + EXPECT_THAT(ceres::AccurateNorm(this->kHuge, Scalar{0}, Scalar{0}), + MaxNumUlp(this->kHuge, 0)); + EXPECT_THAT(ceres::AccurateNorm(Scalar{0}, this->kHuge), + MaxNumUlp(this->kHuge, 0)); + EXPECT_THAT(ceres::AccurateNorm(Scalar{0}, Scalar{0}, this->kHuge), + MaxNumUlp(this->kHuge, 0)); + + EXPECT_THAT(ceres::AccurateNorm(this->kTiny, this->kTiny), + MaxNumUlp(this->kTiny * std::sqrt(Scalar{2}), 1)); + + EXPECT_THAT(ceres::AccurateNorm(Scalar{0}, Scalar{0}), + MaxNumUlp(Scalar{0}, 0)); + + EXPECT_TRUE(std::isinf( + ceres::AccurateNorm(+std::numeric_limits<Scalar>::infinity(), 0))); + EXPECT_TRUE(std::isinf( + ceres::AccurateNorm(-std::numeric_limits<Scalar>::infinity(), 0))); + + EXPECT_TRUE(std::isinf( + ceres::AccurateNorm(0, +std::numeric_limits<Scalar>::infinity()))); + EXPECT_TRUE(std::isinf( + ceres::AccurateNorm(0, -std::numeric_limits<Scalar>::infinity()))); + + EXPECT_TRUE(std::isnan( + ceres::AccurateNorm(std::numeric_limits<Scalar>::quiet_NaN(), 0))); + EXPECT_TRUE(std::isnan( + ceres::AccurateNorm(0, std::numeric_limits<Scalar>::quiet_NaN()))); +} + +TEST(AccurateNorm, VariadicNormAccuracy) { + EXPECT_THAT(ceres::AccurateNorm(1.0, 1.0, 1.0), + MaxNumUlp(ceres::constants::sqrt_3, 0)); + + // Combination exposing a difference of at least two ULPs in inaccurate + // implementations found by random search. + constexpr double kFirst = 0.0; + constexpr double kSecond = -0x1.c4a46e8d5e9f3p-940; + constexpr double kThird = 0x1.2870a0a1f3fa3p-943; + constexpr double kExpected = 0x1.c628110110bf1p-940; + EXPECT_THAT(ceres::AccurateNorm(kFirst, kSecond, kThird), + MaxNumUlp(kExpected, 0)); +} + +TEST(AccurateNorm, HandlesTableMakerDilemma) { + // Values from Borges, Algorithm 1014, Section 6. + constexpr double kFirst = 0x1.a308e1455f447p+0; + constexpr double kSecond = 0x1.9d931a83ef879p+0; + constexpr double kExpected = 0x1.2660d009d54f9p+1; + + EXPECT_THAT(ceres::AccurateNorm(kFirst, kSecond), MaxNumUlp(kExpected, 1)); +} + +TEST(AccurateNorm, ReturnsLargerArgumentWhenSmallerIsNegligible) { + constexpr double kLarger = 1.0; + // √(ε/2) is the largest ratio of the arguments for which the norm equals the + // larger argument. + constexpr double kSmaller = 0x1.6a09e667f3bcdp-27; + + EXPECT_EQ(ceres::AccurateNorm(kLarger, kSmaller), kLarger); +} + +TYPED_TEST(AccurateNormTest, AccountsForSmallerArgumentAboveCutoff) { + using Scalar = TypeParam; + // The squared ratio of the arguments equals the machine epsilon which raises + // the norm by three quarters of an ULP above the larger argument. + constexpr Scalar kLarger{1.5}; + const Scalar kSmaller = + kLarger * std::sqrt(std::numeric_limits<Scalar>::epsilon()); + const Scalar kExpected = + std::nextafter(kLarger, std::numeric_limits<Scalar>::infinity()); + + EXPECT_EQ(ceres::AccurateNorm(kLarger, kSmaller), kExpected); +} + +TEST(AccurateNorm, RecoversSquaringErrorsOfSmallArguments) { + // The arguments are not rescaled but their squares are small enough for the + // rounding errors of the squares to underflow unless the scaling threshold + // accounts for them. + EXPECT_EQ(ceres::AccurateNorm(0x1.23342ep-63f, 0x1.271008p-63f), + 0x1.9e8fdcp-63f); + EXPECT_EQ(ceres::AccurateNorm(0x1.4a15545f86ef4p-510, 0x1.3a578a98bcf9dp-510), + 0x1.c7d03b992e6a8p-510); +#if LDBL_MANT_DIG == 64 && LDBL_MIN_EXP == -16381 + EXPECT_EQ(ceres::AccurateNorm(0x8.b912469c50fab7ap-8194L, + 0x8.c54f2ded5ba8268p-8194L), + 0xc.5eb4864ce876f86p-8194L); +#endif +} + +TEST(AccurateNorm, IsAtLeastAsAccurateAsHypot) { + // The expected values are correctly rounded. Implementations of std::hypot, + // e.g., the one shipped with libstdc++ 16, are off by one ULP for these + // arguments. + constexpr double kFirst = 0x1.424bf2ed916bfp+0; + constexpr double kSecond = 0x1.1435107c5d458p+0; + constexpr double kExpected = 0x1.a8758df39043fp+0; + + EXPECT_EQ(ceres::AccurateNorm(kFirst, kSecond), kExpected); + EXPECT_LE(std::fabs(ceres::AccurateNorm(kFirst, kSecond) - kExpected), + std::fabs(std::hypot(kFirst, kSecond) - kExpected)); + + constexpr double kThreeFirst = 0x1.c11f6531eb66ep+0; + constexpr double kThreeSecond = 0x1.f30567547a34cp+0; + constexpr double kThreeThird = 0x1.1e0edcc120696p+0; + constexpr double kThreeExpected = 0x1.6ce2663da03a7p+1; + + EXPECT_EQ(ceres::AccurateNorm(kThreeFirst, kThreeSecond, kThreeThird), + kThreeExpected); + EXPECT_LE( + std::fabs(ceres::AccurateNorm(kThreeFirst, kThreeSecond, kThreeThird) - + kThreeExpected), + std::fabs(std::hypot(kThreeFirst, kThreeSecond, kThreeThird) - + kThreeExpected)); +} + +TYPED_TEST(AccurateNormTest, PowerOfTwoIsExact) { + using Scalar = TypeParam; + using std::scalbn; + constexpr int kMinExponent = std::numeric_limits<Scalar>::min_exponent - + std::numeric_limits<Scalar>::digits; + constexpr int kMaxExponent = std::numeric_limits<Scalar>::max_exponent - 1; + + for (int exponent = kMinExponent; exponent <= kMaxExponent; ++exponent) { + EXPECT_EQ(ceres::internal::PowerOfTwo<Scalar>(exponent), + scalbn(Scalar{1}, exponent)) + << "exponent " << exponent; + } +} + +TEST(AccurateNorm, TraitsAreConstantExpressions) { + // Compilers that do not fold std::scalbn would otherwise compute the + // thresholds at runtime on every invocation. + using DoubleTraits = ceres::internal::AccurateNormTraits<double>; + static_assert(DoubleTraits::Tiny() == 0x1p-485); + static_assert(DoubleTraits::UnscaledMinimum() == 0x1p-432); + static_assert(DoubleTraits::UnscaledMaximum(2) == 0x1p+511); + static_assert(DoubleTraits::UnscaledMaximum(3) == 0x1p+510); + + using FloatTraits = ceres::internal::AccurateNormTraits<float>; + static_assert(FloatTraits::Tiny() == 0x1p-51f); + static_assert(FloatTraits::UnscaledMinimum() == 0x1p-27f); + static_assert(FloatTraits::UnscaledMaximum(3) == 0x1p+62f); +} + +TEST(AccurateNorm, VariadicNormIsAccurateForExtremeMagnitudes) { + // Subnormal arguments + EXPECT_EQ(ceres::AccurateNorm(0x0.123456789abcdp-1022, + 0x0.fedcba9876543p-1022, + 0x0.0000000000001p-1022), + 0x0.ff82f53036b9cp-1022); + // Small arguments including a negligible one + EXPECT_EQ(ceres::AccurateNorm( + 0x1.3a578a98bcf9dp-600, -0x1.4a15545f86ef4p-601, 0x1.5p-650), + 0x1.6308e012498e5p-600); + // Arguments close to the largest finite value + EXPECT_EQ(ceres::AccurateNorm(0x1.ffffffffffffp+1022, + 0x1.4a15545f86ef4p+1021, + -0x1.3a578a98bcf9dp+1000), + 0x1.0cf8b69a0aff1p+1023); + // Large arguments including a negligible one + EXPECT_EQ(ceres::AccurateNorm(0x1.8p+1000, 0x1p-1000, 0x1.4p+999), + 0x1.ap+1000); + // The norm exceeds the largest finite value + EXPECT_EQ(ceres::AccurateNorm(0x1.ep+1023, 0x1.ep+1023, 0x1.ep+1023), + std::numeric_limits<double>::infinity()); +} + +TEST(AccurateNorm, VariadicNormIsAccurateAroundUnscaledRange) { + constexpr double kSqrt3Largest = 0x1.bb67ae8584caap+510; + constexpr double kSqrt3Smallest = 0x1.bb67ae8584caap-432; + + // Inside the range that requires no rescaling + EXPECT_EQ(ceres::AccurateNorm(0x1p+510, 0x1p+510, 0x1p+510), kSqrt3Largest); + EXPECT_EQ(ceres::AccurateNorm(0x1p-432, 0x1p-432, 0x1p-432), kSqrt3Smallest); + EXPECT_EQ(ceres::AccurateNorm( + 0x1.3a578a98bcf9dp+509, -0x1.4a15545f86ef4p+510, 0x1.5p-200), + 0x1.6d979f8e15b5ap+510); + + // Just outside the range that requires no rescaling + EXPECT_EQ(ceres::AccurateNorm(0x1.fffffffffffffp+510, + 0x1.fffffffffffffp+510, + 0x1.fffffffffffffp+510), + 0x1.bb67ae8584caap+511); + EXPECT_EQ(ceres::AccurateNorm(0x1.fffffffffffffp-433, + 0x1.fffffffffffffp-433, + 0x1.fffffffffffffp-433), + kSqrt3Smallest); + EXPECT_EQ(ceres::AccurateNorm( + 0x1.4a15545f86ef4p-432, -0x1.3a578a98bcf9dp-433, 0x1.5p-500), + 0x1.6d979f8e15b5ap-432); +} + +TEST(AccurateNorm, RescalesByRadixPowersWithoutRounding) { + constexpr int kLargeExponent = 512; + constexpr int kSmallExponent = -514; + + EXPECT_EQ(ceres::AccurateNorm(std::scalbn(3.0, kLargeExponent), + std::scalbn(4.0, kLargeExponent)), + std::scalbn(5.0, kLargeExponent)); + EXPECT_EQ(ceres::AccurateNorm(std::scalbn(3.0, kSmallExponent), + std::scalbn(4.0, kSmallExponent)), + std::scalbn(5.0, kSmallExponent)); +} + +TEST(AccurateNorm, VariadicNormHandlesWideDynamicRange) { + // Combination found by random search that exposes a difference of more than + // two ULPs when normalized values are not stored. + constexpr double kFirst = -0x1.5fdef349a2773p+922; + constexpr double kSecond = -0x1.72ec46b66e1d9p-114; + constexpr double kThird = -0x1.eeb9ef28337eep-462; + constexpr double kFourth = -0x1.137462de2cf44p+205; + constexpr double kFifth = -0x1.3a6cb8edf6622p+264; + constexpr double kSixth = 0x1.0f5b36fe2970ap-326; + constexpr double kSeventh = -0x1.5483d7f40eca8p-537; + constexpr double kEighth = 0x1.4694b77d1bb38p-643; + constexpr double kExpectedNorm = 0x1.5fdef349a2773p+922; + + EXPECT_THAT( + ceres::AccurateNorm( + kFirst, kSecond, kThird, kFourth, kFifth, kSixth, kSeventh, kEighth), + MaxNumUlp(kExpectedNorm, 0)); +} + +TEST(AccurateNorm, VariadicNormRescalesLargeArguments) { + constexpr double kFirst = -0x1.5cf602c1b383ep-360; + constexpr double kSecond = 0x1.e7855aa96a0c7p+850; + constexpr double kThird = -0x1.e3bf678284e4p+876; + constexpr double kFourth = 0x1.7d18d55d5723fp+173; + constexpr double kFifth = -0x1.1a6c3b4a7f1f8p+437; + constexpr double kSixth = -0x1.14b8d80b68457p-561; + constexpr double kExpectedNorm = 0x1.e3bf678284e41p+876; + + EXPECT_THAT( + ceres::AccurateNorm(kFirst, kSecond, kThird, kFourth, kFifth, kSixth), + MaxNumUlp(kExpectedNorm, 0)); +} + +TEST(AccurateNorm, VariadicNormReturnsPositiveInfinity) { + constexpr double kInfinity = std::numeric_limits<double>::infinity(); + + EXPECT_EQ(ceres::AccurateNorm(-kInfinity, 1.0, 2.0), kInfinity); +} + +TEST(AccurateNorm, NonfiniteArgumentHandling) { + constexpr double kInfinity = std::numeric_limits<double>::infinity(); + constexpr double kNaN = std::numeric_limits<double>::quiet_NaN(); + + EXPECT_EQ(ceres::AccurateNorm(kNaN, kInfinity), kInfinity); + EXPECT_EQ(ceres::AccurateNorm(kInfinity, kNaN), kInfinity); + EXPECT_EQ(ceres::AccurateNorm(1.0, kInfinity, kNaN), kInfinity); + EXPECT_EQ(ceres::AccurateNorm(kNaN, 1.0, kInfinity), kInfinity); + EXPECT_EQ(ceres::AccurateNorm(kNaN, kInfinity, 1.0), kInfinity); + EXPECT_EQ(ceres::AccurateNorm(kInfinity, kNaN, 1.0), kInfinity); + + EXPECT_TRUE(std::isnan(ceres::AccurateNorm(kNaN, kNaN, 1.0))); + EXPECT_TRUE(std::isnan(ceres::AccurateNorm(1.0, kNaN, kNaN))); + EXPECT_TRUE(std::isnan(ceres::AccurateNorm(kNaN, 1.0, kNaN))); +} + +TEST(AccurateNorm, PreservesNaNPayloadAcrossArity) { + const double nan = std::copysign(std::nan("12345"), -1.0); + + const double norm = ceres::AccurateNorm(nan, 1.0); + const double variadic_norm = ceres::AccurateNorm(nan, 1.0, 2.0); + EXPECT_EQ(std::memcmp(&variadic_norm, &norm, sizeof(double)), 0); +} + +TEST(AccurateNorm, PreservesNaNPayloadOfAnyArgument) { + const double nan = std::copysign(std::nan("12345"), -1.0); + const double expected = std::fabs(nan); + + const double first = ceres::AccurateNorm(nan, 1.0, 2.0); + const double last = ceres::AccurateNorm(1.0, 2.0, nan); + EXPECT_EQ(std::memcmp(&first, &expected, sizeof(double)), 0); + EXPECT_EQ(std::memcmp(&last, &expected, sizeof(double)), 0); +} + +#endif
diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc index 67e5580..6e1271c 100644 --- a/internal/ceres/jet_test.cc +++ b/internal/ceres/jet_test.cc
@@ -674,6 +674,47 @@ #endif } +TEST(Jet, AccurateNorm) { + // The arguments form Pythagorean quadruples to obtain exact norms. + const J a = MakeJet(3.0, 1.0, 0.0); + const J b = MakeJet(4.0, 0.0, 1.0); + const J c = MakeJet(12.0, 2.0, -1.0); + const J d = MakeJet(84.0, -1.0, 3.0); + + // d/dx_i sqrt(x_1^2 + ... + x_n^2) = x_i / sqrt(x_1^2 + ... + x_n^2) + EXPECT_THAT(AccurateNorm(a, b), + IsAlmostEqualTo(MakeJet(5.0, 3.0 / 5.0, 4.0 / 5.0))); + EXPECT_THAT(AccurateNorm(a, b, c), + IsAlmostEqualTo(MakeJet(13.0, 27.0 / 13.0, -8.0 / 13.0))); + EXPECT_THAT(AccurateNorm(a, b, c, d), + IsAlmostEqualTo(MakeJet(85.0, -57.0 / 85.0, 244.0 / 85.0))); + + // Arguments that are zero do not contribute to the derivative. + const J zero = MakeJet(0.0, 2.0, 3.14); + EXPECT_THAT(AccurateNorm(a, zero), IsAlmostEqualTo(a)); + EXPECT_THAT(AccurateNorm(zero, b, zero), IsAlmostEqualTo(b)); + EXPECT_THAT(AccurateNorm(zero, zero, zero, d), IsAlmostEqualTo(d)); + EXPECT_THAT(AccurateNorm(-a, zero, zero), IsAlmostEqualTo(a)); + + EXPECT_THAT(AccurateNorm(x, y), IsAlmostEqualTo(hypot(x, y))); + EXPECT_THAT(AccurateNorm(x, y, z), IsAlmostEqualTo(hypot(x, y, z))); +} + +template <typename AlwaysVoid, typename... Args> +struct IsAccurateNormCallable : std::false_type {}; + +template <typename... Args> +struct IsAccurateNormCallable< + std::void_t<decltype(AccurateNorm(std::declval<Args>()...))>, + Args...> : std::true_type {}; + +static_assert(IsAccurateNormCallable<void, J, J>::value); +static_assert(IsAccurateNormCallable<void, J, J, const J&, J&>::value); +static_assert(!IsAccurateNormCallable<void, J, J, double>::value, + "AccurateNorm must not accept mixed Jet and scalar arguments"); +static_assert(!IsAccurateNormCallable<void, J, J, Jet<double, 3>>::value, + "AccurateNorm must not accept Jets of different sizes"); + #ifdef CERES_HAS_CPP20 TEST(Jet, Lerp) {
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index 3a2c117..b60a68d 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc
@@ -32,6 +32,7 @@ #include <algorithm> #include <array> +#include <cfenv> #include <cmath> #include <limits> #include <random> @@ -1499,6 +1500,34 @@ testing::Pointwise(JetClose(kTolerance), expected_quarter_turn)); } +// Differentiating the norm at gimbal lock divides zero by zero, even if the +// resulting derivative is discarded afterwards. +TEST(EulerAngles, RotationMatrixToEulerAnglesAtGimbalLockDoesNotDivideByZero) { + using J = Jet<double, 9>; + constexpr int kNumEntries = 9; + constexpr double kIdentity[kNumEntries] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + constexpr double kQuarterTurnAboutY[kNumEntries] = { + 0, 0, 1, 0, 1, 0, -1, 0, 0}; + + J identity[kNumEntries]; + J quarter_turn[kNumEntries]; + + for (int entry = 0; entry < kNumEntries; ++entry) { + identity[entry] = J(kIdentity[entry], entry); + quarter_turn[entry] = J(kQuarterTurnAboutY[entry], entry); + } + + J proper_angles[3]; + J tait_bryan_angles[3]; + + std::feclearexcept(FE_INVALID); + RotationMatrixToEulerAngles<IntrinsicZXZ>(identity, proper_angles); + RotationMatrixToEulerAngles<IntrinsicZYX>(quarter_turn, tait_bryan_angles); + const bool invalid = std::fetestexcept(FE_INVALID) != 0; + + EXPECT_FALSE(invalid); +} + // Test rotation matrix to ZXY/312 Intrinsic Euler Angles conversion using Jets // The two ZXY test cases specifically cover handling of Tait-Bryan angles // i.e. last axis of rotation is different from the first