Home | History | Annotate | Download | only in ceres
      1 // Ceres Solver - A fast non-linear least squares minimizer
      2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
      3 // http://code.google.com/p/ceres-solver/
      4 //
      5 // Redistribution and use in source and binary forms, with or without
      6 // modification, are permitted provided that the following conditions are met:
      7 //
      8 // * Redistributions of source code must retain the above copyright notice,
      9 //   this list of conditions and the following disclaimer.
     10 // * Redistributions in binary form must reproduce the above copyright notice,
     11 //   this list of conditions and the following disclaimer in the documentation
     12 //   and/or other materials provided with the distribution.
     13 // * Neither the name of Google Inc. nor the names of its contributors may be
     14 //   used to endorse or promote products derived from this software without
     15 //   specific prior written permission.
     16 //
     17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
     18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
     19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
     20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
     21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     27 // POSSIBILITY OF SUCH DAMAGE.
     28 //
     29 // Author: keir (at) google.com (Keir Mierle)
     30 //         sameeragarwal (at) google.com (Sameer Agarwal)
     31 //
     32 // Templated functions for manipulating rotations. The templated
     33 // functions are useful when implementing functors for automatic
     34 // differentiation.
     35 //
     36 // In the following, the Quaternions are laid out as 4-vectors, thus:
     37 //
     38 //   q[0]  scalar part.
     39 //   q[1]  coefficient of i.
     40 //   q[2]  coefficient of j.
     41 //   q[3]  coefficient of k.
     42 //
     43 // where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
     44 
     45 #ifndef CERES_PUBLIC_ROTATION_H_
     46 #define CERES_PUBLIC_ROTATION_H_
     47 
     48 #include <algorithm>
     49 #include <cmath>
     50 #include "glog/logging.h"
     51 
     52 namespace ceres {
     53 
     54 // Trivial wrapper to index linear arrays as matrices, given a fixed
     55 // column and row stride. When an array "T* array" is wrapped by a
     56 //
     57 //   (const) MatrixAdapter<T, row_stride, col_stride> M"
     58 //
     59 // the expression  M(i, j) is equivalent to
     60 //
     61 //   arrary[i * row_stride + j * col_stride]
     62 //
     63 // Conversion functions to and from rotation matrices accept
     64 // MatrixAdapters to permit using row-major and column-major layouts,
     65 // and rotation matrices embedded in larger matrices (such as a 3x4
     66 // projection matrix).
     67 template <typename T, int row_stride, int col_stride>
     68 struct MatrixAdapter;
     69 
     70 // Convenience functions to create a MatrixAdapter that treats the
     71 // array pointed to by "pointer" as a 3x3 (contiguous) column-major or
     72 // row-major matrix.
     73 template <typename T>
     74 MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
     75 
     76 template <typename T>
     77 MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
     78 
     79 // Convert a value in combined axis-angle representation to a quaternion.
     80 // The value angle_axis is a triple whose norm is an angle in radians,
     81 // and whose direction is aligned with the axis of rotation,
     82 // and quaternion is a 4-tuple that will contain the resulting quaternion.
     83 // The implementation may be used with auto-differentiation up to the first
     84 // derivative, higher derivatives may have unexpected results near the origin.
     85 template<typename T>
     86 void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
     87 
     88 // Convert a quaternion to the equivalent combined axis-angle representation.
     89 // The value quaternion must be a unit quaternion - it is not normalized first,
     90 // and angle_axis will be filled with a value whose norm is the angle of
     91 // rotation in radians, and whose direction is the axis of rotation.
     92 // The implemention may be used with auto-differentiation up to the first
     93 // derivative, higher derivatives may have unexpected results near the origin.
     94 template<typename T>
     95 void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
     96 
     97 // Conversions between 3x3 rotation matrix (in column major order) and
     98 // axis-angle rotation representations.  Templated for use with
     99 // autodifferentiation.
    100 template <typename T>
    101 void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
    102 
    103 template <typename T, int row_stride, int col_stride>
    104 void RotationMatrixToAngleAxis(
    105     const MatrixAdapter<const T, row_stride, col_stride>& R,
    106     T* angle_axis);
    107 
    108 template <typename T>
    109 void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
    110 
    111 template <typename T, int row_stride, int col_stride>
    112 void AngleAxisToRotationMatrix(
    113     const T* angle_axis,
    114     const MatrixAdapter<T, row_stride, col_stride>& R);
    115 
    116 // Conversions between 3x3 rotation matrix (in row major order) and
    117 // Euler angle (in degrees) rotation representations.
    118 //
    119 // The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
    120 // axes, respectively.  They are applied in that same order, so the
    121 // total rotation R is Rz * Ry * Rx.
    122 template <typename T>
    123 void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
    124 
    125 template <typename T, int row_stride, int col_stride>
    126 void EulerAnglesToRotationMatrix(
    127     const T* euler,
    128     const MatrixAdapter<T, row_stride, col_stride>& R);
    129 
    130 // Convert a 4-vector to a 3x3 scaled rotation matrix.
    131 //
    132 // The choice of rotation is such that the quaternion [1 0 0 0] goes to an
    133 // identity matrix and for small a, b, c the quaternion [1 a b c] goes to
    134 // the matrix
    135 //
    136 //         [  0 -c  b ]
    137 //   I + 2 [  c  0 -a ] + higher order terms
    138 //         [ -b  a  0 ]
    139 //
    140 // which corresponds to a Rodrigues approximation, the last matrix being
    141 // the cross-product matrix of [a b c]. Together with the property that
    142 // R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
    143 //
    144 // The rotation matrix is row-major.
    145 //
    146 // No normalization of the quaternion is performed, i.e.
    147 // R = ||q||^2 * Q, where Q is an orthonormal matrix
    148 // such that det(Q) = 1 and Q*Q' = I
    149 template <typename T> inline
    150 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
    151 
    152 template <typename T, int row_stride, int col_stride> inline
    153 void QuaternionToScaledRotation(
    154     const T q[4],
    155     const MatrixAdapter<T, row_stride, col_stride>& R);
    156 
    157 // Same as above except that the rotation matrix is normalized by the
    158 // Frobenius norm, so that R * R' = I (and det(R) = 1).
    159 template <typename T> inline
    160 void QuaternionToRotation(const T q[4], T R[3 * 3]);
    161 
    162 template <typename T, int row_stride, int col_stride> inline
    163 void QuaternionToRotation(
    164     const T q[4],
    165     const MatrixAdapter<T, row_stride, col_stride>& R);
    166 
    167 // Rotates a point pt by a quaternion q:
    168 //
    169 //   result = R(q) * pt
    170 //
    171 // Assumes the quaternion is unit norm. This assumption allows us to
    172 // write the transform as (something)*pt + pt, as is clear from the
    173 // formula below. If you pass in a quaternion with |q|^2 = 2 then you
    174 // WILL NOT get back 2 times the result you get for a unit quaternion.
    175 template <typename T> inline
    176 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
    177 
    178 // With this function you do not need to assume that q has unit norm.
    179 // It does assume that the norm is non-zero.
    180 template <typename T> inline
    181 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
    182 
    183 // zw = z * w, where * is the Quaternion product between 4 vectors.
    184 template<typename T> inline
    185 void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
    186 
    187 // xy = x cross y;
    188 template<typename T> inline
    189 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
    190 
    191 template<typename T> inline
    192 T DotProduct(const T x[3], const T y[3]);
    193 
    194 // y = R(angle_axis) * x;
    195 template<typename T> inline
    196 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
    197 
    198 // --- IMPLEMENTATION
    199 
    200 template<typename T, int row_stride, int col_stride>
    201 struct MatrixAdapter {
    202   T* pointer_;
    203   explicit MatrixAdapter(T* pointer)
    204     : pointer_(pointer)
    205   {}
    206 
    207   T& operator()(int r, int c) const {
    208     return pointer_[r * row_stride + c * col_stride];
    209   }
    210 };
    211 
    212 template <typename T>
    213 MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
    214   return MatrixAdapter<T, 1, 3>(pointer);
    215 }
    216 
    217 template <typename T>
    218 MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
    219   return MatrixAdapter<T, 3, 1>(pointer);
    220 }
    221 
    222 template<typename T>
    223 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
    224   const T& a0 = angle_axis[0];
    225   const T& a1 = angle_axis[1];
    226   const T& a2 = angle_axis[2];
    227   const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
    228 
    229   // For points not at the origin, the full conversion is numerically stable.
    230   if (theta_squared > T(0.0)) {
    231     const T theta = sqrt(theta_squared);
    232     const T half_theta = theta * T(0.5);
    233     const T k = sin(half_theta) / theta;
    234     quaternion[0] = cos(half_theta);
    235     quaternion[1] = a0 * k;
    236     quaternion[2] = a1 * k;
    237     quaternion[3] = a2 * k;
    238   } else {
    239     // At the origin, sqrt() will produce NaN in the derivative since
    240     // the argument is zero.  By approximating with a Taylor series,
    241     // and truncating at one term, the value and first derivatives will be
    242     // computed correctly when Jets are used.
    243     const T k(0.5);
    244     quaternion[0] = T(1.0);
    245     quaternion[1] = a0 * k;
    246     quaternion[2] = a1 * k;
    247     quaternion[3] = a2 * k;
    248   }
    249 }
    250 
    251 template<typename T>
    252 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
    253   const T& q1 = quaternion[1];
    254   const T& q2 = quaternion[2];
    255   const T& q3 = quaternion[3];
    256   const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
    257 
    258   // For quaternions representing non-zero rotation, the conversion
    259   // is numerically stable.
    260   if (sin_squared_theta > T(0.0)) {
    261     const T sin_theta = sqrt(sin_squared_theta);
    262     const T& cos_theta = quaternion[0];
    263 
    264     // If cos_theta is negative, theta is greater than pi/2, which
    265     // means that angle for the angle_axis vector which is 2 * theta
    266     // would be greater than pi.
    267     //
    268     // While this will result in the correct rotation, it does not
    269     // result in a normalized angle-axis vector.
    270     //
    271     // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
    272     // which is equivalent saying
    273     //
    274     //   theta - pi = atan(sin(theta - pi), cos(theta - pi))
    275     //              = atan(-sin(theta), -cos(theta))
    276     //
    277     const T two_theta =
    278         T(2.0) * ((cos_theta < 0.0)
    279                   ? atan2(-sin_theta, -cos_theta)
    280                   : atan2(sin_theta, cos_theta));
    281     const T k = two_theta / sin_theta;
    282     angle_axis[0] = q1 * k;
    283     angle_axis[1] = q2 * k;
    284     angle_axis[2] = q3 * k;
    285   } else {
    286     // For zero rotation, sqrt() will produce NaN in the derivative since
    287     // the argument is zero.  By approximating with a Taylor series,
    288     // and truncating at one term, the value and first derivatives will be
    289     // computed correctly when Jets are used.
    290     const T k(2.0);
    291     angle_axis[0] = q1 * k;
    292     angle_axis[1] = q2 * k;
    293     angle_axis[2] = q3 * k;
    294   }
    295 }
    296 
    297 // The conversion of a rotation matrix to the angle-axis form is
    298 // numerically problematic when then rotation angle is close to zero
    299 // or to Pi. The following implementation detects when these two cases
    300 // occurs and deals with them by taking code paths that are guaranteed
    301 // to not perform division by a small number.
    302 template <typename T>
    303 inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
    304   RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
    305 }
    306 
    307 template <typename T, int row_stride, int col_stride>
    308 void RotationMatrixToAngleAxis(
    309     const MatrixAdapter<const T, row_stride, col_stride>& R,
    310     T* angle_axis) {
    311   // x = k * 2 * sin(theta), where k is the axis of rotation.
    312   angle_axis[0] = R(2, 1) - R(1, 2);
    313   angle_axis[1] = R(0, 2) - R(2, 0);
    314   angle_axis[2] = R(1, 0) - R(0, 1);
    315 
    316   static const T kOne = T(1.0);
    317   static const T kTwo = T(2.0);
    318 
    319   // Since the right hand side may give numbers just above 1.0 or
    320   // below -1.0 leading to atan misbehaving, we threshold.
    321   T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
    322                                  T(-1.0)),
    323                         kOne);
    324 
    325   // sqrt is guaranteed to give non-negative results, so we only
    326   // threshold above.
    327   T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
    328                              angle_axis[1] * angle_axis[1] +
    329                              angle_axis[2] * angle_axis[2]) / kTwo,
    330                         kOne);
    331 
    332   // Use the arctan2 to get the right sign on theta
    333   const T theta = atan2(sintheta, costheta);
    334 
    335   // Case 1: sin(theta) is large enough, so dividing by it is not a
    336   // problem. We do not use abs here, because while jets.h imports
    337   // std::abs into the namespace, here in this file, abs resolves to
    338   // the int version of the function, which returns zero always.
    339   //
    340   // We use a threshold much larger then the machine epsilon, because
    341   // if sin(theta) is small, not only do we risk overflow but even if
    342   // that does not occur, just dividing by a small number will result
    343   // in numerical garbage. So we play it safe.
    344   static const double kThreshold = 1e-12;
    345   if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
    346     const T r = theta / (kTwo * sintheta);
    347     for (int i = 0; i < 3; ++i) {
    348       angle_axis[i] *= r;
    349     }
    350     return;
    351   }
    352 
    353   // Case 2: theta ~ 0, means sin(theta) ~ theta to a good
    354   // approximation.
    355   if (costheta > 0.0) {
    356     const T kHalf = T(0.5);
    357     for (int i = 0; i < 3; ++i) {
    358       angle_axis[i] *= kHalf;
    359     }
    360     return;
    361   }
    362 
    363   // Case 3: theta ~ pi, this is the hard case. Since theta is large,
    364   // and sin(theta) is small. Dividing by theta by sin(theta) will
    365   // either give an overflow or worse still numerically meaningless
    366   // results. Thus we use an alternate more complicated formula
    367   // here.
    368 
    369   // Since cos(theta) is negative, division by (1-cos(theta)) cannot
    370   // overflow.
    371   const T inv_one_minus_costheta = kOne / (kOne - costheta);
    372 
    373   // We now compute the absolute value of coordinates of the axis
    374   // vector using the diagonal entries of R. To resolve the sign of
    375   // these entries, we compare the sign of angle_axis[i]*sin(theta)
    376   // with the sign of sin(theta). If they are the same, then
    377   // angle_axis[i] should be positive, otherwise negative.
    378   for (int i = 0; i < 3; ++i) {
    379     angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
    380     if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
    381         ((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
    382       angle_axis[i] = -angle_axis[i];
    383     }
    384   }
    385 }
    386 
    387 template <typename T>
    388 inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
    389   AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
    390 }
    391 
    392 template <typename T, int row_stride, int col_stride>
    393 void AngleAxisToRotationMatrix(
    394     const T* angle_axis,
    395     const MatrixAdapter<T, row_stride, col_stride>& R) {
    396   static const T kOne = T(1.0);
    397   const T theta2 = DotProduct(angle_axis, angle_axis);
    398   if (theta2 > 0.0) {
    399     // We want to be careful to only evaluate the square root if the
    400     // norm of the angle_axis vector is greater than zero. Otherwise
    401     // we get a division by zero.
    402     const T theta = sqrt(theta2);
    403     const T wx = angle_axis[0] / theta;
    404     const T wy = angle_axis[1] / theta;
    405     const T wz = angle_axis[2] / theta;
    406 
    407     const T costheta = cos(theta);
    408     const T sintheta = sin(theta);
    409 
    410     R(0, 0) =     costheta   + wx*wx*(kOne -    costheta);
    411     R(1, 0) =  wz*sintheta   + wx*wy*(kOne -    costheta);
    412     R(2, 0) = -wy*sintheta   + wx*wz*(kOne -    costheta);
    413     R(0, 1) =  wx*wy*(kOne - costheta)     - wz*sintheta;
    414     R(1, 1) =     costheta   + wy*wy*(kOne -    costheta);
    415     R(2, 1) =  wx*sintheta   + wy*wz*(kOne -    costheta);
    416     R(0, 2) =  wy*sintheta   + wx*wz*(kOne -    costheta);
    417     R(1, 2) = -wx*sintheta   + wy*wz*(kOne -    costheta);
    418     R(2, 2) =     costheta   + wz*wz*(kOne -    costheta);
    419   } else {
    420     // At zero, we switch to using the first order Taylor expansion.
    421     R(0, 0) =  kOne;
    422     R(1, 0) = -angle_axis[2];
    423     R(2, 0) =  angle_axis[1];
    424     R(0, 1) =  angle_axis[2];
    425     R(1, 1) =  kOne;
    426     R(2, 1) = -angle_axis[0];
    427     R(0, 2) = -angle_axis[1];
    428     R(1, 2) =  angle_axis[0];
    429     R(2, 2) = kOne;
    430   }
    431 }
    432 
    433 template <typename T>
    434 inline void EulerAnglesToRotationMatrix(const T* euler,
    435                                         const int row_stride_parameter,
    436                                         T* R) {
    437   CHECK_EQ(row_stride_parameter, 3);
    438   EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
    439 }
    440 
    441 template <typename T, int row_stride, int col_stride>
    442 void EulerAnglesToRotationMatrix(
    443     const T* euler,
    444     const MatrixAdapter<T, row_stride, col_stride>& R) {
    445   const double kPi = 3.14159265358979323846;
    446   const T degrees_to_radians(kPi / 180.0);
    447 
    448   const T pitch(euler[0] * degrees_to_radians);
    449   const T roll(euler[1] * degrees_to_radians);
    450   const T yaw(euler[2] * degrees_to_radians);
    451 
    452   const T c1 = cos(yaw);
    453   const T s1 = sin(yaw);
    454   const T c2 = cos(roll);
    455   const T s2 = sin(roll);
    456   const T c3 = cos(pitch);
    457   const T s3 = sin(pitch);
    458 
    459   R(0, 0) = c1*c2;
    460   R(0, 1) = -s1*c3 + c1*s2*s3;
    461   R(0, 2) = s1*s3 + c1*s2*c3;
    462 
    463   R(1, 0) = s1*c2;
    464   R(1, 1) = c1*c3 + s1*s2*s3;
    465   R(1, 2) = -c1*s3 + s1*s2*c3;
    466 
    467   R(2, 0) = -s2;
    468   R(2, 1) = c2*s3;
    469   R(2, 2) = c2*c3;
    470 }
    471 
    472 template <typename T> inline
    473 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
    474   QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
    475 }
    476 
    477 template <typename T, int row_stride, int col_stride> inline
    478 void QuaternionToScaledRotation(
    479     const T q[4],
    480     const MatrixAdapter<T, row_stride, col_stride>& R) {
    481   // Make convenient names for elements of q.
    482   T a = q[0];
    483   T b = q[1];
    484   T c = q[2];
    485   T d = q[3];
    486   // This is not to eliminate common sub-expression, but to
    487   // make the lines shorter so that they fit in 80 columns!
    488   T aa = a * a;
    489   T ab = a * b;
    490   T ac = a * c;
    491   T ad = a * d;
    492   T bb = b * b;
    493   T bc = b * c;
    494   T bd = b * d;
    495   T cc = c * c;
    496   T cd = c * d;
    497   T dd = d * d;
    498 
    499   R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad);  R(0, 2) = T(2) * (ac + bd);  // NOLINT
    500   R(1, 0) = T(2) * (ad + bc);  R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab);  // NOLINT
    501   R(2, 0) = T(2) * (bd - ac);  R(2, 1) = T(2) * (ab + cd);  R(2, 2) = aa - bb - cc + dd; // NOLINT
    502 }
    503 
    504 template <typename T> inline
    505 void QuaternionToRotation(const T q[4], T R[3 * 3]) {
    506   QuaternionToRotation(q, RowMajorAdapter3x3(R));
    507 }
    508 
    509 template <typename T, int row_stride, int col_stride> inline
    510 void QuaternionToRotation(const T q[4],
    511                           const MatrixAdapter<T, row_stride, col_stride>& R) {
    512   QuaternionToScaledRotation(q, R);
    513 
    514   T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
    515   CHECK_NE(normalizer, T(0));
    516   normalizer = T(1) / normalizer;
    517 
    518   for (int i = 0; i < 3; ++i) {
    519     for (int j = 0; j < 3; ++j) {
    520       R(i, j) *= normalizer;
    521     }
    522   }
    523 }
    524 
    525 template <typename T> inline
    526 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
    527   const T t2 =  q[0] * q[1];
    528   const T t3 =  q[0] * q[2];
    529   const T t4 =  q[0] * q[3];
    530   const T t5 = -q[1] * q[1];
    531   const T t6 =  q[1] * q[2];
    532   const T t7 =  q[1] * q[3];
    533   const T t8 = -q[2] * q[2];
    534   const T t9 =  q[2] * q[3];
    535   const T t1 = -q[3] * q[3];
    536   result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0];  // NOLINT
    537   result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1];  // NOLINT
    538   result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2];  // NOLINT
    539 }
    540 
    541 template <typename T> inline
    542 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
    543   // 'scale' is 1 / norm(q).
    544   const T scale = T(1) / sqrt(q[0] * q[0] +
    545                               q[1] * q[1] +
    546                               q[2] * q[2] +
    547                               q[3] * q[3]);
    548 
    549   // Make unit-norm version of q.
    550   const T unit[4] = {
    551     scale * q[0],
    552     scale * q[1],
    553     scale * q[2],
    554     scale * q[3],
    555   };
    556 
    557   UnitQuaternionRotatePoint(unit, pt, result);
    558 }
    559 
    560 template<typename T> inline
    561 void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
    562   zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
    563   zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
    564   zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
    565   zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
    566 }
    567 
    568 // xy = x cross y;
    569 template<typename T> inline
    570 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
    571   x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
    572   x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
    573   x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
    574 }
    575 
    576 template<typename T> inline
    577 T DotProduct(const T x[3], const T y[3]) {
    578   return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
    579 }
    580 
    581 template<typename T> inline
    582 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
    583   T w[3];
    584   T sintheta;
    585   T costheta;
    586 
    587   const T theta2 = DotProduct(angle_axis, angle_axis);
    588   if (theta2 > 0.0) {
    589     // Away from zero, use the rodriguez formula
    590     //
    591     //   result = pt costheta +
    592     //            (w x pt) * sintheta +
    593     //            w (w . pt) (1 - costheta)
    594     //
    595     // We want to be careful to only evaluate the square root if the
    596     // norm of the angle_axis vector is greater than zero. Otherwise
    597     // we get a division by zero.
    598     //
    599     const T theta = sqrt(theta2);
    600     w[0] = angle_axis[0] / theta;
    601     w[1] = angle_axis[1] / theta;
    602     w[2] = angle_axis[2] / theta;
    603     costheta = cos(theta);
    604     sintheta = sin(theta);
    605     T w_cross_pt[3];
    606     CrossProduct(w, pt, w_cross_pt);
    607     T w_dot_pt = DotProduct(w, pt);
    608     for (int i = 0; i < 3; ++i) {
    609       result[i] = pt[i] * costheta +
    610           w_cross_pt[i] * sintheta +
    611           w[i] * (T(1.0) - costheta) * w_dot_pt;
    612     }
    613   } else {
    614     // Near zero, the first order Taylor approximation of the rotation
    615     // matrix R corresponding to a vector w and angle w is
    616     //
    617     //   R = I + hat(w) * sin(theta)
    618     //
    619     // But sintheta ~ theta and theta * w = angle_axis, which gives us
    620     //
    621     //  R = I + hat(w)
    622     //
    623     // and actually performing multiplication with the point pt, gives us
    624     // R * pt = pt + w x pt.
    625     //
    626     // Switching to the Taylor expansion at zero helps avoid all sorts
    627     // of numerical nastiness.
    628     T w_cross_pt[3];
    629     CrossProduct(angle_axis, pt, w_cross_pt);
    630     for (int i = 0; i < 3; ++i) {
    631       result[i] = pt[i] + w_cross_pt[i];
    632     }
    633   }
    634 }
    635 
    636 }  // namespace ceres
    637 
    638 #endif  // CERES_PUBLIC_ROTATION_H_
    639