diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h index 1220173dd..ab1e69e6a 100644 --- a/include/ceres/rotation.h +++ b/include/ceres/rotation.h @@ -743,10 +743,10 @@ inline void UnitQuaternionRotatePoint(const T q[4], template 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] = { diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index b59c48737..28a4271c4 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc @@ -30,6 +30,7 @@ #include "ceres/rotation.h" +#include #include #include #include @@ -969,8 +970,8 @@ void ExpectJetArraysClose(const Jet* x, const Jet* y) { } // Log-10 of a value well below machine precision. -static const int kSmallTinyCutoff = - static_cast(2 * log(std::numeric_limits::epsilon()) / log(10.0)); +static const int kSmallTinyCutoff = static_cast( + 2 * log(std::numeric_limits::epsilon()) / log(10.0)); // Log-10 of a value just below values representable by double. static const int kTinyZeroLimit = @@ -1221,6 +1222,23 @@ TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { } } +TEST(Quaternion, UnitQuaternion) { + using Jet = ceres::Jet; + std::array quaternion = { + Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)}; + std::array point = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Eigen::Vector3 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 uniform_distribution{-1.0, 1.0};