blob: 2ace148cae8931519ac5ff40df4fc067a0c5debc [file]
// 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_