1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
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.
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.
29 // Author: keir@google.com (Keir Mierle)
30 // sameeragarwal@google.com (Sameer Agarwal)
32 // Templated functions for manipulating rotations. The templated
33 // functions are useful when implementing functors for automatic
36 // In the following, the Quaternions are laid out as 4-vectors, thus:
39 // q[1] coefficient of i.
40 // q[2] coefficient of j.
41 // q[3] coefficient of k.
43 // where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
45 #ifndef CERES_PUBLIC_ROTATION_H_
46 #define CERES_PUBLIC_ROTATION_H_
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
57 // (const) MatrixAdapter<T, row_stride, col_stride> M"
59 // the expression M(i, j) is equivalent to
61 // arrary[i * row_stride + j * col_stride]
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>
70 // Convenience functions to create a MatrixAdapter that treats the
71 // array pointed to by "pointer" as a 3x3 (contiguous) column-major or
74 MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
77 MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
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.
86 void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
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.
95 void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
97 // Conversions between 3x3 rotation matrix (in column major order) and
98 // quaternion rotation representations. Templated for use with
99 // autodifferentiation.
100 template <typename T>
101 void RotationMatrixToQuaternion(const T* R, T* quaternion);
103 template <typename T, int row_stride, int col_stride>
104 void RotationMatrixToQuaternion(
105 const MatrixAdapter<const T, row_stride, col_stride>& R,
108 // Conversions between 3x3 rotation matrix (in column major order) and
109 // axis-angle rotation representations. Templated for use with
110 // autodifferentiation.
111 template <typename T>
112 void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
114 template <typename T, int row_stride, int col_stride>
115 void RotationMatrixToAngleAxis(
116 const MatrixAdapter<const T, row_stride, col_stride>& R,
119 template <typename T>
120 void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
122 template <typename T, int row_stride, int col_stride>
123 void AngleAxisToRotationMatrix(
125 const MatrixAdapter<T, row_stride, col_stride>& R);
127 // Conversions between 3x3 rotation matrix (in row major order) and
128 // Euler angle (in degrees) rotation representations.
130 // The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
131 // axes, respectively. They are applied in that same order, so the
132 // total rotation R is Rz * Ry * Rx.
133 template <typename T>
134 void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
136 template <typename T, int row_stride, int col_stride>
137 void EulerAnglesToRotationMatrix(
139 const MatrixAdapter<T, row_stride, col_stride>& R);
141 // Convert a 4-vector to a 3x3 scaled rotation matrix.
143 // The choice of rotation is such that the quaternion [1 0 0 0] goes to an
144 // identity matrix and for small a, b, c the quaternion [1 a b c] goes to
148 // I + 2 [ c 0 -a ] + higher order terms
151 // which corresponds to a Rodrigues approximation, the last matrix being
152 // the cross-product matrix of [a b c]. Together with the property that
153 // R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
155 // No normalization of the quaternion is performed, i.e.
156 // R = ||q||^2 * Q, where Q is an orthonormal matrix
157 // such that det(Q) = 1 and Q*Q' = I
159 // WARNING: The rotation matrix is ROW MAJOR
160 template <typename T> inline
161 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
163 template <typename T, int row_stride, int col_stride> inline
164 void QuaternionToScaledRotation(
166 const MatrixAdapter<T, row_stride, col_stride>& R);
168 // Same as above except that the rotation matrix is normalized by the
169 // Frobenius norm, so that R * R' = I (and det(R) = 1).
171 // WARNING: The rotation matrix is ROW MAJOR
172 template <typename T> inline
173 void QuaternionToRotation(const T q[4], T R[3 * 3]);
175 template <typename T, int row_stride, int col_stride> inline
176 void QuaternionToRotation(
178 const MatrixAdapter<T, row_stride, col_stride>& R);
180 // Rotates a point pt by a quaternion q:
182 // result = R(q) * pt
184 // Assumes the quaternion is unit norm. This assumption allows us to
185 // write the transform as (something)*pt + pt, as is clear from the
186 // formula below. If you pass in a quaternion with |q|^2 = 2 then you
187 // WILL NOT get back 2 times the result you get for a unit quaternion.
188 template <typename T> inline
189 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
191 // With this function you do not need to assume that q has unit norm.
192 // It does assume that the norm is non-zero.
193 template <typename T> inline
194 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
196 // zw = z * w, where * is the Quaternion product between 4 vectors.
197 template<typename T> inline
198 void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
201 template<typename T> inline
202 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
204 template<typename T> inline
205 T DotProduct(const T x[3], const T y[3]);
207 // y = R(angle_axis) * x;
208 template<typename T> inline
209 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
211 // --- IMPLEMENTATION
213 template<typename T, int row_stride, int col_stride>
214 struct MatrixAdapter {
216 explicit MatrixAdapter(T* pointer)
220 T& operator()(int r, int c) const {
221 return pointer_[r * row_stride + c * col_stride];
225 template <typename T>
226 MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
227 return MatrixAdapter<T, 1, 3>(pointer);
230 template <typename T>
231 MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
232 return MatrixAdapter<T, 3, 1>(pointer);
236 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
237 const T& a0 = angle_axis[0];
238 const T& a1 = angle_axis[1];
239 const T& a2 = angle_axis[2];
240 const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
242 // For points not at the origin, the full conversion is numerically stable.
243 if (theta_squared > T(0.0)) {
244 const T theta = sqrt(theta_squared);
245 const T half_theta = theta * T(0.5);
246 const T k = sin(half_theta) / theta;
247 quaternion[0] = cos(half_theta);
248 quaternion[1] = a0 * k;
249 quaternion[2] = a1 * k;
250 quaternion[3] = a2 * k;
252 // At the origin, sqrt() will produce NaN in the derivative since
253 // the argument is zero. By approximating with a Taylor series,
254 // and truncating at one term, the value and first derivatives will be
255 // computed correctly when Jets are used.
257 quaternion[0] = T(1.0);
258 quaternion[1] = a0 * k;
259 quaternion[2] = a1 * k;
260 quaternion[3] = a2 * k;
265 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
266 const T& q1 = quaternion[1];
267 const T& q2 = quaternion[2];
268 const T& q3 = quaternion[3];
269 const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
271 // For quaternions representing non-zero rotation, the conversion
272 // is numerically stable.
273 if (sin_squared_theta > T(0.0)) {
274 const T sin_theta = sqrt(sin_squared_theta);
275 const T& cos_theta = quaternion[0];
277 // If cos_theta is negative, theta is greater than pi/2, which
278 // means that angle for the angle_axis vector which is 2 * theta
279 // would be greater than pi.
281 // While this will result in the correct rotation, it does not
282 // result in a normalized angle-axis vector.
284 // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
285 // which is equivalent saying
287 // theta - pi = atan(sin(theta - pi), cos(theta - pi))
288 // = atan(-sin(theta), -cos(theta))
291 T(2.0) * ((cos_theta < 0.0)
292 ? atan2(-sin_theta, -cos_theta)
293 : atan2(sin_theta, cos_theta));
294 const T k = two_theta / sin_theta;
295 angle_axis[0] = q1 * k;
296 angle_axis[1] = q2 * k;
297 angle_axis[2] = q3 * k;
299 // For zero rotation, sqrt() will produce NaN in the derivative since
300 // the argument is zero. By approximating with a Taylor series,
301 // and truncating at one term, the value and first derivatives will be
302 // computed correctly when Jets are used.
304 angle_axis[0] = q1 * k;
305 angle_axis[1] = q2 * k;
306 angle_axis[2] = q3 * k;
310 template <typename T>
311 void RotationMatrixToQuaternion(const T* R, T* angle_axis) {
312 RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis);
315 // This algorithm comes from "Quaternion Calculus and Fast Animation",
316 // Ken Shoemake, 1987 SIGGRAPH course notes
317 template <typename T, int row_stride, int col_stride>
318 void RotationMatrixToQuaternion(
319 const MatrixAdapter<const T, row_stride, col_stride>& R,
321 const T trace = R(0, 0) + R(1, 1) + R(2, 2);
323 T t = sqrt(trace + T(1.0));
324 quaternion[0] = T(0.5) * t;
326 quaternion[1] = (R(2, 1) - R(1, 2)) * t;
327 quaternion[2] = (R(0, 2) - R(2, 0)) * t;
328 quaternion[3] = (R(1, 0) - R(0, 1)) * t;
331 if (R(1, 1) > R(0, 0)) {
335 if (R(2, 2) > R(i, i)) {
339 const int j = (i + 1) % 3;
340 const int k = (j + 1) % 3;
341 T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0));
342 quaternion[i + 1] = T(0.5) * t;
344 quaternion[0] = (R(k, j) - R(j, k)) * t;
345 quaternion[j + 1] = (R(j, i) + R(i, j)) * t;
346 quaternion[k + 1] = (R(k, i) + R(i, k)) * t;
350 // The conversion of a rotation matrix to the angle-axis form is
351 // numerically problematic when then rotation angle is close to zero
352 // or to Pi. The following implementation detects when these two cases
353 // occurs and deals with them by taking code paths that are guaranteed
354 // to not perform division by a small number.
355 template <typename T>
356 inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
357 RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
360 template <typename T, int row_stride, int col_stride>
361 void RotationMatrixToAngleAxis(
362 const MatrixAdapter<const T, row_stride, col_stride>& R,
365 RotationMatrixToQuaternion(R, quaternion);
366 QuaternionToAngleAxis(quaternion, angle_axis);
370 template <typename T>
371 inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
372 AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
375 template <typename T, int row_stride, int col_stride>
376 void AngleAxisToRotationMatrix(
378 const MatrixAdapter<T, row_stride, col_stride>& R) {
379 static const T kOne = T(1.0);
380 const T theta2 = DotProduct(angle_axis, angle_axis);
381 if (theta2 > T(std::numeric_limits<double>::epsilon())) {
382 // We want to be careful to only evaluate the square root if the
383 // norm of the angle_axis vector is greater than zero. Otherwise
384 // we get a division by zero.
385 const T theta = sqrt(theta2);
386 const T wx = angle_axis[0] / theta;
387 const T wy = angle_axis[1] / theta;
388 const T wz = angle_axis[2] / theta;
390 const T costheta = cos(theta);
391 const T sintheta = sin(theta);
393 R(0, 0) = costheta + wx*wx*(kOne - costheta);
394 R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
395 R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
396 R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
397 R(1, 1) = costheta + wy*wy*(kOne - costheta);
398 R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
399 R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
400 R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
401 R(2, 2) = costheta + wz*wz*(kOne - costheta);
403 // Near zero, we switch to using the first order Taylor expansion.
405 R(1, 0) = angle_axis[2];
406 R(2, 0) = -angle_axis[1];
407 R(0, 1) = -angle_axis[2];
409 R(2, 1) = angle_axis[0];
410 R(0, 2) = angle_axis[1];
411 R(1, 2) = -angle_axis[0];
416 template <typename T>
417 inline void EulerAnglesToRotationMatrix(const T* euler,
418 const int row_stride_parameter,
420 EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
423 template <typename T, int row_stride, int col_stride>
424 void EulerAnglesToRotationMatrix(
426 const MatrixAdapter<T, row_stride, col_stride>& R) {
427 const double kPi = 3.14159265358979323846;
428 const T degrees_to_radians(kPi / 180.0);
430 const T pitch(euler[0] * degrees_to_radians);
431 const T roll(euler[1] * degrees_to_radians);
432 const T yaw(euler[2] * degrees_to_radians);
434 const T c1 = cos(yaw);
435 const T s1 = sin(yaw);
436 const T c2 = cos(roll);
437 const T s2 = sin(roll);
438 const T c3 = cos(pitch);
439 const T s3 = sin(pitch);
442 R(0, 1) = -s1*c3 + c1*s2*s3;
443 R(0, 2) = s1*s3 + c1*s2*c3;
446 R(1, 1) = c1*c3 + s1*s2*s3;
447 R(1, 2) = -c1*s3 + s1*s2*c3;
454 template <typename T> inline
455 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
456 QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
459 template <typename T, int row_stride, int col_stride> inline
460 void QuaternionToScaledRotation(
462 const MatrixAdapter<T, row_stride, col_stride>& R) {
463 // Make convenient names for elements of q.
468 // This is not to eliminate common sub-expression, but to
469 // make the lines shorter so that they fit in 80 columns!
481 R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
482 R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
483 R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
486 template <typename T> inline
487 void QuaternionToRotation(const T q[4], T R[3 * 3]) {
488 QuaternionToRotation(q, RowMajorAdapter3x3(R));
491 template <typename T, int row_stride, int col_stride> inline
492 void QuaternionToRotation(const T q[4],
493 const MatrixAdapter<T, row_stride, col_stride>& R) {
494 QuaternionToScaledRotation(q, R);
496 T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
497 normalizer = T(1) / normalizer;
499 for (int i = 0; i < 3; ++i) {
500 for (int j = 0; j < 3; ++j) {
501 R(i, j) *= normalizer;
506 template <typename T> inline
507 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
508 const T t2 = q[0] * q[1];
509 const T t3 = q[0] * q[2];
510 const T t4 = q[0] * q[3];
511 const T t5 = -q[1] * q[1];
512 const T t6 = q[1] * q[2];
513 const T t7 = q[1] * q[3];
514 const T t8 = -q[2] * q[2];
515 const T t9 = q[2] * q[3];
516 const T t1 = -q[3] * q[3];
517 result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
518 result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
519 result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
522 template <typename T> inline
523 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
524 // 'scale' is 1 / norm(q).
525 const T scale = T(1) / sqrt(q[0] * q[0] +
530 // Make unit-norm version of q.
538 UnitQuaternionRotatePoint(unit, pt, result);
541 template<typename T> inline
542 void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
543 zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
544 zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
545 zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
546 zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
550 template<typename T> inline
551 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
552 x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
553 x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
554 x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
557 template<typename T> inline
558 T DotProduct(const T x[3], const T y[3]) {
559 return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
562 template<typename T> inline
563 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
564 const T theta2 = DotProduct(angle_axis, angle_axis);
565 if (theta2 > T(std::numeric_limits<double>::epsilon())) {
566 // Away from zero, use the rodriguez formula
568 // result = pt costheta +
569 // (w x pt) * sintheta +
570 // w (w . pt) (1 - costheta)
572 // We want to be careful to only evaluate the square root if the
573 // norm of the angle_axis vector is greater than zero. Otherwise
574 // we get a division by zero.
576 const T theta = sqrt(theta2);
577 const T costheta = cos(theta);
578 const T sintheta = sin(theta);
579 const T theta_inverse = T(1.0) / theta;
581 const T w[3] = { angle_axis[0] * theta_inverse,
582 angle_axis[1] * theta_inverse,
583 angle_axis[2] * theta_inverse };
585 // Explicitly inlined evaluation of the cross product for
586 // performance reasons.
587 const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
588 w[2] * pt[0] - w[0] * pt[2],
589 w[0] * pt[1] - w[1] * pt[0] };
591 (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
593 result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
594 result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
595 result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
597 // Near zero, the first order Taylor approximation of the rotation
598 // matrix R corresponding to a vector w and angle w is
600 // R = I + hat(w) * sin(theta)
602 // But sintheta ~ theta and theta * w = angle_axis, which gives us
606 // and actually performing multiplication with the point pt, gives us
607 // R * pt = pt + w x pt.
609 // Switching to the Taylor expansion near zero provides meaningful
610 // derivatives when evaluated using Jets.
612 // Explicitly inlined evaluation of the cross product for
613 // performance reasons.
614 const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
615 angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
616 angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
618 result[0] = pt[0] + w_cross_pt[0];
619 result[1] = pt[1] + w_cross_pt[1];
620 result[2] = pt[2] + w_cross_pt[2];
626 #endif // CERES_PUBLIC_ROTATION_H_