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: sameeragarwal (at) google.com (Sameer Agarwal) 30 31 #include <cmath> 32 #include <limits> 33 #include <string> 34 #include "ceres/internal/eigen.h" 35 #include "ceres/internal/port.h" 36 #include "ceres/jet.h" 37 #include "ceres/rotation.h" 38 #include "ceres/stringprintf.h" 39 #include "ceres/test_util.h" 40 #include "glog/logging.h" 41 #include "gmock/gmock.h" 42 #include "gtest/gtest.h" 43 44 namespace ceres { 45 namespace internal { 46 47 const double kPi = 3.14159265358979323846; 48 const double kHalfSqrt2 = 0.707106781186547524401; 49 50 double RandDouble() { 51 double r = rand(); 52 return r / RAND_MAX; 53 } 54 55 // A tolerance value for floating-point comparisons. 56 static double const kTolerance = numeric_limits<double>::epsilon() * 10; 57 58 // Looser tolerance used for numerically unstable conversions. 59 static double const kLooseTolerance = 1e-9; 60 61 // Use as: 62 // double quaternion[4]; 63 // EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 64 MATCHER(IsNormalizedQuaternion, "") { 65 if (arg == NULL) { 66 *result_listener << "Null quaternion"; 67 return false; 68 } 69 70 double norm2 = arg[0] * arg[0] + arg[1] * arg[1] + 71 arg[2] * arg[2] + arg[3] * arg[3]; 72 if (fabs(norm2 - 1.0) > kTolerance) { 73 *result_listener << "squared norm is " << norm2; 74 return false; 75 } 76 77 return true; 78 } 79 80 // Use as: 81 // double expected_quaternion[4]; 82 // double actual_quaternion[4]; 83 // EXPECT_THAT(actual_quaternion, IsNearQuaternion(expected_quaternion)); 84 MATCHER_P(IsNearQuaternion, expected, "") { 85 if (arg == NULL) { 86 *result_listener << "Null quaternion"; 87 return false; 88 } 89 90 // Quaternions are equivalent upto a sign change. So we will compare 91 // both signs before declaring failure. 92 bool near = true; 93 for (int i = 0; i < 4; i++) { 94 if (fabs(arg[i] - expected[i]) > kTolerance) { 95 near = false; 96 break; 97 } 98 } 99 100 if (near) { 101 return true; 102 } 103 104 near = true; 105 for (int i = 0; i < 4; i++) { 106 if (fabs(arg[i] + expected[i]) > kTolerance) { 107 near = false; 108 break; 109 } 110 } 111 112 if (near) { 113 return true; 114 } 115 116 *result_listener << "expected : " 117 << expected[0] << " " 118 << expected[1] << " " 119 << expected[2] << " " 120 << expected[3] << " " 121 << "actual : " 122 << arg[0] << " " 123 << arg[1] << " " 124 << arg[2] << " " 125 << arg[3]; 126 return false; 127 } 128 129 // Use as: 130 // double expected_axis_angle[4]; 131 // double actual_axis_angle[4]; 132 // EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle)); 133 MATCHER_P(IsNearAngleAxis, expected, "") { 134 if (arg == NULL) { 135 *result_listener << "Null axis/angle"; 136 return false; 137 } 138 139 for (int i = 0; i < 3; i++) { 140 if (fabs(arg[i] - expected[i]) > kTolerance) { 141 *result_listener << "component " << i << " should be " << expected[i]; 142 return false; 143 } 144 } 145 146 return true; 147 } 148 149 // Use as: 150 // double matrix[9]; 151 // EXPECT_THAT(matrix, IsOrthonormal()); 152 MATCHER(IsOrthonormal, "") { 153 if (arg == NULL) { 154 *result_listener << "Null matrix"; 155 return false; 156 } 157 158 for (int c1 = 0; c1 < 3; c1++) { 159 for (int c2 = 0; c2 < 3; c2++) { 160 double v = 0; 161 for (int i = 0; i < 3; i++) { 162 v += arg[i + 3 * c1] * arg[i + 3 * c2]; 163 } 164 double expected = (c1 == c2) ? 1 : 0; 165 if (fabs(expected - v) > kTolerance) { 166 *result_listener << "Columns " << c1 << " and " << c2 167 << " should have dot product " << expected 168 << " but have " << v; 169 return false; 170 } 171 } 172 } 173 174 return true; 175 } 176 177 // Use as: 178 // double matrix1[9]; 179 // double matrix2[9]; 180 // EXPECT_THAT(matrix1, IsNear3x3Matrix(matrix2)); 181 MATCHER_P(IsNear3x3Matrix, expected, "") { 182 if (arg == NULL) { 183 *result_listener << "Null matrix"; 184 return false; 185 } 186 187 for (int i = 0; i < 9; i++) { 188 if (fabs(arg[i] - expected[i]) > kTolerance) { 189 *result_listener << "component " << i << " should be " << expected[i]; 190 return false; 191 } 192 } 193 194 return true; 195 } 196 197 // Transforms a zero axis/angle to a quaternion. 198 TEST(Rotation, ZeroAngleAxisToQuaternion) { 199 double axis_angle[3] = { 0, 0, 0 }; 200 double quaternion[4]; 201 double expected[4] = { 1, 0, 0, 0 }; 202 AngleAxisToQuaternion(axis_angle, quaternion); 203 EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 204 EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 205 } 206 207 // Test that exact conversion works for small angles. 208 TEST(Rotation, SmallAngleAxisToQuaternion) { 209 // Small, finite value to test. 210 double theta = 1.0e-2; 211 double axis_angle[3] = { theta, 0, 0 }; 212 double quaternion[4]; 213 double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 214 AngleAxisToQuaternion(axis_angle, quaternion); 215 EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 216 EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 217 } 218 219 // Test that approximate conversion works for very small angles. 220 TEST(Rotation, TinyAngleAxisToQuaternion) { 221 // Very small value that could potentially cause underflow. 222 double theta = pow(numeric_limits<double>::min(), 0.75); 223 double axis_angle[3] = { theta, 0, 0 }; 224 double quaternion[4]; 225 double expected[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 226 AngleAxisToQuaternion(axis_angle, quaternion); 227 EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 228 EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 229 } 230 231 // Transforms a rotation by pi/2 around X to a quaternion. 232 TEST(Rotation, XRotationToQuaternion) { 233 double axis_angle[3] = { kPi / 2, 0, 0 }; 234 double quaternion[4]; 235 double expected[4] = { kHalfSqrt2, kHalfSqrt2, 0, 0 }; 236 AngleAxisToQuaternion(axis_angle, quaternion); 237 EXPECT_THAT(quaternion, IsNormalizedQuaternion()); 238 EXPECT_THAT(quaternion, IsNearQuaternion(expected)); 239 } 240 241 // Transforms a unit quaternion to an axis angle. 242 TEST(Rotation, UnitQuaternionToAngleAxis) { 243 double quaternion[4] = { 1, 0, 0, 0 }; 244 double axis_angle[3]; 245 double expected[3] = { 0, 0, 0 }; 246 QuaternionToAngleAxis(quaternion, axis_angle); 247 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 248 } 249 250 // Transforms a quaternion that rotates by pi about the Y axis to an axis angle. 251 TEST(Rotation, YRotationQuaternionToAngleAxis) { 252 double quaternion[4] = { 0, 0, 1, 0 }; 253 double axis_angle[3]; 254 double expected[3] = { 0, kPi, 0 }; 255 QuaternionToAngleAxis(quaternion, axis_angle); 256 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 257 } 258 259 // Transforms a quaternion that rotates by pi/3 about the Z axis to an axis 260 // angle. 261 TEST(Rotation, ZRotationQuaternionToAngleAxis) { 262 double quaternion[4] = { sqrt(3) / 2, 0, 0, 0.5 }; 263 double axis_angle[3]; 264 double expected[3] = { 0, 0, kPi / 3 }; 265 QuaternionToAngleAxis(quaternion, axis_angle); 266 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 267 } 268 269 // Test that exact conversion works for small angles. 270 TEST(Rotation, SmallQuaternionToAngleAxis) { 271 // Small, finite value to test. 272 double theta = 1.0e-2; 273 double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 274 double axis_angle[3]; 275 double expected[3] = { theta, 0, 0 }; 276 QuaternionToAngleAxis(quaternion, axis_angle); 277 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 278 } 279 280 // Test that approximate conversion works for very small angles. 281 TEST(Rotation, TinyQuaternionToAngleAxis) { 282 // Very small value that could potentially cause underflow. 283 double theta = pow(numeric_limits<double>::min(), 0.75); 284 double quaternion[4] = { cos(theta/2), sin(theta/2.0), 0, 0 }; 285 double axis_angle[3]; 286 double expected[3] = { theta, 0, 0 }; 287 QuaternionToAngleAxis(quaternion, axis_angle); 288 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); 289 } 290 291 TEST(Rotation, QuaternionToAngleAxisAngleIsLessThanPi) { 292 double quaternion[4]; 293 double angle_axis[3]; 294 295 const double half_theta = 0.75 * kPi; 296 297 quaternion[0] = cos(half_theta); 298 quaternion[1] = 1.0 * sin(half_theta); 299 quaternion[2] = 0.0; 300 quaternion[3] = 0.0; 301 QuaternionToAngleAxis(quaternion, angle_axis); 302 const double angle = sqrt(angle_axis[0] * angle_axis[0] + 303 angle_axis[1] * angle_axis[1] + 304 angle_axis[2] * angle_axis[2]); 305 EXPECT_LE(angle, kPi); 306 } 307 308 static const int kNumTrials = 10000; 309 310 // Takes a bunch of random axis/angle values, converts them to quaternions, 311 // and back again. 312 TEST(Rotation, AngleAxisToQuaterionAndBack) { 313 srand(5); 314 for (int i = 0; i < kNumTrials; i++) { 315 double axis_angle[3]; 316 // Make an axis by choosing three random numbers in [-1, 1) and 317 // normalizing. 318 double norm = 0; 319 for (int i = 0; i < 3; i++) { 320 axis_angle[i] = RandDouble() * 2 - 1; 321 norm += axis_angle[i] * axis_angle[i]; 322 } 323 norm = sqrt(norm); 324 325 // Angle in [-pi, pi). 326 double theta = kPi * 2 * RandDouble() - kPi; 327 for (int i = 0; i < 3; i++) { 328 axis_angle[i] = axis_angle[i] * theta / norm; 329 } 330 331 double quaternion[4]; 332 double round_trip[3]; 333 // We use ASSERTs here because if there's one failure, there are 334 // probably many and spewing a million failures doesn't make anyone's 335 // day. 336 AngleAxisToQuaternion(axis_angle, quaternion); 337 ASSERT_THAT(quaternion, IsNormalizedQuaternion()); 338 QuaternionToAngleAxis(quaternion, round_trip); 339 ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 340 } 341 } 342 343 // Takes a bunch of random quaternions, converts them to axis/angle, 344 // and back again. 345 TEST(Rotation, QuaterionToAngleAxisAndBack) { 346 srand(5); 347 for (int i = 0; i < kNumTrials; i++) { 348 double quaternion[4]; 349 // Choose four random numbers in [-1, 1) and normalize. 350 double norm = 0; 351 for (int i = 0; i < 4; i++) { 352 quaternion[i] = RandDouble() * 2 - 1; 353 norm += quaternion[i] * quaternion[i]; 354 } 355 norm = sqrt(norm); 356 357 for (int i = 0; i < 4; i++) { 358 quaternion[i] = quaternion[i] / norm; 359 } 360 361 double axis_angle[3]; 362 double round_trip[4]; 363 QuaternionToAngleAxis(quaternion, axis_angle); 364 AngleAxisToQuaternion(axis_angle, round_trip); 365 ASSERT_THAT(round_trip, IsNormalizedQuaternion()); 366 ASSERT_THAT(round_trip, IsNearQuaternion(quaternion)); 367 } 368 } 369 370 // Transforms a zero axis/angle to a rotation matrix. 371 TEST(Rotation, ZeroAngleAxisToRotationMatrix) { 372 double axis_angle[3] = { 0, 0, 0 }; 373 double matrix[9]; 374 double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 375 AngleAxisToRotationMatrix(axis_angle, matrix); 376 EXPECT_THAT(matrix, IsOrthonormal()); 377 EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 378 } 379 380 TEST(Rotation, NearZeroAngleAxisToRotationMatrix) { 381 double axis_angle[3] = { 1e-24, 2e-24, 3e-24 }; 382 double matrix[9]; 383 double expected[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 384 AngleAxisToRotationMatrix(axis_angle, matrix); 385 EXPECT_THAT(matrix, IsOrthonormal()); 386 EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 387 } 388 389 // Transforms a rotation by pi/2 around X to a rotation matrix and back. 390 TEST(Rotation, XRotationToRotationMatrix) { 391 double axis_angle[3] = { kPi / 2, 0, 0 }; 392 double matrix[9]; 393 // The rotation matrices are stored column-major. 394 double expected[9] = { 1, 0, 0, 0, 0, 1, 0, -1, 0 }; 395 AngleAxisToRotationMatrix(axis_angle, matrix); 396 EXPECT_THAT(matrix, IsOrthonormal()); 397 EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 398 double round_trip[3]; 399 RotationMatrixToAngleAxis(matrix, round_trip); 400 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 401 } 402 403 // Transforms an axis angle that rotates by pi about the Y axis to a 404 // rotation matrix and back. 405 TEST(Rotation, YRotationToRotationMatrix) { 406 double axis_angle[3] = { 0, kPi, 0 }; 407 double matrix[9]; 408 double expected[9] = { -1, 0, 0, 0, 1, 0, 0, 0, -1 }; 409 AngleAxisToRotationMatrix(axis_angle, matrix); 410 EXPECT_THAT(matrix, IsOrthonormal()); 411 EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 412 413 double round_trip[3]; 414 RotationMatrixToAngleAxis(matrix, round_trip); 415 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 416 } 417 418 TEST(Rotation, NearPiAngleAxisRoundTrip) { 419 double in_axis_angle[3]; 420 double matrix[9]; 421 double out_axis_angle[3]; 422 423 srand(5); 424 for (int i = 0; i < kNumTrials; i++) { 425 // Make an axis by choosing three random numbers in [-1, 1) and 426 // normalizing. 427 double norm = 0; 428 for (int i = 0; i < 3; i++) { 429 in_axis_angle[i] = RandDouble() * 2 - 1; 430 norm += in_axis_angle[i] * in_axis_angle[i]; 431 } 432 norm = sqrt(norm); 433 434 // Angle in [pi - kMaxSmallAngle, pi). 435 const double kMaxSmallAngle = 1e-2; 436 double theta = kPi - kMaxSmallAngle * RandDouble(); 437 438 for (int i = 0; i < 3; i++) { 439 in_axis_angle[i] *= (theta / norm); 440 } 441 AngleAxisToRotationMatrix(in_axis_angle, matrix); 442 RotationMatrixToAngleAxis(matrix, out_axis_angle); 443 444 for (int i = 0; i < 3; ++i) { 445 EXPECT_NEAR(out_axis_angle[i], in_axis_angle[i], kLooseTolerance); 446 } 447 } 448 } 449 450 TEST(Rotation, AtPiAngleAxisRoundTrip) { 451 // A rotation of kPi about the X axis; 452 static const double kMatrix[3][3] = { 453 {1.0, 0.0, 0.0}, 454 {0.0, -1.0, 0.0}, 455 {0.0, 0.0, -1.0} 456 }; 457 458 double in_matrix[9]; 459 // Fill it from kMatrix in col-major order. 460 for (int j = 0, k = 0; j < 3; ++j) { 461 for (int i = 0; i < 3; ++i, ++k) { 462 in_matrix[k] = kMatrix[i][j]; 463 } 464 } 465 466 const double expected_axis_angle[3] = { kPi, 0, 0 }; 467 468 double out_matrix[9]; 469 double axis_angle[3]; 470 RotationMatrixToAngleAxis(in_matrix, axis_angle); 471 AngleAxisToRotationMatrix(axis_angle, out_matrix); 472 473 LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1] 474 << " " << axis_angle[2]; 475 LOG(INFO) << "Expected AngleAxis = " << kPi << " 0 0"; 476 double out_rowmajor[3][3]; 477 for (int j = 0, k = 0; j < 3; ++j) { 478 for (int i = 0; i < 3; ++i, ++k) { 479 out_rowmajor[i][j] = out_matrix[k]; 480 } 481 } 482 LOG(INFO) << "Rotation:"; 483 LOG(INFO) << "EXPECTED | ACTUAL"; 484 for (int i = 0; i < 3; ++i) { 485 string line; 486 for (int j = 0; j < 3; ++j) { 487 StringAppendF(&line, "%g ", kMatrix[i][j]); 488 } 489 line += " | "; 490 for (int j = 0; j < 3; ++j) { 491 StringAppendF(&line, "%g ", out_rowmajor[i][j]); 492 } 493 LOG(INFO) << line; 494 } 495 496 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected_axis_angle)); 497 EXPECT_THAT(out_matrix, IsNear3x3Matrix(in_matrix)); 498 } 499 500 // Transforms an axis angle that rotates by pi/3 about the Z axis to a 501 // rotation matrix. 502 TEST(Rotation, ZRotationToRotationMatrix) { 503 double axis_angle[3] = { 0, 0, kPi / 3 }; 504 double matrix[9]; 505 // This is laid-out row-major on the screen but is actually stored 506 // column-major. 507 double expected[9] = { 0.5, sqrt(3) / 2, 0, // Column 1 508 -sqrt(3) / 2, 0.5, 0, // Column 2 509 0, 0, 1 }; // Column 3 510 AngleAxisToRotationMatrix(axis_angle, matrix); 511 EXPECT_THAT(matrix, IsOrthonormal()); 512 EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); 513 double round_trip[3]; 514 RotationMatrixToAngleAxis(matrix, round_trip); 515 EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); 516 } 517 518 // Takes a bunch of random axis/angle values, converts them to rotation 519 // matrices, and back again. 520 TEST(Rotation, AngleAxisToRotationMatrixAndBack) { 521 srand(5); 522 for (int i = 0; i < kNumTrials; i++) { 523 double axis_angle[3]; 524 // Make an axis by choosing three random numbers in [-1, 1) and 525 // normalizing. 526 double norm = 0; 527 for (int i = 0; i < 3; i++) { 528 axis_angle[i] = RandDouble() * 2 - 1; 529 norm += axis_angle[i] * axis_angle[i]; 530 } 531 norm = sqrt(norm); 532 533 // Angle in [-pi, pi). 534 double theta = kPi * 2 * RandDouble() - kPi; 535 for (int i = 0; i < 3; i++) { 536 axis_angle[i] = axis_angle[i] * theta / norm; 537 } 538 539 double matrix[9]; 540 double round_trip[3]; 541 AngleAxisToRotationMatrix(axis_angle, matrix); 542 ASSERT_THAT(matrix, IsOrthonormal()); 543 RotationMatrixToAngleAxis(matrix, round_trip); 544 545 for (int i = 0; i < 3; ++i) { 546 EXPECT_NEAR(round_trip[i], axis_angle[i], kLooseTolerance); 547 } 548 } 549 } 550 551 // Transposes a 3x3 matrix. 552 static void Transpose3x3(double m[9]) { 553 std::swap(m[1], m[3]); 554 std::swap(m[2], m[6]); 555 std::swap(m[5], m[7]); 556 } 557 558 // Convert Euler angles from radians to degrees. 559 static void ToDegrees(double ea[3]) { 560 for (int i = 0; i < 3; ++i) 561 ea[i] *= 180.0 / kPi; 562 } 563 564 // Compare the 3x3 rotation matrices produced by the axis-angle 565 // rotation 'aa' and the Euler angle rotation 'ea' (in radians). 566 static void CompareEulerToAngleAxis(double aa[3], double ea[3]) { 567 double aa_matrix[9]; 568 AngleAxisToRotationMatrix(aa, aa_matrix); 569 Transpose3x3(aa_matrix); // Column to row major order. 570 571 double ea_matrix[9]; 572 ToDegrees(ea); // Radians to degrees. 573 const int kRowStride = 3; 574 EulerAnglesToRotationMatrix(ea, kRowStride, ea_matrix); 575 576 EXPECT_THAT(aa_matrix, IsOrthonormal()); 577 EXPECT_THAT(ea_matrix, IsOrthonormal()); 578 EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix)); 579 } 580 581 // Test with rotation axis along the x/y/z axes. 582 // Also test zero rotation. 583 TEST(EulerAnglesToRotationMatrix, OnAxis) { 584 int n_tests = 0; 585 for (double x = -1.0; x <= 1.0; x += 1.0) { 586 for (double y = -1.0; y <= 1.0; y += 1.0) { 587 for (double z = -1.0; z <= 1.0; z += 1.0) { 588 if ((x != 0) + (y != 0) + (z != 0) > 1) 589 continue; 590 double axis_angle[3] = {x, y, z}; 591 double euler_angles[3] = {x, y, z}; 592 CompareEulerToAngleAxis(axis_angle, euler_angles); 593 ++n_tests; 594 } 595 } 596 } 597 CHECK_EQ(7, n_tests); 598 } 599 600 // Test that a random rotation produces an orthonormal rotation 601 // matrix. 602 TEST(EulerAnglesToRotationMatrix, IsOrthonormal) { 603 srand(5); 604 for (int trial = 0; trial < kNumTrials; ++trial) { 605 double ea[3]; 606 for (int i = 0; i < 3; ++i) 607 ea[i] = 360.0 * (RandDouble() * 2.0 - 1.0); 608 double ea_matrix[9]; 609 ToDegrees(ea); // Radians to degrees. 610 EulerAnglesToRotationMatrix(ea, 3, ea_matrix); 611 EXPECT_THAT(ea_matrix, IsOrthonormal()); 612 } 613 } 614 615 // Tests using Jets for specific behavior involving auto differentiation 616 // near singularity points. 617 618 typedef Jet<double, 3> J3; 619 typedef Jet<double, 4> J4; 620 621 J3 MakeJ3(double a, double v0, double v1, double v2) { 622 J3 j; 623 j.a = a; 624 j.v[0] = v0; 625 j.v[1] = v1; 626 j.v[2] = v2; 627 return j; 628 } 629 630 J4 MakeJ4(double a, double v0, double v1, double v2, double v3) { 631 J4 j; 632 j.a = a; 633 j.v[0] = v0; 634 j.v[1] = v1; 635 j.v[2] = v2; 636 j.v[3] = v3; 637 return j; 638 } 639 640 641 bool IsClose(double x, double y) { 642 EXPECT_FALSE(IsNaN(x)); 643 EXPECT_FALSE(IsNaN(y)); 644 double absdiff = fabs(x - y); 645 if (x == 0 || y == 0) { 646 return absdiff <= kTolerance; 647 } 648 double reldiff = absdiff / max(fabs(x), fabs(y)); 649 return reldiff <= kTolerance; 650 } 651 652 template <int N> 653 bool IsClose(const Jet<double, N> &x, const Jet<double, N> &y) { 654 if (IsClose(x.a, y.a)) { 655 for (int i = 0; i < N; i++) { 656 if (!IsClose(x.v[i], y.v[i])) { 657 return false; 658 } 659 } 660 } 661 return true; 662 } 663 664 template <int M, int N> 665 void ExpectJetArraysClose(const Jet<double, N> *x, const Jet<double, N> *y) { 666 for (int i = 0; i < M; i++) { 667 if (!IsClose(x[i], y[i])) { 668 LOG(ERROR) << "Jet " << i << "/" << M << " not equal"; 669 LOG(ERROR) << "x[" << i << "]: " << x[i]; 670 LOG(ERROR) << "y[" << i << "]: " << y[i]; 671 Jet<double, N> d, zero; 672 d.a = y[i].a - x[i].a; 673 for (int j = 0; j < N; j++) { 674 d.v[j] = y[i].v[j] - x[i].v[j]; 675 } 676 LOG(ERROR) << "diff: " << d; 677 EXPECT_TRUE(IsClose(x[i], y[i])); 678 } 679 } 680 } 681 682 // Log-10 of a value well below machine precision. 683 static const int kSmallTinyCutoff = 684 static_cast<int>(2 * log(numeric_limits<double>::epsilon())/log(10.0)); 685 686 // Log-10 of a value just below values representable by double. 687 static const int kTinyZeroLimit = 688 static_cast<int>(1 + log(numeric_limits<double>::min())/log(10.0)); 689 690 // Test that exact conversion works for small angles when jets are used. 691 TEST(Rotation, SmallAngleAxisToQuaternionForJets) { 692 // Examine small x rotations that are still large enough 693 // to be well within the range represented by doubles. 694 for (int i = -2; i >= kSmallTinyCutoff; i--) { 695 double theta = pow(10.0, i); 696 J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) }; 697 J3 quaternion[4]; 698 J3 expected[4] = { 699 MakeJ3(cos(theta/2), -sin(theta/2)/2, 0, 0), 700 MakeJ3(sin(theta/2), cos(theta/2)/2, 0, 0), 701 MakeJ3(0, 0, sin(theta/2)/theta, 0), 702 MakeJ3(0, 0, 0, sin(theta/2)/theta), 703 }; 704 AngleAxisToQuaternion(axis_angle, quaternion); 705 ExpectJetArraysClose<4, 3>(quaternion, expected); 706 } 707 } 708 709 710 // Test that conversion works for very small angles when jets are used. 711 TEST(Rotation, TinyAngleAxisToQuaternionForJets) { 712 // Examine tiny x rotations that extend all the way to where 713 // underflow occurs. 714 for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { 715 double theta = pow(10.0, i); 716 J3 axis_angle[3] = { J3(theta, 0), J3(0, 1), J3(0, 2) }; 717 J3 quaternion[4]; 718 // To avoid loss of precision in the test itself, 719 // a finite expansion is used here, which will 720 // be exact up to machine precision for the test values used. 721 J3 expected[4] = { 722 MakeJ3(1.0, 0, 0, 0), 723 MakeJ3(0, 0.5, 0, 0), 724 MakeJ3(0, 0, 0.5, 0), 725 MakeJ3(0, 0, 0, 0.5), 726 }; 727 AngleAxisToQuaternion(axis_angle, quaternion); 728 ExpectJetArraysClose<4, 3>(quaternion, expected); 729 } 730 } 731 732 // Test that derivatives are correct for zero rotation. 733 TEST(Rotation, ZeroAngleAxisToQuaternionForJets) { 734 J3 axis_angle[3] = { J3(0, 0), J3(0, 1), J3(0, 2) }; 735 J3 quaternion[4]; 736 J3 expected[4] = { 737 MakeJ3(1.0, 0, 0, 0), 738 MakeJ3(0, 0.5, 0, 0), 739 MakeJ3(0, 0, 0.5, 0), 740 MakeJ3(0, 0, 0, 0.5), 741 }; 742 AngleAxisToQuaternion(axis_angle, quaternion); 743 ExpectJetArraysClose<4, 3>(quaternion, expected); 744 } 745 746 // Test that exact conversion works for small angles. 747 TEST(Rotation, SmallQuaternionToAngleAxisForJets) { 748 // Examine small x rotations that are still large enough 749 // to be well within the range represented by doubles. 750 for (int i = -2; i >= kSmallTinyCutoff; i--) { 751 double theta = pow(10.0, i); 752 double s = sin(theta); 753 double c = cos(theta); 754 J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) }; 755 J4 axis_angle[3]; 756 J4 expected[3] = { 757 MakeJ4(s, -2*theta, 2*theta*c, 0, 0), 758 MakeJ4(0, 0, 0, 2*theta/s, 0), 759 MakeJ4(0, 0, 0, 0, 2*theta/s), 760 }; 761 QuaternionToAngleAxis(quaternion, axis_angle); 762 ExpectJetArraysClose<3, 4>(axis_angle, expected); 763 } 764 } 765 766 // Test that conversion works for very small angles. 767 TEST(Rotation, TinyQuaternionToAngleAxisForJets) { 768 // Examine tiny x rotations that extend all the way to where 769 // underflow occurs. 770 for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { 771 double theta = pow(10.0, i); 772 double s = sin(theta); 773 double c = cos(theta); 774 J4 quaternion[4] = { J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3) }; 775 J4 axis_angle[3]; 776 // To avoid loss of precision in the test itself, 777 // a finite expansion is used here, which will 778 // be exact up to machine precision for the test values used. 779 J4 expected[3] = { 780 MakeJ4(theta, -2*theta, 2.0, 0, 0), 781 MakeJ4(0, 0, 0, 2.0, 0), 782 MakeJ4(0, 0, 0, 0, 2.0), 783 }; 784 QuaternionToAngleAxis(quaternion, axis_angle); 785 ExpectJetArraysClose<3, 4>(axis_angle, expected); 786 } 787 } 788 789 // Test that conversion works for no rotation. 790 TEST(Rotation, ZeroQuaternionToAngleAxisForJets) { 791 J4 quaternion[4] = { J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3) }; 792 J4 axis_angle[3]; 793 J4 expected[3] = { 794 MakeJ4(0, 0, 2.0, 0, 0), 795 MakeJ4(0, 0, 0, 2.0, 0), 796 MakeJ4(0, 0, 0, 0, 2.0), 797 }; 798 QuaternionToAngleAxis(quaternion, axis_angle); 799 ExpectJetArraysClose<3, 4>(axis_angle, expected); 800 } 801 802 TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) { 803 // Canned data generated in octave. 804 double const q[4] = { 805 +0.1956830471754074, 806 -0.0150618562474847, 807 +0.7634572982788086, 808 -0.3019454777240753, 809 }; 810 double const Q[3][3] = { // Scaled rotation matrix. 811 { -0.6355194033477252, 0.0951730541682254, 0.3078870197911186 }, 812 { -0.1411693904792992, 0.5297609702153905, -0.4551502574482019 }, 813 { -0.2896955822708862, -0.4669396571547050, -0.4536309793389248 }, 814 }; 815 double const R[3][3] = { // With unit rows and columns. 816 { -0.8918859164053080, 0.1335655625725649, 0.4320876677394745 }, 817 { -0.1981166751680096, 0.7434648665444399, -0.6387564287225856 }, 818 { -0.4065578619806013, -0.6553016349046693, -0.6366242786393164 }, 819 }; 820 821 // Compute R from q and compare to known answer. 822 double Rq[3][3]; 823 QuaternionToScaledRotation<double>(q, Rq[0]); 824 ExpectArraysClose(9, Q[0], Rq[0], kTolerance); 825 826 // Now do the same but compute R with normalization. 827 QuaternionToRotation<double>(q, Rq[0]); 828 ExpectArraysClose(9, R[0], Rq[0], kTolerance); 829 } 830 831 832 TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) { 833 // Rotation defined by a unit quaternion. 834 double const q[4] = { 835 0.2318160216097109, 836 -0.0178430356832060, 837 0.9044300776717159, 838 -0.3576998641394597, 839 }; 840 double const p[3] = { 841 +0.11, 842 -13.15, 843 1.17, 844 }; 845 846 double R[3 * 3]; 847 QuaternionToRotation(q, R); 848 849 double result1[3]; 850 UnitQuaternionRotatePoint(q, p, result1); 851 852 double result2[3]; 853 VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3)* ConstVectorRef(p, 3); 854 ExpectArraysClose(3, result1, result2, kTolerance); 855 } 856 857 858 // Verify that (a * b) * c == a * (b * c). 859 TEST(Quaternion, MultiplicationIsAssociative) { 860 double a[4]; 861 double b[4]; 862 double c[4]; 863 for (int i = 0; i < 4; ++i) { 864 a[i] = 2 * RandDouble() - 1; 865 b[i] = 2 * RandDouble() - 1; 866 c[i] = 2 * RandDouble() - 1; 867 } 868 869 double ab[4]; 870 double ab_c[4]; 871 QuaternionProduct(a, b, ab); 872 QuaternionProduct(ab, c, ab_c); 873 874 double bc[4]; 875 double a_bc[4]; 876 QuaternionProduct(b, c, bc); 877 QuaternionProduct(a, bc, a_bc); 878 879 ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance); 880 ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance); 881 ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance); 882 ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance); 883 } 884 885 886 TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { 887 double angle_axis[3]; 888 double R[9]; 889 double p[3]; 890 double angle_axis_rotated_p[3]; 891 double rotation_matrix_rotated_p[3]; 892 893 for (int i = 0; i < 10000; ++i) { 894 double theta = (2.0 * i * 0.0011 - 1.0) * kPi; 895 for (int j = 0; j < 50; ++j) { 896 double norm2 = 0.0; 897 for (int k = 0; k < 3; ++k) { 898 angle_axis[k] = 2.0 * RandDouble() - 1.0; 899 p[k] = 2.0 * RandDouble() - 1.0; 900 norm2 = angle_axis[k] * angle_axis[k]; 901 } 902 903 const double inv_norm = theta / sqrt(norm2); 904 for (int k = 0; k < 3; ++k) { 905 angle_axis[k] *= inv_norm; 906 } 907 908 AngleAxisToRotationMatrix(angle_axis, R); 909 rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; 910 rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; 911 rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; 912 913 AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); 914 for (int k = 0; k < 3; ++k) { 915 EXPECT_NEAR(rotation_matrix_rotated_p[k], 916 angle_axis_rotated_p[k], 917 kTolerance) << "p: " << p[0] 918 << " " << p[1] 919 << " " << p[2] 920 << " angle_axis: " << angle_axis[0] 921 << " " << angle_axis[1] 922 << " " << angle_axis[2]; 923 } 924 } 925 } 926 } 927 928 TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) { 929 double angle_axis[3]; 930 double R[9]; 931 double p[3]; 932 double angle_axis_rotated_p[3]; 933 double rotation_matrix_rotated_p[3]; 934 935 for (int i = 0; i < 10000; ++i) { 936 double norm2 = 0.0; 937 for (int k = 0; k < 3; ++k) { 938 angle_axis[k] = 2.0 * RandDouble() - 1.0; 939 p[k] = 2.0 * RandDouble() - 1.0; 940 norm2 = angle_axis[k] * angle_axis[k]; 941 } 942 943 double theta = (2.0 * i * 0.0001 - 1.0) * 1e-16; 944 const double inv_norm = theta / sqrt(norm2); 945 for (int k = 0; k < 3; ++k) { 946 angle_axis[k] *= inv_norm; 947 } 948 949 AngleAxisToRotationMatrix(angle_axis, R); 950 rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; 951 rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; 952 rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; 953 954 AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); 955 for (int k = 0; k < 3; ++k) { 956 EXPECT_NEAR(rotation_matrix_rotated_p[k], 957 angle_axis_rotated_p[k], 958 kTolerance) << "p: " << p[0] 959 << " " << p[1] 960 << " " << p[2] 961 << " angle_axis: " << angle_axis[0] 962 << " " << angle_axis[1] 963 << " " << angle_axis[2]; 964 } 965 } 966 } 967 968 TEST(MatrixAdapter, RowMajor3x3ReturnTypeAndAccessIsCorrect) { 969 double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 }; 970 const float const_array[9] = 971 { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f }; 972 MatrixAdapter<double, 3, 1> A = RowMajorAdapter3x3(array); 973 MatrixAdapter<const float, 3, 1> B = RowMajorAdapter3x3(const_array); 974 975 for (int i = 0; i < 3; ++i) { 976 for (int j = 0; j < 3; ++j) { 977 // The values are integers from 1 to 9, so equality tests are appropriate 978 // even for float and double values. 979 EXPECT_EQ(A(i, j), array[3*i+j]); 980 EXPECT_EQ(B(i, j), const_array[3*i+j]); 981 } 982 } 983 } 984 985 TEST(MatrixAdapter, ColumnMajor3x3ReturnTypeAndAccessIsCorrect) { 986 double array[9] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 }; 987 const float const_array[9] = 988 { 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f }; 989 MatrixAdapter<double, 1, 3> A = ColumnMajorAdapter3x3(array); 990 MatrixAdapter<const float, 1, 3> B = ColumnMajorAdapter3x3(const_array); 991 992 for (int i = 0; i < 3; ++i) { 993 for (int j = 0; j < 3; ++j) { 994 // The values are integers from 1 to 9, so equality tests are 995 // appropriate even for float and double values. 996 EXPECT_EQ(A(i, j), array[3*j+i]); 997 EXPECT_EQ(B(i, j), const_array[3*j+i]); 998 } 999 } 1000 } 1001 1002 TEST(MatrixAdapter, RowMajor2x4IsCorrect) { 1003 const int expected[8] = { 1, 2, 3, 4, 5, 6, 7, 8 }; 1004 int array[8]; 1005 MatrixAdapter<int, 4, 1> M(array); 1006 M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; 1007 M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; 1008 for (int k = 0; k < 8; ++k) { 1009 EXPECT_EQ(array[k], expected[k]); 1010 } 1011 } 1012 1013 TEST(MatrixAdapter, ColumnMajor2x4IsCorrect) { 1014 const int expected[8] = { 1, 5, 2, 6, 3, 7, 4, 8 }; 1015 int array[8]; 1016 MatrixAdapter<int, 1, 2> M(array); 1017 M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; 1018 M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; 1019 for (int k = 0; k < 8; ++k) { 1020 EXPECT_EQ(array[k], expected[k]); 1021 } 1022 } 1023 1024 } // namespace internal 1025 } // namespace ceres 1026
