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@google.com (Keir Mierle) 30// sameeragarwal@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 52namespace 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). 67template <typename T, int row_stride, int col_stride> 68struct 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. 73template <typename T> 74MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer); 75 76template <typename T> 77MatrixAdapter<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. 85template<typename T> 86void 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. 94template<typename T> 95void 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. 100template <typename T> 101void RotationMatrixToAngleAxis(const T* R, T* angle_axis); 102 103template <typename T, int row_stride, int col_stride> 104void RotationMatrixToAngleAxis( 105 const MatrixAdapter<const T, row_stride, col_stride>& R, 106 T* angle_axis); 107 108template <typename T> 109void AngleAxisToRotationMatrix(const T* angle_axis, T* R); 110 111template <typename T, int row_stride, int col_stride> 112void 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. 122template <typename T> 123void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); 124 125template <typename T, int row_stride, int col_stride> 126void 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 149template <typename T> inline 150void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); 151 152template <typename T, int row_stride, int col_stride> inline 153void 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). 159template <typename T> inline 160void QuaternionToRotation(const T q[4], T R[3 * 3]); 161 162template <typename T, int row_stride, int col_stride> inline 163void 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. 175template <typename T> inline 176void 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. 180template <typename T> inline 181void 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. 184template<typename T> inline 185void QuaternionProduct(const T z[4], const T w[4], T zw[4]); 186 187// xy = x cross y; 188template<typename T> inline 189void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); 190 191template<typename T> inline 192T DotProduct(const T x[3], const T y[3]); 193 194// y = R(angle_axis) * x; 195template<typename T> inline 196void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); 197 198// --- IMPLEMENTATION 199 200template<typename T, int row_stride, int col_stride> 201struct 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 212template <typename T> 213MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) { 214 return MatrixAdapter<T, 1, 3>(pointer); 215} 216 217template <typename T> 218MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) { 219 return MatrixAdapter<T, 3, 1>(pointer); 220} 221 222template<typename T> 223inline 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 251template<typename T> 252inline 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. 302template <typename T> 303inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { 304 RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); 305} 306 307template <typename T, int row_stride, int col_stride> 308void 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 387template <typename T> 388inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { 389 AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); 390} 391 392template <typename T, int row_stride, int col_stride> 393void 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 > T(std::numeric_limits<double>::epsilon())) { 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 // Near 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 433template <typename T> 434inline 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 441template <typename T, int row_stride, int col_stride> 442void 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 472template <typename T> inline 473void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { 474 QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); 475} 476 477template <typename T, int row_stride, int col_stride> inline 478void 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 504template <typename T> inline 505void QuaternionToRotation(const T q[4], T R[3 * 3]) { 506 QuaternionToRotation(q, RowMajorAdapter3x3(R)); 507} 508 509template <typename T, int row_stride, int col_stride> inline 510void 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 525template <typename T> inline 526void 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 541template <typename T> inline 542void 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 560template<typename T> inline 561void 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; 569template<typename T> inline 570void 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 576template<typename T> inline 577T 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 581template<typename T> inline 582void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { 583 const T theta2 = DotProduct(angle_axis, angle_axis); 584 if (theta2 > T(std::numeric_limits<double>::epsilon())) { 585 // Away from zero, use the rodriguez formula 586 // 587 // result = pt costheta + 588 // (w x pt) * sintheta + 589 // w (w . pt) (1 - costheta) 590 // 591 // We want to be careful to only evaluate the square root if the 592 // norm of the angle_axis vector is greater than zero. Otherwise 593 // we get a division by zero. 594 // 595 const T theta = sqrt(theta2); 596 const T costheta = cos(theta); 597 const T sintheta = sin(theta); 598 const T theta_inverse = 1.0 / theta; 599 600 const T w[3] = { angle_axis[0] * theta_inverse, 601 angle_axis[1] * theta_inverse, 602 angle_axis[2] * theta_inverse }; 603 604 // Explicitly inlined evaluation of the cross product for 605 // performance reasons. 606 const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], 607 w[2] * pt[0] - w[0] * pt[2], 608 w[0] * pt[1] - w[1] * pt[0] }; 609 const T tmp = 610 (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); 611 612 result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; 613 result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; 614 result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; 615 } else { 616 // Near zero, the first order Taylor approximation of the rotation 617 // matrix R corresponding to a vector w and angle w is 618 // 619 // R = I + hat(w) * sin(theta) 620 // 621 // But sintheta ~ theta and theta * w = angle_axis, which gives us 622 // 623 // R = I + hat(w) 624 // 625 // and actually performing multiplication with the point pt, gives us 626 // R * pt = pt + w x pt. 627 // 628 // Switching to the Taylor expansion near zero provides meaningful 629 // derivatives when evaluated using Jets. 630 // 631 // Explicitly inlined evaluation of the cross product for 632 // performance reasons. 633 const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], 634 angle_axis[2] * pt[0] - angle_axis[0] * pt[2], 635 angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; 636 637 result[0] = pt[0] + w_cross_pt[0]; 638 result[1] = pt[1] + w_cross_pt[1]; 639 result[2] = pt[2] + w_cross_pt[2]; 640 } 641} 642 643} // namespace ceres 644 645#endif // CERES_PUBLIC_ROTATION_H_ 646