diff options
Diffstat (limited to 'extern/libmv/third_party/ceres/include/ceres/rotation.h')
-rw-r--r-- | extern/libmv/third_party/ceres/include/ceres/rotation.h | 62 |
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_ |