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 // Convert a value in combined axis-angle representation to a quaternion. 55 // The value angle_axis is a triple whose norm is an angle in radians, 56 // and whose direction is aligned with the axis of rotation, 57 // and quaternion is a 4-tuple that will contain the resulting quaternion. 58 // The implementation may be used with auto-differentiation up to the first 59 // derivative, higher derivatives may have unexpected results near the origin. 60 template<typename T> 61 void AngleAxisToQuaternion(T const* angle_axis, T* quaternion); 62 63 // Convert a quaternion to the equivalent combined axis-angle representation. 64 // The value quaternion must be a unit quaternion - it is not normalized first, 65 // and angle_axis will be filled with a value whose norm is the angle of 66 // rotation in radians, and whose direction is the axis of rotation. 67 // The implemention may be used with auto-differentiation up to the first 68 // derivative, higher derivatives may have unexpected results near the origin. 69 template<typename T> 70 void QuaternionToAngleAxis(T const* quaternion, T* angle_axis); 71 72 // Conversions between 3x3 rotation matrix (in column major order) and 73 // axis-angle rotation representations. Templated for use with 74 // autodifferentiation. 75 template <typename T> 76 void RotationMatrixToAngleAxis(T const * R, T * angle_axis); 77 template <typename T> 78 void AngleAxisToRotationMatrix(T const * angle_axis, T * R); 79 80 // Conversions between 3x3 rotation matrix (in row major order) and 81 // Euler angle (in degrees) rotation representations. 82 // 83 // The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} 84 // axes, respectively. They are applied in that same order, so the 85 // total rotation R is Rz * Ry * Rx. 86 template <typename T> 87 void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); 88 89 // Convert a 4-vector to a 3x3 scaled rotation matrix. 90 // 91 // The choice of rotation is such that the quaternion [1 0 0 0] goes to an 92 // identity matrix and for small a, b, c the quaternion [1 a b c] goes to 93 // the matrix 94 // 95 // [ 0 -c b ] 96 // I + 2 [ c 0 -a ] + higher order terms 97 // [ -b a 0 ] 98 // 99 // which corresponds to a Rodrigues approximation, the last matrix being 100 // the cross-product matrix of [a b c]. Together with the property that 101 // R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. 102 // 103 // The rotation matrix is row-major. 104 // 105 // No normalization of the quaternion is performed, i.e. 106 // R = ||q||^2 * Q, where Q is an orthonormal matrix 107 // such that det(Q) = 1 and Q*Q' = I 108 template <typename T> inline 109 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); 110 111 // Same as above except that the rotation matrix is normalized by the 112 // Frobenius norm, so that R * R' = I (and det(R) = 1). 113 template <typename T> inline 114 void QuaternionToRotation(const T q[4], T R[3 * 3]); 115 116 // Rotates a point pt by a quaternion q: 117 // 118 // result = R(q) * pt 119 // 120 // Assumes the quaternion is unit norm. This assumption allows us to 121 // write the transform as (something)*pt + pt, as is clear from the 122 // formula below. If you pass in a quaternion with |q|^2 = 2 then you 123 // WILL NOT get back 2 times the result you get for a unit quaternion. 124 template <typename T> inline 125 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); 126 127 // With this function you do not need to assume that q has unit norm. 128 // It does assume that the norm is non-zero. 129 template <typename T> inline 130 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); 131 132 // zw = z * w, where * is the Quaternion product between 4 vectors. 133 template<typename T> inline 134 void QuaternionProduct(const T z[4], const T w[4], T zw[4]); 135 136 // xy = x cross y; 137 template<typename T> inline 138 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); 139 140 template<typename T> inline 141 T DotProduct(const T x[3], const T y[3]); 142 143 // y = R(angle_axis) * x; 144 template<typename T> inline 145 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); 146 147 // --- IMPLEMENTATION 148 149 template<typename T> 150 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { 151 const T& a0 = angle_axis[0]; 152 const T& a1 = angle_axis[1]; 153 const T& a2 = angle_axis[2]; 154 const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; 155 156 // For points not at the origin, the full conversion is numerically stable. 157 if (theta_squared > T(0.0)) { 158 const T theta = sqrt(theta_squared); 159 const T half_theta = theta * T(0.5); 160 const T k = sin(half_theta) / theta; 161 quaternion[0] = cos(half_theta); 162 quaternion[1] = a0 * k; 163 quaternion[2] = a1 * k; 164 quaternion[3] = a2 * k; 165 } else { 166 // At the origin, sqrt() will produce NaN in the derivative since 167 // the argument is zero. By approximating with a Taylor series, 168 // and truncating at one term, the value and first derivatives will be 169 // computed correctly when Jets are used. 170 const T k(0.5); 171 quaternion[0] = T(1.0); 172 quaternion[1] = a0 * k; 173 quaternion[2] = a1 * k; 174 quaternion[3] = a2 * k; 175 } 176 } 177 178 template<typename T> 179 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { 180 const T& q1 = quaternion[1]; 181 const T& q2 = quaternion[2]; 182 const T& q3 = quaternion[3]; 183 const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; 184 185 // For quaternions representing non-zero rotation, the conversion 186 // is numerically stable. 187 if (sin_squared_theta > T(0.0)) { 188 const T sin_theta = sqrt(sin_squared_theta); 189 const T& cos_theta = quaternion[0]; 190 191 // If cos_theta is negative, theta is greater than pi/2, which 192 // means that angle for the angle_axis vector which is 2 * theta 193 // would be greater than pi. 194 // 195 // While this will result in the correct rotation, it does not 196 // result in a normalized angle-axis vector. 197 // 198 // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, 199 // which is equivalent saying 200 // 201 // theta - pi = atan(sin(theta - pi), cos(theta - pi)) 202 // = atan(-sin(theta), -cos(theta)) 203 // 204 const T two_theta = 205 T(2.0) * ((cos_theta < 0.0) 206 ? atan2(-sin_theta, -cos_theta) 207 : atan2(sin_theta, cos_theta)); 208 const T k = two_theta / sin_theta; 209 angle_axis[0] = q1 * k; 210 angle_axis[1] = q2 * k; 211 angle_axis[2] = q3 * k; 212 } else { 213 // For zero rotation, sqrt() will produce NaN in the derivative since 214 // the argument is zero. By approximating with a Taylor series, 215 // and truncating at one term, the value and first derivatives will be 216 // computed correctly when Jets are used. 217 const T k(2.0); 218 angle_axis[0] = q1 * k; 219 angle_axis[1] = q2 * k; 220 angle_axis[2] = q3 * k; 221 } 222 } 223 224 // The conversion of a rotation matrix to the angle-axis form is 225 // numerically problematic when then rotation angle is close to zero 226 // or to Pi. The following implementation detects when these two cases 227 // occurs and deals with them by taking code paths that are guaranteed 228 // to not perform division by a small number. 229 template <typename T> 230 inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) { 231 // x = k * 2 * sin(theta), where k is the axis of rotation. 232 angle_axis[0] = R[5] - R[7]; 233 angle_axis[1] = R[6] - R[2]; 234 angle_axis[2] = R[1] - R[3]; 235 236 static const T kOne = T(1.0); 237 static const T kTwo = T(2.0); 238 239 // Since the right hand side may give numbers just above 1.0 or 240 // below -1.0 leading to atan misbehaving, we threshold. 241 T costheta = std::min(std::max((R[0] + R[4] + R[8] - kOne) / kTwo, 242 T(-1.0)), 243 kOne); 244 245 // sqrt is guaranteed to give non-negative results, so we only 246 // threshold above. 247 T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + 248 angle_axis[1] * angle_axis[1] + 249 angle_axis[2] * angle_axis[2]) / kTwo, 250 kOne); 251 252 // Use the arctan2 to get the right sign on theta 253 const T theta = atan2(sintheta, costheta); 254 255 // Case 1: sin(theta) is large enough, so dividing by it is not a 256 // problem. We do not use abs here, because while jets.h imports 257 // std::abs into the namespace, here in this file, abs resolves to 258 // the int version of the function, which returns zero always. 259 // 260 // We use a threshold much larger then the machine epsilon, because 261 // if sin(theta) is small, not only do we risk overflow but even if 262 // that does not occur, just dividing by a small number will result 263 // in numerical garbage. So we play it safe. 264 static const double kThreshold = 1e-12; 265 if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { 266 const T r = theta / (kTwo * sintheta); 267 for (int i = 0; i < 3; ++i) { 268 angle_axis[i] *= r; 269 } 270 return; 271 } 272 273 // Case 2: theta ~ 0, means sin(theta) ~ theta to a good 274 // approximation. 275 if (costheta > 0.0) { 276 const T kHalf = T(0.5); 277 for (int i = 0; i < 3; ++i) { 278 angle_axis[i] *= kHalf; 279 } 280 return; 281 } 282 283 // Case 3: theta ~ pi, this is the hard case. Since theta is large, 284 // and sin(theta) is small. Dividing by theta by sin(theta) will 285 // either give an overflow or worse still numerically meaningless 286 // results. Thus we use an alternate more complicated formula 287 // here. 288 289 // Since cos(theta) is negative, division by (1-cos(theta)) cannot 290 // overflow. 291 const T inv_one_minus_costheta = kOne / (kOne - costheta); 292 293 // We now compute the absolute value of coordinates of the axis 294 // vector using the diagonal entries of R. To resolve the sign of 295 // these entries, we compare the sign of angle_axis[i]*sin(theta) 296 // with the sign of sin(theta). If they are the same, then 297 // angle_axis[i] should be positive, otherwise negative. 298 for (int i = 0; i < 3; ++i) { 299 angle_axis[i] = theta * sqrt((R[i*4] - costheta) * inv_one_minus_costheta); 300 if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || 301 ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { 302 angle_axis[i] = -angle_axis[i]; 303 } 304 } 305 } 306 307 template <typename T> 308 inline void AngleAxisToRotationMatrix(const T * angle_axis, T * R) { 309 static const T kOne = T(1.0); 310 const T theta2 = DotProduct(angle_axis, angle_axis); 311 if (theta2 > 0.0) { 312 // We want to be careful to only evaluate the square root if the 313 // norm of the angle_axis vector is greater than zero. Otherwise 314 // we get a division by zero. 315 const T theta = sqrt(theta2); 316 const T wx = angle_axis[0] / theta; 317 const T wy = angle_axis[1] / theta; 318 const T wz = angle_axis[2] / theta; 319 320 const T costheta = cos(theta); 321 const T sintheta = sin(theta); 322 323 R[0] = costheta + wx*wx*(kOne - costheta); 324 R[1] = wz*sintheta + wx*wy*(kOne - costheta); 325 R[2] = -wy*sintheta + wx*wz*(kOne - costheta); 326 R[3] = wx*wy*(kOne - costheta) - wz*sintheta; 327 R[4] = costheta + wy*wy*(kOne - costheta); 328 R[5] = wx*sintheta + wy*wz*(kOne - costheta); 329 R[6] = wy*sintheta + wx*wz*(kOne - costheta); 330 R[7] = -wx*sintheta + wy*wz*(kOne - costheta); 331 R[8] = costheta + wz*wz*(kOne - costheta); 332 } else { 333 // At zero, we switch to using the first order Taylor expansion. 334 R[0] = kOne; 335 R[1] = -angle_axis[2]; 336 R[2] = angle_axis[1]; 337 R[3] = angle_axis[2]; 338 R[4] = kOne; 339 R[5] = -angle_axis[0]; 340 R[6] = -angle_axis[1]; 341 R[7] = angle_axis[0]; 342 R[8] = kOne; 343 } 344 } 345 346 template <typename T> 347 inline void EulerAnglesToRotationMatrix(const T* euler, 348 const int row_stride, 349 T* R) { 350 const double kPi = 3.14159265358979323846; 351 const T degrees_to_radians(kPi / 180.0); 352 353 const T pitch(euler[0] * degrees_to_radians); 354 const T roll(euler[1] * degrees_to_radians); 355 const T yaw(euler[2] * degrees_to_radians); 356 357 const T c1 = cos(yaw); 358 const T s1 = sin(yaw); 359 const T c2 = cos(roll); 360 const T s2 = sin(roll); 361 const T c3 = cos(pitch); 362 const T s3 = sin(pitch); 363 364 // Rows of the rotation matrix. 365 T* R1 = R; 366 T* R2 = R1 + row_stride; 367 T* R3 = R2 + row_stride; 368 369 R1[0] = c1*c2; 370 R1[1] = -s1*c3 + c1*s2*s3; 371 R1[2] = s1*s3 + c1*s2*c3; 372 373 R2[0] = s1*c2; 374 R2[1] = c1*c3 + s1*s2*s3; 375 R2[2] = -c1*s3 + s1*s2*c3; 376 377 R3[0] = -s2; 378 R3[1] = c2*s3; 379 R3[2] = c2*c3; 380 } 381 382 template <typename T> inline 383 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { 384 // Make convenient names for elements of q. 385 T a = q[0]; 386 T b = q[1]; 387 T c = q[2]; 388 T d = q[3]; 389 // This is not to eliminate common sub-expression, but to 390 // make the lines shorter so that they fit in 80 columns! 391 T aa = a * a; 392 T ab = a * b; 393 T ac = a * c; 394 T ad = a * d; 395 T bb = b * b; 396 T bc = b * c; 397 T bd = b * d; 398 T cc = c * c; 399 T cd = c * d; 400 T dd = d * d; 401 402 R[0] = aa + bb - cc - dd; R[1] = T(2) * (bc - ad); R[2] = T(2) * (ac + bd); // NOLINT 403 R[3] = T(2) * (ad + bc); R[4] = aa - bb + cc - dd; R[5] = T(2) * (cd - ab); // NOLINT 404 R[6] = T(2) * (bd - ac); R[7] = T(2) * (ab + cd); R[8] = aa - bb - cc + dd; // NOLINT 405 } 406 407 template <typename T> inline 408 void QuaternionToRotation(const T q[4], T R[3 * 3]) { 409 QuaternionToScaledRotation(q, R); 410 411 T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; 412 CHECK_NE(normalizer, T(0)); 413 normalizer = T(1) / normalizer; 414 415 for (int i = 0; i < 9; ++i) { 416 R[i] *= normalizer; 417 } 418 } 419 420 template <typename T> inline 421 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 422 const T t2 = q[0] * q[1]; 423 const T t3 = q[0] * q[2]; 424 const T t4 = q[0] * q[3]; 425 const T t5 = -q[1] * q[1]; 426 const T t6 = q[1] * q[2]; 427 const T t7 = q[1] * q[3]; 428 const T t8 = -q[2] * q[2]; 429 const T t9 = q[2] * q[3]; 430 const T t1 = -q[3] * q[3]; 431 result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT 432 result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT 433 result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT 434 } 435 436 437 template <typename T> inline 438 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 439 // 'scale' is 1 / norm(q). 440 const T scale = T(1) / sqrt(q[0] * q[0] + 441 q[1] * q[1] + 442 q[2] * q[2] + 443 q[3] * q[3]); 444 445 // Make unit-norm version of q. 446 const T unit[4] = { 447 scale * q[0], 448 scale * q[1], 449 scale * q[2], 450 scale * q[3], 451 }; 452 453 UnitQuaternionRotatePoint(unit, pt, result); 454 } 455 456 template<typename T> inline 457 void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { 458 zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; 459 zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; 460 zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; 461 zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; 462 } 463 464 // xy = x cross y; 465 template<typename T> inline 466 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { 467 x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; 468 x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; 469 x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; 470 } 471 472 template<typename T> inline 473 T DotProduct(const T x[3], const T y[3]) { 474 return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); 475 } 476 477 template<typename T> inline 478 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { 479 T w[3]; 480 T sintheta; 481 T costheta; 482 483 const T theta2 = DotProduct(angle_axis, angle_axis); 484 if (theta2 > 0.0) { 485 // Away from zero, use the rodriguez formula 486 // 487 // result = pt costheta + 488 // (w x pt) * sintheta + 489 // w (w . pt) (1 - costheta) 490 // 491 // We want to be careful to only evaluate the square root if the 492 // norm of the angle_axis vector is greater than zero. Otherwise 493 // we get a division by zero. 494 // 495 const T theta = sqrt(theta2); 496 w[0] = angle_axis[0] / theta; 497 w[1] = angle_axis[1] / theta; 498 w[2] = angle_axis[2] / theta; 499 costheta = cos(theta); 500 sintheta = sin(theta); 501 T w_cross_pt[3]; 502 CrossProduct(w, pt, w_cross_pt); 503 T w_dot_pt = DotProduct(w, pt); 504 for (int i = 0; i < 3; ++i) { 505 result[i] = pt[i] * costheta + 506 w_cross_pt[i] * sintheta + 507 w[i] * (T(1.0) - costheta) * w_dot_pt; 508 } 509 } else { 510 // Near zero, the first order Taylor approximation of the rotation 511 // matrix R corresponding to a vector w and angle w is 512 // 513 // R = I + hat(w) * sin(theta) 514 // 515 // But sintheta ~ theta and theta * w = angle_axis, which gives us 516 // 517 // R = I + hat(w) 518 // 519 // and actually performing multiplication with the point pt, gives us 520 // R * pt = pt + w x pt. 521 // 522 // Switching to the Taylor expansion at zero helps avoid all sorts 523 // of numerical nastiness. 524 T w_cross_pt[3]; 525 CrossProduct(angle_axis, pt, w_cross_pt); 526 for (int i = 0; i < 3; ++i) { 527 result[i] = pt[i] + w_cross_pt[i]; 528 } 529 } 530 } 531 532 } // namespace ceres 533 534 #endif // CERES_PUBLIC_ROTATION_H_ 535