Fix a bug in QuaternionRotatePoint.

In https://ceres-solver-review.git.corp.google.com/c/ceres-solver/+/23802

the computation of the norm of a quaternion

scale = 1/sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

was replaced by

scale = 1/hypot(q[0], q[1], hypot(q[2], q[3]));

while this appear to be a more accurate computation because of the
use of hypot which can handle over and underflow it introduces a
bug for the case where q[2] = q[3] = 0.

While the hypot(q[2], q[3]) == 0 as scalars, if q[2] and q[3] are
jets, then the derivative will be NaN. Which means that even though
q[0] or q[1] is non-zero and the norm of the quaternion is non-zero,
and the resulting derivative is finite, this way of computing the
scale will produce nans in the derivative of scale.

The following quaternion will replicate the problem described above.

using Jet = ceres::Jet<double, 4>;
std::array<Jet, 4> quaternion = {Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)};

This CL reverts the change to QuaternionRotatePoint and
adds a test for it.

Thanks to Jonathan Taylor for reproducing this bug.

Change-Id: I0fbbcc77d6945a38563d82efba4429f4b5278cd5

include/ceres/rotation.h

diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index 1220173..ab1e69e 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -743,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) / hypot(q[0], q[1], hypot(q[2], q[3]));
+  const T scale =
+      T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
 
   // Make unit-norm version of q.
   const T unit[4] = {

internal/ceres/rotation_test.cc

diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index b59c487..28a4271 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -30,6 +30,7 @@
 
 #include "ceres/rotation.h"
 
+#include <array>
 #include <cmath>
 #include <limits>
 #include <random>
@@ -969,8 +970,8 @@
 }
 
 // Log-10 of a value well below machine precision.
-static const int kSmallTinyCutoff =
-    static_cast<int>(2 * log(std::numeric_limits<double>::epsilon()) / log(10.0));
+static const int kSmallTinyCutoff = static_cast<int>(
+    2 * log(std::numeric_limits<double>::epsilon()) / log(10.0));
 
 // Log-10 of a value just below values representable by double.
 static const int kTinyZeroLimit =
@@ -1221,6 +1222,23 @@
   }
 }
 
+TEST(Quaternion, UnitQuaternion) {
+  using Jet = ceres::Jet<double, 4>;
+  std::array<Jet, 4> quaternion = {
+      Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)};
+  std::array<Jet, 3> point = {Jet(0.0), Jet(0.0), Jet(0.0)};
+  Eigen::Vector3<Jet> rotated_point;
+  QuaternionRotatePoint(quaternion.data(), point.data(), rotated_point.data());
+  LOG(INFO) << rotated_point[0];
+  LOG(INFO) << rotated_point[1];
+  LOG(INFO) << rotated_point[2];
+
+  for (int i = 0; i < 3; ++i) {
+    EXPECT_EQ(rotated_point[i], point[i]);
+    EXPECT_FALSE(rotated_point[i].v.array().isNaN().any());
+  }
+}
+
 TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) {
   std::mt19937 prng;
   std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};