Do not differentiate the norm at gimbal lock At gimbal lock, the norm determining the middle Euler angle is exactly zero. Its kink carries over through atan2, so the angle has no derivative. Differentiating the norm anyway produces NaN derivatives. Compute the norm only away from gimbal lock and use its zero subgradient otherwise. Likewise, compute the norms in the quaternion manifold test functor only if they are nonzero. Change-Id: Ic5c4eaaf1b268b7f002c1489296b212fd45f7727
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index 7062ec3..6571a68 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.h
@@ -634,25 +634,31 @@ T ea[3]; if constexpr (EulerSystem::kIsProperEuler) { - const T sy = hypot(R(i, j), R(i, k)); - if (fpclassify(sy) != FP_ZERO) { + if (fpclassify(R(i, j)) != FP_ZERO || fpclassify(R(i, k)) != FP_ZERO) { + const T sy = hypot(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)); } else { + // At gimbal lock, the norm is zero and has no derivative, and atan2 + // carries its kink over to the angle. Use the zero subgradient instead of + // differentiating the norm. ea[0] = atan2(-R(j, k), R(j, j)); - ea[1] = atan2(sy, R(i, i)); + ea[1] = atan2(T(0.0), R(i, i)); ea[2] = T(0.0); } } else { - const T cy = hypot(R(i, i), R(j, i)); - if (fpclassify(cy) != FP_ZERO) { + if (fpclassify(R(i, i)) != FP_ZERO || fpclassify(R(j, i)) != FP_ZERO) { + const T cy = hypot(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)); } else { + // At gimbal lock, the norm is zero and has no derivative, and atan2 + // carries its kink over to the angle. Use the zero subgradient instead of + // differentiating the norm. ea[0] = atan2(-R(j, k), R(j, j)); - ea[1] = atan2(-R(k, i), cy); + ea[1] = atan2(-R(k, i), T(0.0)); ea[2] = T(0.0); } }
diff --git a/internal/ceres/autodiff_manifold_test.cc b/internal/ceres/autodiff_manifold_test.cc index c7a473d..8c7de26 100644 --- a/internal/ceres/autodiff_manifold_test.cc +++ b/internal/ceres/autodiff_manifold_test.cc
@@ -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 @@ -143,8 +143,9 @@ template <typename T> bool Plus(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; - T norm_delta = hypot(delta[0], delta[1], delta[2]); - if (fpclassify(norm_delta) != FP_ZERO) { + 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]; @@ -170,9 +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 = - hypot(ambient_y_minus_x[1], ambient_y_minus_x[2], ambient_y_minus_x[3]); - if (fpclassify(u_norm) != FP_ZERO) { + 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/rotation_test.cc b/internal/ceres/rotation_test.cc index d7093bc..3a2c117 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc
@@ -1455,6 +1455,50 @@ }; // clang-format on +// At gimbal lock, the norm determining the middle angle is exactly zero and +// has no derivative. The middle angle must use the zero subgradient instead +// of propagating a NaN into the derivatives of the Euler angles. +TEST(EulerAngles, RotationMatrixToEulerAnglesAtGimbalLockForJets) { + using J = Jet<double, 9>; + constexpr int kNumEntries = 9; + // The identity locks all proper Euler sequences. + constexpr double kIdentity[kNumEntries] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + // A rotation about the Y axis by 90 degrees locks the ZYX sequence. + constexpr double kQuarterTurnAboutY[kNumEntries] = { + 0, 0, 1, 0, 1, 0, -1, 0, 0}; + + // Assigns every entry its own derivative direction. + const auto make_matrix = [](const double (&entries)[kNumEntries], + J(&matrix)[kNumEntries]) { + for (int entry = 0; entry < kNumEntries; ++entry) { + matrix[entry] = J(entries[entry], entry); + } + }; + + J matrix[kNumEntries]; + J angles[3]; + + // At gimbal lock with the axes (i, j, k), the first angle of the sequence is + // atan2(-R(j, k), R(j, j)) whose only nonzero derivative is -1 with respect + // to R(j, k). Intrinsic sequences report this angle last. The other angles + // are constant. + make_matrix(kIdentity, matrix); + RotationMatrixToEulerAngles<IntrinsicZXZ>(matrix, angles); + // The axes of the ZXZ sequence are (2, 0, 1), i.e., R(j, k) = R(0, 1). + J expected_identity[3] = {J(0.0), J(0.0), J(0.0)}; + expected_identity[2].v[1] = -1.0; + EXPECT_THAT(angles, + testing::Pointwise(JetClose(kTolerance), expected_identity)); + + make_matrix(kQuarterTurnAboutY, matrix); + RotationMatrixToEulerAngles<IntrinsicZYX>(matrix, angles); + // The axes of the ZYX sequence are (0, 1, 2), i.e., R(j, k) = R(1, 2). + J expected_quarter_turn[3] = {J(0.0), J(kPi / 2), J(0.0)}; + expected_quarter_turn[2].v[5] = -1.0; + EXPECT_THAT(angles, + testing::Pointwise(JetClose(kTolerance), expected_quarter_turn)); +} + // 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