1 files changed, 35 insertions, 27 deletions

diff --git a/extern/libmv/third_party/ceres/include/ceres/rotation.h b/extern/libmv/third_party/ceres/include/ceres/rotation.h
index e4227e78b9a..0d8a390d5d1 100644
--- a/extern/libmv/third_party/ceres/include/ceres/rotation.h
+++ b/extern/libmv/third_party/ceres/include/ceres/rotation.h
@@ -47,6 +47,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include "glog/logging.h"
 
 namespace ceres {
 
@@ -145,18 +146,11 @@ void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
 
 // --- IMPLEMENTATION
 
-// Duplicate rather than decorate every use of cmath with _USE_MATH_CONSTANTS.
-// Necessitated by Windows.
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#define CERES_NEED_M_PI_UNDEF
-#endif
-
 template<typename T>
 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
-  const T &a0 = angle_axis[0];
-  const T &a1 = angle_axis[1];
-  const T &a2 = angle_axis[2];
+  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;
 
   // For points not at the origin, the full conversion is numerically stable.
@@ -183,16 +177,35 @@ inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
 
 template<typename T>
 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
-  const T &q1 = quaternion[1];
-  const T &q2 = quaternion[2];
-  const T &q3 = quaternion[3];
-  const T sin_squared = q1 * q1 + q2 * q2 + q3 * q3;
+  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;
 
   // For quaternions representing non-zero rotation, the conversion
   // is numerically stable.
-  if (sin_squared > T(0.0)) {
-    const T sin_theta = sqrt(sin_squared);
-    const T k = T(2.0) * atan2(sin_theta, quaternion[0]) / sin_theta;
+  if (sin_squared_theta > T(0.0)) {
+    const T sin_theta = sqrt(sin_squared_theta);
+    const T& cos_theta = quaternion[0];
+
+    // If cos_theta is negative, theta is greater than pi/2, which
+    // means that angle for the angle_axis vector which is 2 * theta
+    // would be greater than pi.
+    //
+    // While this will result in the correct rotation, it does not
+    // result in a normalized angle-axis vector.
+    //
+    // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
+    // which is equivalent saying
+    //
+    //   theta - pi = atan(sin(theta - pi), cos(theta - pi))
+    //              = atan(-sin(theta), -cos(theta))
+    //
+    const T two_theta =
+        T(2.0) * ((cos_theta < 0.0)
+                  ? atan2(-sin_theta, -cos_theta)
+                  : atan2(sin_theta, cos_theta));
+    const T k = two_theta / sin_theta;
     angle_axis[0] = q1 * k;
     angle_axis[1] = q2 * k;
     angle_axis[2] = q3 * k;
@@ -259,7 +272,7 @@ inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) {
 
   // Case 2: theta ~ 0, means sin(theta) ~ theta to a good
   // approximation.
-  if (costheta > 0) {
+  if (costheta > 0.0) {
     const T kHalf = T(0.5);
     for (int i = 0; i < 3; ++i) {
       angle_axis[i] *= kHalf;
@@ -284,8 +297,8 @@ inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) {
   // angle_axis[i] should be positive, otherwise negative.
   for (int i = 0; i < 3; ++i) {
     angle_axis[i] = theta * sqrt((R[i*4] - costheta) * inv_one_minus_costheta);
-    if (((sintheta < 0) && (angle_axis[i] > 0)) ||
-        ((sintheta > 0) && (angle_axis[i] < 0))) {
+    if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
+        ((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
       angle_axis[i] = -angle_axis[i];
     }
   }
@@ -334,7 +347,8 @@ template <typename T>
 inline void EulerAnglesToRotationMatrix(const T* euler,
                                         const int row_stride,
                                         T* R) {
-  const T degrees_to_radians(M_PI / 180.0);
+  const double kPi = 3.14159265358979323846;
+  const T degrees_to_radians(kPi / 180.0);
 
   const T pitch(euler[0] * degrees_to_radians);
   const T roll(euler[1] * degrees_to_radians);
@@ -517,10 +531,4 @@ void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
 
 }  // namespace ceres
 
-// Clean define pollution.
-#ifdef CERES_NEED_M_PI_UNDEF
-#undef CERES_NEED_M_PI_UNDEF
-#undef M_PI
-#endif
-
 #endif  // CERES_PUBLIC_ROTATION_H_