Blame include/ceres/rotation.h

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