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;
}