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 | 645 |
1 files changed, 0 insertions, 645 deletions
diff --git a/extern/libmv/third_party/ceres/include/ceres/rotation.h b/extern/libmv/third_party/ceres/include/ceres/rotation.h deleted file mode 100644 index e3dbfe84a5a..00000000000 --- a/extern/libmv/third_party/ceres/include/ceres/rotation.h +++ /dev/null @@ -1,645 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. -// http://code.google.com/p/ceres-solver/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright notice, -// this list of conditions and the following disclaimer in the documentation -// and/or other materials provided with the distribution. -// * Neither the name of Google Inc. nor the names of its contributors may be -// used to endorse or promote products derived from this software without -// specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: keir@google.com (Keir Mierle) -// sameeragarwal@google.com (Sameer Agarwal) -// -// Templated functions for manipulating rotations. The templated -// functions are useful when implementing functors for automatic -// differentiation. -// -// In the following, the Quaternions are laid out as 4-vectors, thus: -// -// q[0] scalar part. -// q[1] coefficient of i. -// q[2] coefficient of j. -// q[3] coefficient of k. -// -// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. - -#ifndef CERES_PUBLIC_ROTATION_H_ -#define CERES_PUBLIC_ROTATION_H_ - -#include <algorithm> -#include <cmath> -#include "glog/logging.h" - -namespace ceres { - -// Trivial wrapper to index linear arrays as matrices, given a fixed -// column and row stride. When an array "T* array" is wrapped by a -// -// (const) MatrixAdapter<T, row_stride, col_stride> M" -// -// the expression M(i, j) is equivalent to -// -// arrary[i * row_stride + j * col_stride] -// -// Conversion functions to and from rotation matrices accept -// MatrixAdapters to permit using row-major and column-major layouts, -// and rotation matrices embedded in larger matrices (such as a 3x4 -// projection matrix). -template <typename T, int row_stride, int col_stride> -struct MatrixAdapter; - -// Convenience functions to create a MatrixAdapter that treats the -// array pointed to by "pointer" as a 3x3 (contiguous) column-major or -// row-major matrix. -template <typename T> -MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer); - -template <typename T> -MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer); - -// Convert a value in combined axis-angle representation to a quaternion. -// The value angle_axis is a triple whose norm is an angle in radians, -// and whose direction is aligned with the axis of rotation, -// and quaternion is a 4-tuple that will contain the resulting quaternion. -// The implementation may be used with auto-differentiation up to the first -// derivative, higher derivatives may have unexpected results near the origin. -template<typename T> -void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); - -// Convert a quaternion to the equivalent combined axis-angle representation. -// The value quaternion must be a unit quaternion - it is not normalized first, -// and angle_axis will be filled with a value whose norm is the angle of -// rotation in radians, and whose direction is the axis of rotation. -// The implemention may be used with auto-differentiation up to the first -// derivative, higher derivatives may have unexpected results near the origin. -template<typename T> -void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); - -// Conversions between 3x3 rotation matrix (in column major order) and -// axis-angle rotation representations. Templated for use with -// autodifferentiation. -template <typename T> -void RotationMatrixToAngleAxis(const T* R, T* angle_axis); - -template <typename T, int row_stride, int col_stride> -void RotationMatrixToAngleAxis( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* angle_axis); - -template <typename T> -void AngleAxisToRotationMatrix(const T* angle_axis, T* R); - -template <typename T, int row_stride, int col_stride> -void AngleAxisToRotationMatrix( - const T* angle_axis, - const MatrixAdapter<T, row_stride, col_stride>& R); - -// Conversions between 3x3 rotation matrix (in row major order) and -// Euler angle (in degrees) rotation representations. -// -// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} -// axes, respectively. They are applied in that same order, so the -// total rotation R is Rz * Ry * Rx. -template <typename T> -void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); - -template <typename T, int row_stride, int col_stride> -void EulerAnglesToRotationMatrix( - const T* euler, - const MatrixAdapter<T, row_stride, col_stride>& R); - -// Convert a 4-vector to a 3x3 scaled rotation matrix. -// -// The choice of rotation is such that the quaternion [1 0 0 0] goes to an -// identity matrix and for small a, b, c the quaternion [1 a b c] goes to -// the matrix -// -// [ 0 -c b ] -// I + 2 [ c 0 -a ] + higher order terms -// [ -b a 0 ] -// -// which corresponds to a Rodrigues approximation, the last matrix being -// the cross-product matrix of [a b c]. Together with the property that -// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. -// -// The rotation matrix is row-major. -// -// No normalization of the quaternion is performed, i.e. -// R = ||q||^2 * Q, where Q is an orthonormal matrix -// such that det(Q) = 1 and Q*Q' = I -template <typename T> inline -void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); - -template <typename T, int row_stride, int col_stride> inline -void QuaternionToScaledRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R); - -// Same as above except that the rotation matrix is normalized by the -// Frobenius norm, so that R * R' = I (and det(R) = 1). -template <typename T> inline -void QuaternionToRotation(const T q[4], T R[3 * 3]); - -template <typename T, int row_stride, int col_stride> inline -void QuaternionToRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R); - -// Rotates a point pt by a quaternion q: -// -// result = R(q) * pt -// -// Assumes the quaternion is unit norm. This assumption allows us to -// write the transform as (something)*pt + pt, as is clear from the -// formula below. If you pass in a quaternion with |q|^2 = 2 then you -// WILL NOT get back 2 times the result you get for a unit quaternion. -template <typename T> inline -void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); - -// With this function you do not need to assume that q has unit norm. -// It does assume that the norm is non-zero. -template <typename T> inline -void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); - -// zw = z * w, where * is the Quaternion product between 4 vectors. -template<typename T> inline -void QuaternionProduct(const T z[4], const T w[4], T zw[4]); - -// xy = x cross y; -template<typename T> inline -void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); - -template<typename T> inline -T DotProduct(const T x[3], const T y[3]); - -// y = R(angle_axis) * x; -template<typename T> inline -void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); - -// --- IMPLEMENTATION - -template<typename T, int row_stride, int col_stride> -struct MatrixAdapter { - T* pointer_; - explicit MatrixAdapter(T* pointer) - : pointer_(pointer) - {} - - T& operator()(int r, int c) const { - return pointer_[r * row_stride + c * col_stride]; - } -}; - -template <typename T> -MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) { - return MatrixAdapter<T, 1, 3>(pointer); -} - -template <typename T> -MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) { - return MatrixAdapter<T, 3, 1>(pointer); -} - -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 theta_squared = a0 * a0 + a1 * a1 + a2 * a2; - - // For points not at the origin, the full conversion is numerically stable. - if (theta_squared > T(0.0)) { - const T theta = sqrt(theta_squared); - const T half_theta = theta * T(0.5); - const T k = sin(half_theta) / theta; - quaternion[0] = cos(half_theta); - quaternion[1] = a0 * k; - quaternion[2] = a1 * k; - quaternion[3] = a2 * k; - } else { - // At the origin, sqrt() will produce NaN in the derivative since - // the argument is zero. By approximating with a Taylor series, - // and truncating at one term, the value and first derivatives will be - // computed correctly when Jets are used. - const T k(0.5); - quaternion[0] = T(1.0); - quaternion[1] = a0 * k; - quaternion[2] = a1 * k; - quaternion[3] = a2 * k; - } -} - -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_theta = q1 * q1 + q2 * q2 + q3 * q3; - - // For quaternions representing non-zero rotation, the conversion - // is numerically stable. - 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; - } else { - // For zero rotation, sqrt() will produce NaN in the derivative since - // the argument is zero. By approximating with a Taylor series, - // and truncating at one term, the value and first derivatives will be - // computed correctly when Jets are used. - const T k(2.0); - angle_axis[0] = q1 * k; - angle_axis[1] = q2 * k; - angle_axis[2] = q3 * k; - } -} - -// The conversion of a rotation matrix to the angle-axis form is -// numerically problematic when then rotation angle is close to zero -// or to Pi. The following implementation detects when these two cases -// occurs and deals with them by taking code paths that are guaranteed -// to not perform division by a small number. -template <typename T> -inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { - RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); -} - -template <typename T, int row_stride, int col_stride> -void RotationMatrixToAngleAxis( - const MatrixAdapter<const T, row_stride, col_stride>& R, - T* angle_axis) { - // x = k * 2 * sin(theta), where k is the axis of rotation. - angle_axis[0] = R(2, 1) - R(1, 2); - angle_axis[1] = R(0, 2) - R(2, 0); - angle_axis[2] = R(1, 0) - R(0, 1); - - static const T kOne = T(1.0); - static const T kTwo = T(2.0); - - // Since the right hand side may give numbers just above 1.0 or - // below -1.0 leading to atan misbehaving, we threshold. - T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo, - T(-1.0)), - kOne); - - // sqrt is guaranteed to give non-negative results, so we only - // threshold above. - T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + - angle_axis[1] * angle_axis[1] + - angle_axis[2] * angle_axis[2]) / kTwo, - kOne); - - // Use the arctan2 to get the right sign on theta - const T theta = atan2(sintheta, costheta); - - // Case 1: sin(theta) is large enough, so dividing by it is not a - // problem. We do not use abs here, because while jets.h imports - // std::abs into the namespace, here in this file, abs resolves to - // the int version of the function, which returns zero always. - // - // We use a threshold much larger then the machine epsilon, because - // if sin(theta) is small, not only do we risk overflow but even if - // that does not occur, just dividing by a small number will result - // in numerical garbage. So we play it safe. - static const double kThreshold = 1e-12; - if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { - const T r = theta / (kTwo * sintheta); - for (int i = 0; i < 3; ++i) { - angle_axis[i] *= r; - } - return; - } - - // Case 2: theta ~ 0, means sin(theta) ~ theta to a good - // approximation. - if (costheta > 0.0) { - const T kHalf = T(0.5); - for (int i = 0; i < 3; ++i) { - angle_axis[i] *= kHalf; - } - return; - } - - // Case 3: theta ~ pi, this is the hard case. Since theta is large, - // and sin(theta) is small. Dividing by theta by sin(theta) will - // either give an overflow or worse still numerically meaningless - // results. Thus we use an alternate more complicated formula - // here. - - // Since cos(theta) is negative, division by (1-cos(theta)) cannot - // overflow. - const T inv_one_minus_costheta = kOne / (kOne - costheta); - - // We now compute the absolute value of coordinates of the axis - // vector using the diagonal entries of R. To resolve the sign of - // these entries, we compare the sign of angle_axis[i]*sin(theta) - // with the sign of sin(theta). If they are the same, then - // angle_axis[i] should be positive, otherwise negative. - for (int i = 0; i < 3; ++i) { - angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta); - if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || - ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { - angle_axis[i] = -angle_axis[i]; - } - } -} - -template <typename T> -inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { - AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); -} - -template <typename T, int row_stride, int col_stride> -void AngleAxisToRotationMatrix( - const T* angle_axis, - const MatrixAdapter<T, row_stride, col_stride>& R) { - static const T kOne = T(1.0); - const T theta2 = DotProduct(angle_axis, angle_axis); - if (theta2 > T(std::numeric_limits<double>::epsilon())) { - // We want to be careful to only evaluate the square root if the - // norm of the angle_axis vector is greater than zero. Otherwise - // we get a division by zero. - const T theta = sqrt(theta2); - const T wx = angle_axis[0] / theta; - const T wy = angle_axis[1] / theta; - const T wz = angle_axis[2] / theta; - - const T costheta = cos(theta); - const T sintheta = sin(theta); - - R(0, 0) = costheta + wx*wx*(kOne - costheta); - R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); - R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); - R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; - R(1, 1) = costheta + wy*wy*(kOne - costheta); - R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); - R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); - R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); - R(2, 2) = costheta + wz*wz*(kOne - costheta); - } else { - // Near zero, we switch to using the first order Taylor expansion. - R(0, 0) = kOne; - R(1, 0) = angle_axis[2]; - R(2, 0) = -angle_axis[1]; - R(0, 1) = -angle_axis[2]; - R(1, 1) = kOne; - R(2, 1) = angle_axis[0]; - R(0, 2) = angle_axis[1]; - R(1, 2) = -angle_axis[0]; - R(2, 2) = kOne; - } -} - -template <typename T> -inline void EulerAnglesToRotationMatrix(const T* euler, - const int row_stride_parameter, - T* R) { - CHECK_EQ(row_stride_parameter, 3); - EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); -} - -template <typename T, int row_stride, int col_stride> -void EulerAnglesToRotationMatrix( - const T* euler, - const MatrixAdapter<T, row_stride, col_stride>& R) { - 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); - const T yaw(euler[2] * degrees_to_radians); - - const T c1 = cos(yaw); - const T s1 = sin(yaw); - const T c2 = cos(roll); - const T s2 = sin(roll); - const T c3 = cos(pitch); - const T s3 = sin(pitch); - - R(0, 0) = c1*c2; - R(0, 1) = -s1*c3 + c1*s2*s3; - R(0, 2) = s1*s3 + c1*s2*c3; - - R(1, 0) = s1*c2; - R(1, 1) = c1*c3 + s1*s2*s3; - R(1, 2) = -c1*s3 + s1*s2*c3; - - R(2, 0) = -s2; - R(2, 1) = c2*s3; - R(2, 2) = c2*c3; -} - -template <typename T> inline -void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { - QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); -} - -template <typename T, int row_stride, int col_stride> inline -void QuaternionToScaledRotation( - const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R) { - // Make convenient names for elements of q. - T a = q[0]; - T b = q[1]; - T c = q[2]; - T d = q[3]; - // This is not to eliminate common sub-expression, but to - // make the lines shorter so that they fit in 80 columns! - T aa = a * a; - T ab = a * b; - T ac = a * c; - T ad = a * d; - T bb = b * b; - T bc = b * c; - T bd = b * d; - T cc = c * c; - T cd = c * d; - T dd = d * d; - - R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT - R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT - R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT -} - -template <typename T> inline -void QuaternionToRotation(const T q[4], T R[3 * 3]) { - QuaternionToRotation(q, RowMajorAdapter3x3(R)); -} - -template <typename T, int row_stride, int col_stride> inline -void QuaternionToRotation(const T q[4], - const MatrixAdapter<T, row_stride, col_stride>& R) { - QuaternionToScaledRotation(q, R); - - T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; - CHECK_NE(normalizer, T(0)); - normalizer = T(1) / normalizer; - - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - R(i, j) *= normalizer; - } - } -} - -template <typename T> inline -void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { - const T t2 = q[0] * q[1]; - const T t3 = q[0] * q[2]; - const T t4 = q[0] * q[3]; - const T t5 = -q[1] * q[1]; - const T t6 = q[1] * q[2]; - const T t7 = q[1] * q[3]; - const T t8 = -q[2] * q[2]; - const T t9 = q[2] * q[3]; - const T t1 = -q[3] * q[3]; - result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT - result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT - result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT -} - -template <typename T> inline -void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { - // 'scale' is 1 / norm(q). - 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] = { - scale * q[0], - scale * q[1], - scale * q[2], - scale * q[3], - }; - - UnitQuaternionRotatePoint(unit, pt, result); -} - -template<typename T> inline -void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { - zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; - zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; - zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; - zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; -} - -// xy = x cross y; -template<typename T> inline -void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { - x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; - x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; - x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; -} - -template<typename T> inline -T DotProduct(const T x[3], const T y[3]) { - return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); -} - -template<typename T> inline -void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { - const T theta2 = DotProduct(angle_axis, angle_axis); - if (theta2 > T(std::numeric_limits<double>::epsilon())) { - // Away from zero, use the rodriguez formula - // - // result = pt costheta + - // (w x pt) * sintheta + - // w (w . pt) (1 - costheta) - // - // We want to be careful to only evaluate the square root if the - // norm of the angle_axis vector is greater than zero. Otherwise - // we get a division by zero. - // - const T theta = sqrt(theta2); - const T costheta = cos(theta); - const T sintheta = sin(theta); - const T theta_inverse = 1.0 / theta; - - const T w[3] = { angle_axis[0] * theta_inverse, - angle_axis[1] * theta_inverse, - angle_axis[2] * theta_inverse }; - - // Explicitly inlined evaluation of the cross product for - // performance reasons. - const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], - w[2] * pt[0] - w[0] * pt[2], - w[0] * pt[1] - w[1] * pt[0] }; - const T tmp = - (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); - - result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; - result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; - result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; - } else { - // Near zero, the first order Taylor approximation of the rotation - // matrix R corresponding to a vector w and angle w is - // - // R = I + hat(w) * sin(theta) - // - // But sintheta ~ theta and theta * w = angle_axis, which gives us - // - // R = I + hat(w) - // - // and actually performing multiplication with the point pt, gives us - // R * pt = pt + w x pt. - // - // Switching to the Taylor expansion near zero provides meaningful - // derivatives when evaluated using Jets. - // - // Explicitly inlined evaluation of the cross product for - // performance reasons. - const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], - angle_axis[2] * pt[0] - angle_axis[0] * pt[2], - angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; - - result[0] = pt[0] + w_cross_pt[0]; - result[1] = pt[1] + w_cross_pt[1]; - result[2] = pt[2] + w_cross_pt[2]; - } -} - -} // namespace ceres - -#endif // CERES_PUBLIC_ROTATION_H_ |