Use hypot to compute the L^2 norm Change-Id: I908eaaa279452aa16346dfd3f25aac53685e3172
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index a92d84a..1220173 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 2019 Google Inc. All rights reserved. +// Copyright 2023 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -47,7 +47,6 @@ #include <algorithm> #include <cmath> -#include <limits> #include "ceres/constants.h" #include "ceres/internal/euler_angles.h" @@ -314,14 +313,15 @@ template <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]; - const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + const T theta = hypot(a0, a1, a2); // For points not at the origin, the full conversion is numerically stable. - if (theta_squared > T(0.0)) { - const T theta = sqrt(theta_squared); + if (fpclassify(theta) != FP_ZERO) { const T half_theta = theta * T(0.5); const T k = sin(half_theta) / theta; quaternion[0] = cos(half_theta); @@ -343,15 +343,16 @@ template <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 sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; + const T sin_theta = hypot(q1, q2, q3); // For quaternions representing non-zero rotation, the conversion // is numerically stable. - if (sin_squared_theta > T(0.0)) { - const T sin_theta = sqrt(sin_squared_theta); + if (fpclassify(sin_theta) != FP_ZERO) { const T& cos_theta = quaternion[0]; // If cos_theta is negative, theta is greater than pi/2, which @@ -452,13 +453,14 @@ 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 theta2 = DotProduct(angle_axis, angle_axis); - if (theta2 > T(std::numeric_limits<double>::epsilon())) { + const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]); + if (fpclassify(theta) != FP_ZERO) { // 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 = sqrt(theta2); const T wx = angle_axis[0] / theta; const T wy = angle_axis[1] / theta; const T wz = angle_axis[2] / theta; @@ -478,7 +480,7 @@ R(2, 2) = costheta + wz*wz*(kOne - costheta); // clang-format on } else { - // Near zero, we switch to using the first order Taylor expansion. + // At zero, we switch to using the first order Taylor expansion. R(0, 0) = kOne; R(1, 0) = angle_axis[2]; R(2, 0) = -angle_axis[1]; @@ -741,10 +743,10 @@ template <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."; + using std::hypot; // '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]); + const T scale = T(1) / hypot(q[0], q[1], hypot(q[2], q[3])); // Make unit-norm version of q. const T unit[4] = { @@ -791,9 +793,12 @@ const T pt[3], T result[3]) { DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + using std::fpclassify; + using std::hypot; - const T theta2 = DotProduct(angle_axis, angle_axis); - if (theta2 > T(std::numeric_limits<double>::epsilon())) { + const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]); + + if (fpclassify(theta) != FP_ZERO) { // Away from zero, use the rodriguez formula // // result = pt costheta + @@ -804,7 +809,6 @@ // norm of the angle_axis vector is greater than zero. Otherwise // we get a division by zero. // - const T theta = sqrt(theta2); const T costheta = cos(theta); const T sintheta = sin(theta); const T theta_inverse = T(1.0) / theta; @@ -825,7 +829,7 @@ result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; } else { - // Near zero, the first order Taylor approximation of the rotation + // At zero, the first order Taylor approximation of the rotation // matrix R corresponding to a vector w and angle theta is // // R = I + hat(w) * sin(theta) @@ -837,7 +841,7 @@ // and actually performing multiplication with the point pt, gives us // R * pt = pt + angle_axis x pt. // - // Switching to the Taylor expansion near zero provides meaningful + // Switching to the Taylor expansion at zero provides meaningful // derivatives when evaluated using Jets. // // Explicitly inlined evaluation of the cross product for
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc index f412a79..a2e4c6d 100644 --- a/internal/ceres/manifold.cc +++ b/internal/ceres/manifold.cc
@@ -30,13 +30,11 @@ 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::sqrt( - delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + const double norm_delta = std::hypot(delta[0], delta[1], delta[2]); - if (norm_delta == 0.0) { - for (int i = 0; i < 4; ++i) { - x_plus_delta[i] = x[i]; - } + 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; } @@ -100,19 +98,16 @@ -y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] + y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW]; - const double u_norm = - std::sqrt(ambient_y_minus_x[Order::kX] * ambient_y_minus_x[Order::kX] + - ambient_y_minus_x[Order::kY] * ambient_y_minus_x[Order::kY] + - ambient_y_minus_x[Order::kZ] * ambient_y_minus_x[Order::kZ]); - if (u_norm > 0.0) { + 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 = 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 { - y_minus_x[0] = 0.0; - y_minus_x[1] = 0.0; - y_minus_x[2] = 0.0; + std::fill_n(y_minus_x, 3, 0.0); } }
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index 688edc3..577f8ed 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc
@@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2022 Google Inc. All rights reserved. +// Copyright 2023 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -309,9 +309,7 @@ quaternion[2] = 0.0; quaternion[3] = 0.0; QuaternionToAngleAxis(quaternion, angle_axis); - const double angle = - sqrt(angle_axis[0] * angle_axis[0] + angle_axis[1] * angle_axis[1] + - angle_axis[2] * angle_axis[2]); + const double angle = std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]); EXPECT_LE(angle, kPi); } @@ -1196,14 +1194,13 @@ for (int i = 0; i < 10000; ++i) { double theta = (2.0 * i * 0.0011 - 1.0) * kPi; for (int j = 0; j < 50; ++j) { - double norm2 = 0.0; for (int k = 0; k < 3; ++k) { angle_axis[k] = uniform_distribution(prng); p[k] = uniform_distribution(prng); - norm2 = angle_axis[k] * angle_axis[k]; } - const double inv_norm = theta / sqrt(norm2); + const double inv_norm = + theta / std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]); for (double& angle_axi : angle_axis) { angle_axi *= inv_norm; }