Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1 | // Ceres Solver - A fast non-linear least squares minimizer |
Sergiu Deitsch | cb6b306 | 2023-01-05 21:37:36 +0100 | [diff] [blame] | 2 | // Copyright 2023 Google Inc. All rights reserved. |
Keir Mierle | 7492b0d | 2015-03-17 22:30:16 -0700 | [diff] [blame] | 3 | // http://ceres-solver.org/ |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 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@google.com (Sameer Agarwal) |
| 30 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 31 | #include "ceres/rotation.h" |
| 32 | |
Sameer Agarwal | 749a442 | 2023-01-16 07:38:05 -0800 | [diff] [blame] | 33 | #include <algorithm> |
Sameer Agarwal | 79a554f | 2023-01-13 11:37:27 -0800 | [diff] [blame] | 34 | #include <array> |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 35 | #include <cmath> |
| 36 | #include <limits> |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 37 | #include <random> |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 38 | #include <string> |
Sameer Agarwal | d340f81 | 2023-04-10 16:43:30 -0700 | [diff] [blame] | 39 | #include <utility> |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 40 | |
Sameer Agarwal | 0a53aa9 | 2024-07-07 10:24:18 -0700 | [diff] [blame] | 41 | #include "absl/log/log.h" |
hs293go | 2b89ce6 | 2021-11-23 15:39:14 -0500 | [diff] [blame] | 42 | #include "ceres/constants.h" |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 43 | #include "ceres/internal/eigen.h" |
hs293go | 2b89ce6 | 2021-11-23 15:39:14 -0500 | [diff] [blame] | 44 | #include "ceres/internal/euler_angles.h" |
Sergiu Deitsch | f90833f | 2022-02-07 23:43:19 +0100 | [diff] [blame] | 45 | #include "ceres/internal/export.h" |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 46 | #include "ceres/is_close.h" |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 47 | #include "ceres/jet.h" |
Sameer Agarwal | 0beab86 | 2012-08-13 15:12:01 -0700 | [diff] [blame] | 48 | #include "ceres/stringprintf.h" |
| 49 | #include "ceres/test_util.h" |
Sameer Agarwal | 0beab86 | 2012-08-13 15:12:01 -0700 | [diff] [blame] | 50 | #include "gmock/gmock.h" |
| 51 | #include "gtest/gtest.h" |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 52 | |
| 53 | namespace ceres { |
| 54 | namespace internal { |
| 55 | |
Sergiu Deitsch | 4519b8d | 2023-10-04 22:45:42 +0200 | [diff] [blame] | 56 | inline constexpr double kPi = constants::pi; |
Keir Mierle | efe7ac6 | 2012-06-24 22:25:28 -0700 | [diff] [blame] | 57 | const double kHalfSqrt2 = 0.707106781186547524401; |
| 58 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 59 | // A tolerance value for floating-point comparisons. |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 60 | static double const kTolerance = std::numeric_limits<double>::epsilon() * 10; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 61 | |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 62 | // Looser tolerance used for numerically unstable conversions. |
Keir Mierle | efe7ac6 | 2012-06-24 22:25:28 -0700 | [diff] [blame] | 63 | static double const kLooseTolerance = 1e-9; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 64 | |
| 65 | // Use as: |
| 66 | // double quaternion[4]; |
| 67 | // EXPECT_THAT(quaternion, IsNormalizedQuaternion()); |
| 68 | MATCHER(IsNormalizedQuaternion, "") { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 69 | double norm2 = |
| 70 | arg[0] * arg[0] + arg[1] * arg[1] + arg[2] * arg[2] + arg[3] * arg[3]; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 71 | if (fabs(norm2 - 1.0) > kTolerance) { |
| 72 | *result_listener << "squared norm is " << norm2; |
| 73 | return false; |
| 74 | } |
| 75 | |
| 76 | return true; |
| 77 | } |
| 78 | |
| 79 | // Use as: |
| 80 | // double expected_quaternion[4]; |
| 81 | // double actual_quaternion[4]; |
| 82 | // EXPECT_THAT(actual_quaternion, IsNearQuaternion(expected_quaternion)); |
| 83 | MATCHER_P(IsNearQuaternion, expected, "") { |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 84 | // Quaternions are equivalent upto a sign change. So we will compare |
| 85 | // both signs before declaring failure. |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 86 | bool is_near = true; |
| 87 | // NOTE: near (and far) can be defined as macros on the Windows platform (for |
| 88 | // ancient pascal calling convention). Do not use these identifiers. |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 89 | for (int i = 0; i < 4; i++) { |
| 90 | if (fabs(arg[i] - expected[i]) > kTolerance) { |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 91 | is_near = false; |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 92 | break; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 93 | } |
| 94 | } |
| 95 | |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 96 | if (is_near) { |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 97 | return true; |
| 98 | } |
| 99 | |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 100 | is_near = true; |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 101 | for (int i = 0; i < 4; i++) { |
| 102 | if (fabs(arg[i] + expected[i]) > kTolerance) { |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 103 | is_near = false; |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 104 | break; |
| 105 | } |
| 106 | } |
| 107 | |
Sergiu Deitsch | f085166 | 2022-02-17 00:56:27 +0100 | [diff] [blame] | 108 | if (is_near) { |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 109 | return true; |
| 110 | } |
| 111 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 112 | // clang-format off |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 113 | *result_listener << "expected : " |
| 114 | << expected[0] << " " |
| 115 | << expected[1] << " " |
| 116 | << expected[2] << " " |
| 117 | << expected[3] << " " |
| 118 | << "actual : " |
| 119 | << arg[0] << " " |
| 120 | << arg[1] << " " |
| 121 | << arg[2] << " " |
| 122 | << arg[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 123 | // clang-format on |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 124 | return false; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 125 | } |
| 126 | |
| 127 | // Use as: |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 128 | // double expected_axis_angle[3]; |
| 129 | // double actual_axis_angle[3]; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 130 | // EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle)); |
| 131 | MATCHER_P(IsNearAngleAxis, expected, "") { |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 132 | Eigen::Vector3d a(arg[0], arg[1], arg[2]); |
| 133 | Eigen::Vector3d e(expected[0], expected[1], expected[2]); |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 134 | const double e_norm = e.norm(); |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 135 | |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 136 | double delta_norm = std::numeric_limits<double>::max(); |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 137 | if (e_norm > 0) { |
| 138 | // Deal with the sign ambiguity near PI. Since the sign can flip, |
| 139 | // we take the smaller of the two differences. |
| 140 | if (fabs(e_norm - kPi) < kLooseTolerance) { |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 141 | delta_norm = std::min((a - e).norm(), (a + e).norm()) / e_norm; |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 142 | } else { |
| 143 | delta_norm = (a - e).norm() / e_norm; |
| 144 | } |
| 145 | } else { |
| 146 | delta_norm = a.norm(); |
| 147 | } |
| 148 | |
| 149 | if (delta_norm <= kLooseTolerance) { |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 150 | return true; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 151 | } |
| 152 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 153 | // clang-format off |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 154 | *result_listener << " arg:" |
| 155 | << " " << arg[0] |
| 156 | << " " << arg[1] |
| 157 | << " " << arg[2] |
| 158 | << " was expected to be:" |
| 159 | << " " << expected[0] |
| 160 | << " " << expected[1] |
| 161 | << " " << expected[2]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 162 | // clang-format on |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 163 | return false; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 164 | } |
| 165 | |
| 166 | // Use as: |
| 167 | // double matrix[9]; |
| 168 | // EXPECT_THAT(matrix, IsOrthonormal()); |
| 169 | MATCHER(IsOrthonormal, "") { |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 170 | for (int c1 = 0; c1 < 3; c1++) { |
| 171 | for (int c2 = 0; c2 < 3; c2++) { |
| 172 | double v = 0; |
| 173 | for (int i = 0; i < 3; i++) { |
| 174 | v += arg[i + 3 * c1] * arg[i + 3 * c2]; |
| 175 | } |
| 176 | double expected = (c1 == c2) ? 1 : 0; |
| 177 | if (fabs(expected - v) > kTolerance) { |
| 178 | *result_listener << "Columns " << c1 << " and " << c2 |
| 179 | << " should have dot product " << expected |
| 180 | << " but have " << v; |
| 181 | return false; |
| 182 | } |
| 183 | } |
| 184 | } |
| 185 | |
| 186 | return true; |
| 187 | } |
| 188 | |
| 189 | // Use as: |
| 190 | // double matrix1[9]; |
| 191 | // double matrix2[9]; |
| 192 | // EXPECT_THAT(matrix1, IsNear3x3Matrix(matrix2)); |
| 193 | MATCHER_P(IsNear3x3Matrix, expected, "") { |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 194 | for (int i = 0; i < 9; i++) { |
| 195 | if (fabs(arg[i] - expected[i]) > kTolerance) { |
| 196 | *result_listener << "component " << i << " should be " << expected[i]; |
| 197 | return false; |
| 198 | } |
| 199 | } |
| 200 | |
| 201 | return true; |
| 202 | } |
| 203 | |
| 204 | // Transforms a zero axis/angle to a quaternion. |
| 205 | TEST(Rotation, ZeroAngleAxisToQuaternion) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 206 | double axis_angle[3] = {0, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 207 | double quaternion[4]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 208 | double expected[4] = {1, 0, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 209 | AngleAxisToQuaternion(axis_angle, quaternion); |
| 210 | EXPECT_THAT(quaternion, IsNormalizedQuaternion()); |
| 211 | EXPECT_THAT(quaternion, IsNearQuaternion(expected)); |
| 212 | } |
| 213 | |
| 214 | // Test that exact conversion works for small angles. |
| 215 | TEST(Rotation, SmallAngleAxisToQuaternion) { |
| 216 | // Small, finite value to test. |
| 217 | double theta = 1.0e-2; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 218 | double axis_angle[3] = {theta, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 219 | double quaternion[4]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 220 | double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 221 | AngleAxisToQuaternion(axis_angle, quaternion); |
| 222 | EXPECT_THAT(quaternion, IsNormalizedQuaternion()); |
| 223 | EXPECT_THAT(quaternion, IsNearQuaternion(expected)); |
| 224 | } |
| 225 | |
| 226 | // Test that approximate conversion works for very small angles. |
| 227 | TEST(Rotation, TinyAngleAxisToQuaternion) { |
| 228 | // Very small value that could potentially cause underflow. |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 229 | double theta = pow(std::numeric_limits<double>::min(), 0.75); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 230 | double axis_angle[3] = {theta, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 231 | double quaternion[4]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 232 | double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 233 | AngleAxisToQuaternion(axis_angle, quaternion); |
| 234 | EXPECT_THAT(quaternion, IsNormalizedQuaternion()); |
| 235 | EXPECT_THAT(quaternion, IsNearQuaternion(expected)); |
| 236 | } |
| 237 | |
| 238 | // Transforms a rotation by pi/2 around X to a quaternion. |
| 239 | TEST(Rotation, XRotationToQuaternion) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 240 | double axis_angle[3] = {kPi / 2, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 241 | double quaternion[4]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 242 | double expected[4] = {kHalfSqrt2, kHalfSqrt2, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 243 | AngleAxisToQuaternion(axis_angle, quaternion); |
| 244 | EXPECT_THAT(quaternion, IsNormalizedQuaternion()); |
| 245 | EXPECT_THAT(quaternion, IsNearQuaternion(expected)); |
| 246 | } |
| 247 | |
| 248 | // Transforms a unit quaternion to an axis angle. |
| 249 | TEST(Rotation, UnitQuaternionToAngleAxis) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 250 | double quaternion[4] = {1, 0, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 251 | double axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 252 | double expected[3] = {0, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 253 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 254 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); |
| 255 | } |
| 256 | |
| 257 | // Transforms a quaternion that rotates by pi about the Y axis to an axis angle. |
| 258 | TEST(Rotation, YRotationQuaternionToAngleAxis) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 259 | double quaternion[4] = {0, 0, 1, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 260 | double axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 261 | double expected[3] = {0, kPi, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 262 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 263 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); |
| 264 | } |
| 265 | |
| 266 | // Transforms a quaternion that rotates by pi/3 about the Z axis to an axis |
| 267 | // angle. |
| 268 | TEST(Rotation, ZRotationQuaternionToAngleAxis) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 269 | double quaternion[4] = {sqrt(3) / 2, 0, 0, 0.5}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 270 | double axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 271 | double expected[3] = {0, 0, kPi / 3}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 272 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 273 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); |
| 274 | } |
| 275 | |
| 276 | // Test that exact conversion works for small angles. |
| 277 | TEST(Rotation, SmallQuaternionToAngleAxis) { |
| 278 | // Small, finite value to test. |
| 279 | double theta = 1.0e-2; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 280 | double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 281 | double axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 282 | double expected[3] = {theta, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 283 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 284 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); |
| 285 | } |
| 286 | |
| 287 | // Test that approximate conversion works for very small angles. |
| 288 | TEST(Rotation, TinyQuaternionToAngleAxis) { |
| 289 | // Very small value that could potentially cause underflow. |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 290 | double theta = pow(std::numeric_limits<double>::min(), 0.75); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 291 | double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 292 | double axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 293 | double expected[3] = {theta, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 294 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 295 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected)); |
| 296 | } |
| 297 | |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 298 | TEST(Rotation, QuaternionToAngleAxisAngleIsLessThanPi) { |
| 299 | double quaternion[4]; |
| 300 | double angle_axis[3]; |
| 301 | |
| 302 | const double half_theta = 0.75 * kPi; |
| 303 | |
| 304 | quaternion[0] = cos(half_theta); |
| 305 | quaternion[1] = 1.0 * sin(half_theta); |
| 306 | quaternion[2] = 0.0; |
| 307 | quaternion[3] = 0.0; |
| 308 | QuaternionToAngleAxis(quaternion, angle_axis); |
Sergiu Deitsch | cb6b306 | 2023-01-05 21:37:36 +0100 | [diff] [blame] | 309 | const double angle = std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]); |
Sameer Agarwal | 383c04f | 2012-08-17 10:14:04 -0700 | [diff] [blame] | 310 | EXPECT_LE(angle, kPi); |
| 311 | } |
| 312 | |
Sameer Agarwal | 57cf20a | 2020-04-21 10:10:01 -0700 | [diff] [blame] | 313 | static constexpr int kNumTrials = 10000; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 314 | |
| 315 | // Takes a bunch of random axis/angle values, converts them to quaternions, |
| 316 | // and back again. |
| 317 | TEST(Rotation, AngleAxisToQuaterionAndBack) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 318 | std::mt19937 prng; |
| 319 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 320 | for (int i = 0; i < kNumTrials; i++) { |
| 321 | double axis_angle[3]; |
| 322 | // Make an axis by choosing three random numbers in [-1, 1) and |
| 323 | // normalizing. |
| 324 | double norm = 0; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 325 | for (double& coeff : axis_angle) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 326 | coeff = uniform_distribution(prng); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 327 | norm += coeff * coeff; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 328 | } |
| 329 | norm = sqrt(norm); |
| 330 | |
| 331 | // Angle in [-pi, pi). |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 332 | double theta = uniform_distribution( |
| 333 | prng, std::uniform_real_distribution<double>::param_type{-kPi, kPi}); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 334 | for (double& coeff : axis_angle) { |
| 335 | coeff = coeff * theta / norm; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 336 | } |
| 337 | |
| 338 | double quaternion[4]; |
| 339 | double round_trip[3]; |
| 340 | // We use ASSERTs here because if there's one failure, there are |
| 341 | // probably many and spewing a million failures doesn't make anyone's |
| 342 | // day. |
| 343 | AngleAxisToQuaternion(axis_angle, quaternion); |
| 344 | ASSERT_THAT(quaternion, IsNormalizedQuaternion()); |
| 345 | QuaternionToAngleAxis(quaternion, round_trip); |
| 346 | ASSERT_THAT(round_trip, IsNearAngleAxis(axis_angle)); |
| 347 | } |
| 348 | } |
| 349 | |
| 350 | // Takes a bunch of random quaternions, converts them to axis/angle, |
| 351 | // and back again. |
| 352 | TEST(Rotation, QuaterionToAngleAxisAndBack) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 353 | std::mt19937 prng; |
| 354 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 355 | for (int i = 0; i < kNumTrials; i++) { |
| 356 | double quaternion[4]; |
| 357 | // Choose four random numbers in [-1, 1) and normalize. |
| 358 | double norm = 0; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 359 | for (double& coeff : quaternion) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 360 | coeff = uniform_distribution(prng); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 361 | norm += coeff * coeff; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 362 | } |
| 363 | norm = sqrt(norm); |
| 364 | |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 365 | for (double& coeff : quaternion) { |
| 366 | coeff = coeff / norm; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 367 | } |
| 368 | |
| 369 | double axis_angle[3]; |
| 370 | double round_trip[4]; |
| 371 | QuaternionToAngleAxis(quaternion, axis_angle); |
| 372 | AngleAxisToQuaternion(axis_angle, round_trip); |
| 373 | ASSERT_THAT(round_trip, IsNormalizedQuaternion()); |
| 374 | ASSERT_THAT(round_trip, IsNearQuaternion(quaternion)); |
| 375 | } |
| 376 | } |
| 377 | |
| 378 | // Transforms a zero axis/angle to a rotation matrix. |
| 379 | TEST(Rotation, ZeroAngleAxisToRotationMatrix) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 380 | double axis_angle[3] = {0, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 381 | double matrix[9]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 382 | double expected[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 383 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 384 | EXPECT_THAT(matrix, IsOrthonormal()); |
| 385 | EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); |
| 386 | } |
| 387 | |
| 388 | TEST(Rotation, NearZeroAngleAxisToRotationMatrix) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 389 | double axis_angle[3] = {1e-24, 2e-24, 3e-24}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 390 | double matrix[9]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 391 | double expected[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 392 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 393 | EXPECT_THAT(matrix, IsOrthonormal()); |
| 394 | EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); |
| 395 | } |
| 396 | |
| 397 | // Transforms a rotation by pi/2 around X to a rotation matrix and back. |
| 398 | TEST(Rotation, XRotationToRotationMatrix) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 399 | double axis_angle[3] = {kPi / 2, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 400 | double matrix[9]; |
| 401 | // The rotation matrices are stored column-major. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 402 | double expected[9] = {1, 0, 0, 0, 0, 1, 0, -1, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 403 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 404 | EXPECT_THAT(matrix, IsOrthonormal()); |
| 405 | EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); |
| 406 | double round_trip[3]; |
| 407 | RotationMatrixToAngleAxis(matrix, round_trip); |
| 408 | EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); |
| 409 | } |
| 410 | |
| 411 | // Transforms an axis angle that rotates by pi about the Y axis to a |
| 412 | // rotation matrix and back. |
| 413 | TEST(Rotation, YRotationToRotationMatrix) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 414 | double axis_angle[3] = {0, kPi, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 415 | double matrix[9]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 416 | double expected[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 417 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 418 | EXPECT_THAT(matrix, IsOrthonormal()); |
| 419 | EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); |
| 420 | |
| 421 | double round_trip[3]; |
| 422 | RotationMatrixToAngleAxis(matrix, round_trip); |
| 423 | EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); |
| 424 | } |
| 425 | |
| 426 | TEST(Rotation, NearPiAngleAxisRoundTrip) { |
| 427 | double in_axis_angle[3]; |
| 428 | double matrix[9]; |
| 429 | double out_axis_angle[3]; |
| 430 | |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 431 | std::mt19937 prng; |
| 432 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 433 | for (int i = 0; i < kNumTrials; i++) { |
| 434 | // Make an axis by choosing three random numbers in [-1, 1) and |
| 435 | // normalizing. |
| 436 | double norm = 0; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 437 | for (double& coeff : in_axis_angle) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 438 | coeff = uniform_distribution(prng); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 439 | norm += coeff * coeff; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 440 | } |
| 441 | norm = sqrt(norm); |
| 442 | |
| 443 | // Angle in [pi - kMaxSmallAngle, pi). |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 444 | constexpr double kMaxSmallAngle = 1e-8; |
| 445 | double theta = |
| 446 | uniform_distribution(prng, |
| 447 | std::uniform_real_distribution<double>::param_type{ |
| 448 | kPi - kMaxSmallAngle, kPi}); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 449 | |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 450 | for (double& coeff : in_axis_angle) { |
| 451 | coeff *= (theta / norm); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 452 | } |
| 453 | AngleAxisToRotationMatrix(in_axis_angle, matrix); |
| 454 | RotationMatrixToAngleAxis(matrix, out_axis_angle); |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 455 | EXPECT_THAT(in_axis_angle, IsNearAngleAxis(out_axis_angle)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 456 | } |
| 457 | } |
| 458 | |
| 459 | TEST(Rotation, AtPiAngleAxisRoundTrip) { |
Keir Mierle | efe7ac6 | 2012-06-24 22:25:28 -0700 | [diff] [blame] | 460 | // A rotation of kPi about the X axis; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 461 | // clang-format off |
Sameer Agarwal | 57cf20a | 2020-04-21 10:10:01 -0700 | [diff] [blame] | 462 | static constexpr double kMatrix[3][3] = { |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 463 | {1.0, 0.0, 0.0}, |
| 464 | {0.0, -1.0, 0.0}, |
| 465 | {0.0, 0.0, -1.0} |
| 466 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 467 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 468 | |
| 469 | double in_matrix[9]; |
| 470 | // Fill it from kMatrix in col-major order. |
| 471 | for (int j = 0, k = 0; j < 3; ++j) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 472 | for (int i = 0; i < 3; ++i, ++k) { |
| 473 | in_matrix[k] = kMatrix[i][j]; |
| 474 | } |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 475 | } |
| 476 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 477 | const double expected_axis_angle[3] = {kPi, 0, 0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 478 | |
| 479 | double out_matrix[9]; |
| 480 | double axis_angle[3]; |
| 481 | RotationMatrixToAngleAxis(in_matrix, axis_angle); |
| 482 | AngleAxisToRotationMatrix(axis_angle, out_matrix); |
| 483 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 484 | LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1] << " " |
| 485 | << axis_angle[2]; |
Keir Mierle | efe7ac6 | 2012-06-24 22:25:28 -0700 | [diff] [blame] | 486 | LOG(INFO) << "Expected AngleAxis = " << kPi << " 0 0"; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 487 | double out_rowmajor[3][3]; |
| 488 | for (int j = 0, k = 0; j < 3; ++j) { |
| 489 | for (int i = 0; i < 3; ++i, ++k) { |
| 490 | out_rowmajor[i][j] = out_matrix[k]; |
| 491 | } |
| 492 | } |
| 493 | LOG(INFO) << "Rotation:"; |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 494 | LOG(INFO) << "EXPECTED | ACTUAL"; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 495 | for (int i = 0; i < 3; ++i) { |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 496 | std::string line; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 497 | for (int j = 0; j < 3; ++j) { |
| 498 | StringAppendF(&line, "%g ", kMatrix[i][j]); |
| 499 | } |
| 500 | line += " | "; |
| 501 | for (int j = 0; j < 3; ++j) { |
| 502 | StringAppendF(&line, "%g ", out_rowmajor[i][j]); |
| 503 | } |
| 504 | LOG(INFO) << line; |
| 505 | } |
| 506 | |
| 507 | EXPECT_THAT(axis_angle, IsNearAngleAxis(expected_axis_angle)); |
| 508 | EXPECT_THAT(out_matrix, IsNear3x3Matrix(in_matrix)); |
| 509 | } |
| 510 | |
| 511 | // Transforms an axis angle that rotates by pi/3 about the Z axis to a |
| 512 | // rotation matrix. |
| 513 | TEST(Rotation, ZRotationToRotationMatrix) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 514 | double axis_angle[3] = {0, 0, kPi / 3}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 515 | double matrix[9]; |
| 516 | // This is laid-out row-major on the screen but is actually stored |
| 517 | // column-major. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 518 | // clang-format off |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 519 | double expected[9] = { 0.5, sqrt(3) / 2, 0, // Column 1 |
| 520 | -sqrt(3) / 2, 0.5, 0, // Column 2 |
| 521 | 0, 0, 1 }; // Column 3 |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 522 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 523 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 524 | EXPECT_THAT(matrix, IsOrthonormal()); |
| 525 | EXPECT_THAT(matrix, IsNear3x3Matrix(expected)); |
| 526 | double round_trip[3]; |
| 527 | RotationMatrixToAngleAxis(matrix, round_trip); |
| 528 | EXPECT_THAT(round_trip, IsNearAngleAxis(axis_angle)); |
| 529 | } |
| 530 | |
| 531 | // Takes a bunch of random axis/angle values, converts them to rotation |
| 532 | // matrices, and back again. |
| 533 | TEST(Rotation, AngleAxisToRotationMatrixAndBack) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 534 | std::mt19937 prng; |
| 535 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 536 | for (int i = 0; i < kNumTrials; i++) { |
| 537 | double axis_angle[3]; |
| 538 | // Make an axis by choosing three random numbers in [-1, 1) and |
| 539 | // normalizing. |
| 540 | double norm = 0; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 541 | for (double& i : axis_angle) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 542 | i = uniform_distribution(prng); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 543 | norm += i * i; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 544 | } |
| 545 | norm = sqrt(norm); |
| 546 | |
| 547 | // Angle in [-pi, pi). |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 548 | double theta = uniform_distribution( |
| 549 | prng, std::uniform_real_distribution<double>::param_type{-kPi, kPi}); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 550 | for (double& i : axis_angle) { |
| 551 | i = i * theta / norm; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 552 | } |
| 553 | |
| 554 | double matrix[9]; |
| 555 | double round_trip[3]; |
| 556 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 557 | ASSERT_THAT(matrix, IsOrthonormal()); |
| 558 | RotationMatrixToAngleAxis(matrix, round_trip); |
| 559 | |
| 560 | for (int i = 0; i < 3; ++i) { |
| 561 | EXPECT_NEAR(round_trip[i], axis_angle[i], kLooseTolerance); |
| 562 | } |
| 563 | } |
| 564 | } |
| 565 | |
Sameer Agarwal | 21d6a99 | 2013-10-25 10:20:24 -0700 | [diff] [blame] | 566 | // Takes a bunch of random axis/angle values near zero, converts them |
| 567 | // to rotation matrices, and back again. |
| 568 | TEST(Rotation, AngleAxisToRotationMatrixAndBackNearZero) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 569 | std::mt19937 prng; |
| 570 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Sameer Agarwal | 21d6a99 | 2013-10-25 10:20:24 -0700 | [diff] [blame] | 571 | for (int i = 0; i < kNumTrials; i++) { |
| 572 | double axis_angle[3]; |
| 573 | // Make an axis by choosing three random numbers in [-1, 1) and |
| 574 | // normalizing. |
| 575 | double norm = 0; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 576 | for (double& i : axis_angle) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 577 | i = uniform_distribution(prng); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 578 | norm += i * i; |
Sameer Agarwal | 21d6a99 | 2013-10-25 10:20:24 -0700 | [diff] [blame] | 579 | } |
| 580 | norm = sqrt(norm); |
| 581 | |
| 582 | // Tiny theta. |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 583 | constexpr double kScale = 1e-16; |
| 584 | double theta = |
| 585 | uniform_distribution(prng, |
| 586 | std::uniform_real_distribution<double>::param_type{ |
| 587 | -kScale * kPi, kScale * kPi}); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 588 | for (double& i : axis_angle) { |
| 589 | i = i * theta / norm; |
Sameer Agarwal | 21d6a99 | 2013-10-25 10:20:24 -0700 | [diff] [blame] | 590 | } |
| 591 | |
| 592 | double matrix[9]; |
| 593 | double round_trip[3]; |
| 594 | AngleAxisToRotationMatrix(axis_angle, matrix); |
| 595 | ASSERT_THAT(matrix, IsOrthonormal()); |
| 596 | RotationMatrixToAngleAxis(matrix, round_trip); |
| 597 | |
| 598 | for (int i = 0; i < 3; ++i) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 599 | EXPECT_NEAR( |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 600 | round_trip[i], axis_angle[i], std::numeric_limits<double>::epsilon()); |
Sameer Agarwal | 21d6a99 | 2013-10-25 10:20:24 -0700 | [diff] [blame] | 601 | } |
| 602 | } |
| 603 | } |
| 604 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 605 | // Transposes a 3x3 matrix. |
| 606 | static void Transpose3x3(double m[9]) { |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 607 | std::swap(m[1], m[3]); |
| 608 | std::swap(m[2], m[6]); |
| 609 | std::swap(m[5], m[7]); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 610 | } |
| 611 | |
| 612 | // Convert Euler angles from radians to degrees. |
Keir Mierle | f4ba28d | 2016-03-16 11:35:28 -0700 | [diff] [blame] | 613 | static void ToDegrees(double euler_angles[3]) { |
| 614 | for (int i = 0; i < 3; ++i) { |
| 615 | euler_angles[i] *= 180.0 / kPi; |
| 616 | } |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 617 | } |
| 618 | |
| 619 | // Compare the 3x3 rotation matrices produced by the axis-angle |
| 620 | // rotation 'aa' and the Euler angle rotation 'ea' (in radians). |
| 621 | static void CompareEulerToAngleAxis(double aa[3], double ea[3]) { |
| 622 | double aa_matrix[9]; |
| 623 | AngleAxisToRotationMatrix(aa, aa_matrix); |
| 624 | Transpose3x3(aa_matrix); // Column to row major order. |
| 625 | |
| 626 | double ea_matrix[9]; |
| 627 | ToDegrees(ea); // Radians to degrees. |
| 628 | const int kRowStride = 3; |
| 629 | EulerAnglesToRotationMatrix(ea, kRowStride, ea_matrix); |
| 630 | |
| 631 | EXPECT_THAT(aa_matrix, IsOrthonormal()); |
| 632 | EXPECT_THAT(ea_matrix, IsOrthonormal()); |
| 633 | EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix)); |
| 634 | } |
| 635 | |
| 636 | // Test with rotation axis along the x/y/z axes. |
| 637 | // Also test zero rotation. |
| 638 | TEST(EulerAnglesToRotationMatrix, OnAxis) { |
| 639 | int n_tests = 0; |
| 640 | for (double x = -1.0; x <= 1.0; x += 1.0) { |
| 641 | for (double y = -1.0; y <= 1.0; y += 1.0) { |
| 642 | for (double z = -1.0; z <= 1.0; z += 1.0) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 643 | if ((x != 0) + (y != 0) + (z != 0) > 1) continue; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 644 | double axis_angle[3] = {x, y, z}; |
| 645 | double euler_angles[3] = {x, y, z}; |
| 646 | CompareEulerToAngleAxis(axis_angle, euler_angles); |
| 647 | ++n_tests; |
| 648 | } |
| 649 | } |
| 650 | } |
Sameer Agarwal | 0a53aa9 | 2024-07-07 10:24:18 -0700 | [diff] [blame] | 651 | ASSERT_EQ(7, n_tests); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 652 | } |
| 653 | |
| 654 | // Test that a random rotation produces an orthonormal rotation |
| 655 | // matrix. |
| 656 | TEST(EulerAnglesToRotationMatrix, IsOrthonormal) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 657 | std::mt19937 prng; |
| 658 | std::uniform_real_distribution<double> uniform_distribution{-180.0, 180.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 659 | for (int trial = 0; trial < kNumTrials; ++trial) { |
Keir Mierle | f4ba28d | 2016-03-16 11:35:28 -0700 | [diff] [blame] | 660 | double euler_angles_degrees[3]; |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 661 | for (double& euler_angles_degree : euler_angles_degrees) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 662 | euler_angles_degree = uniform_distribution(prng); |
Keir Mierle | f4ba28d | 2016-03-16 11:35:28 -0700 | [diff] [blame] | 663 | } |
| 664 | double rotation_matrix[9]; |
| 665 | EulerAnglesToRotationMatrix(euler_angles_degrees, 3, rotation_matrix); |
| 666 | EXPECT_THAT(rotation_matrix, IsOrthonormal()); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 667 | } |
| 668 | } |
| 669 | |
hs293go | 2b89ce6 | 2021-11-23 15:39:14 -0500 | [diff] [blame] | 670 | static double sample_euler[][3] = {{0.5235988, 1.047198, 0.7853982}, |
| 671 | {0.5235988, 1.047198, 0.5235988}, |
| 672 | {0.7853982, 0.5235988, 1.047198}}; |
| 673 | |
| 674 | // ZXY Intrinsic Euler Angle to rotation matrix conversion test from |
| 675 | // scipy/spatial/transform/test/test_rotation.py |
| 676 | TEST(EulerAngles, IntrinsicEulerSequence312ToRotationMatrixCanned) { |
| 677 | // clang-format off |
| 678 | double const expected[][9] = |
| 679 | {{0.306186083320088, -0.249999816228639, 0.918558748402491, |
| 680 | 0.883883627842492, 0.433012359189203, -0.176776777947208, |
| 681 | -0.353553128699351, 0.866025628186053, 0.353553102817459}, |
| 682 | { 0.533493553519713, -0.249999816228639, 0.808012821828067, |
| 683 | 0.808012821828067, 0.433012359189203, -0.399519181705765, |
| 684 | -0.249999816228639, 0.866025628186053, 0.433012359189203}, |
| 685 | { 0.047366781483451, -0.612372449482883, 0.789149143778432, |
| 686 | 0.659739427618959, 0.612372404654096, 0.435596057905909, |
| 687 | -0.750000183771249, 0.500000021132493, 0.433012359189203}}; |
| 688 | // clang-format on |
| 689 | |
| 690 | for (int i = 0; i < 3; ++i) { |
| 691 | double results[9]; |
| 692 | EulerAnglesToRotation<IntrinsicZXY>(sample_euler[i], results); |
| 693 | ASSERT_THAT(results, IsNear3x3Matrix(expected[i])); |
| 694 | } |
| 695 | } |
| 696 | |
| 697 | // ZXY Extrinsic Euler Angle to rotation matrix conversion test from |
| 698 | // scipy/spatial/transform/test/test_rotation.py |
| 699 | TEST(EulerAngles, ExtrinsicEulerSequence312ToRotationMatrix) { |
| 700 | // clang-format off |
| 701 | double const expected[][9] = |
| 702 | {{0.918558725988105, 0.176776842651999, 0.353553128699352, |
| 703 | 0.249999816228639, 0.433012359189203, -0.866025628186053, |
| 704 | -0.306186150563275, 0.883883614901527, 0.353553102817459}, |
| 705 | { 0.966506404215301, -0.058012606358071, 0.249999816228639, |
| 706 | 0.249999816228639, 0.433012359189203, -0.866025628186053, |
| 707 | -0.058012606358071, 0.899519223970752, 0.433012359189203}, |
| 708 | { 0.659739424151467, -0.047366829779744, 0.750000183771249, |
| 709 | 0.612372449482883, 0.612372404654096, -0.500000021132493, |
| 710 | -0.435596000136163, 0.789149175666285, 0.433012359189203}}; |
| 711 | // clang-format on |
| 712 | |
| 713 | for (int i = 0; i < 3; ++i) { |
| 714 | double results[9]; |
| 715 | EulerAnglesToRotation<ExtrinsicZXY>(sample_euler[i], results); |
| 716 | ASSERT_THAT(results, IsNear3x3Matrix(expected[i])); |
| 717 | } |
| 718 | } |
| 719 | |
| 720 | // ZXZ Intrinsic Euler Angle to rotation matrix conversion test from |
| 721 | // scipy/spatial/transform/test/test_rotation.py |
| 722 | TEST(EulerAngles, IntrinsicEulerSequence313ToRotationMatrix) { |
| 723 | // clang-format off |
| 724 | double expected[][9] = |
| 725 | {{0.435595832832961, -0.789149008363071, 0.433012832394307, |
| 726 | 0.659739379322704, -0.047367454164077, -0.750000183771249, |
| 727 | 0.612372616786097, 0.612372571957297, 0.499999611324802}, |
| 728 | { 0.625000065470068, -0.649518902838302, 0.433012832394307, |
| 729 | 0.649518902838302, 0.124999676794869, -0.750000183771249, |
| 730 | 0.433012832394307, 0.750000183771249, 0.499999611324802}, |
| 731 | {-0.176777132429787, -0.918558558684756, 0.353553418477159, |
| 732 | 0.883883325123719, -0.306186652473014, -0.353553392595246, |
| 733 | 0.433012832394307, 0.249999816228639, 0.866025391583588}}; |
| 734 | // clang-format on |
| 735 | for (int i = 0; i < 3; ++i) { |
| 736 | double results[9]; |
| 737 | EulerAnglesToRotation<IntrinsicZXZ>(sample_euler[i], results); |
| 738 | ASSERT_THAT(results, IsNear3x3Matrix(expected[i])); |
| 739 | } |
| 740 | } |
| 741 | |
| 742 | // ZXZ Extrinsic Euler Angle to rotation matrix conversion test from |
| 743 | // scipy/spatial/transform/test/test_rotation.py |
| 744 | TEST(EulerAngles, ExtrinsicEulerSequence313ToRotationMatrix) { |
| 745 | // clang-format off |
| 746 | double expected[][9] = |
| 747 | {{0.435595832832961, -0.659739379322704, 0.612372616786097, |
| 748 | 0.789149008363071, -0.047367454164077, -0.612372571957297, |
| 749 | 0.433012832394307, 0.750000183771249, 0.499999611324802}, |
| 750 | { 0.625000065470068, -0.649518902838302, 0.433012832394307, |
| 751 | 0.649518902838302, 0.124999676794869, -0.750000183771249, |
| 752 | 0.433012832394307, 0.750000183771249, 0.499999611324802}, |
| 753 | {-0.176777132429787, -0.883883325123719, 0.433012832394307, |
| 754 | 0.918558558684756, -0.306186652473014, -0.249999816228639, |
| 755 | 0.353553418477159, 0.353553392595246, 0.866025391583588}}; |
| 756 | // clang-format on |
| 757 | for (int i = 0; i < 3; ++i) { |
| 758 | double results[9]; |
| 759 | EulerAnglesToRotation<ExtrinsicZXZ>(sample_euler[i], results); |
| 760 | ASSERT_THAT(results, IsNear3x3Matrix(expected[i])); |
| 761 | } |
| 762 | } |
| 763 | |
| 764 | template <typename T> |
| 765 | struct GeneralEulerAngles : public ::testing::Test { |
| 766 | public: |
| 767 | static constexpr bool kIsParityOdd = T::kIsParityOdd; |
| 768 | static constexpr bool kIsProperEuler = T::kIsProperEuler; |
| 769 | static constexpr bool kIsIntrinsic = T::kIsIntrinsic; |
| 770 | |
| 771 | template <typename URBG> |
| 772 | static void RandomEulerAngles(double* euler, URBG& prng) { |
| 773 | using ParamType = std::uniform_real_distribution<double>::param_type; |
| 774 | std::uniform_real_distribution<double> uniform_distribution{-kPi, kPi}; |
| 775 | // Euler angles should be in |
| 776 | // [-pi,pi) x [0,pi) x [-pi,pi]) |
| 777 | // if the outer axes are repeated and |
| 778 | // [-pi,pi) x [-pi/2,pi/2) x [-pi,pi]) |
| 779 | // otherwise |
| 780 | euler[0] = uniform_distribution(prng); |
| 781 | euler[2] = uniform_distribution(prng); |
| 782 | if constexpr (kIsProperEuler) { |
| 783 | euler[1] = uniform_distribution(prng, ParamType{0, kPi}); |
| 784 | } else { |
| 785 | euler[1] = uniform_distribution(prng, ParamType{-kPi / 2, kPi / 2}); |
| 786 | } |
| 787 | } |
| 788 | |
| 789 | static void CheckPrincipalRotationMatrixProduct(double angles[3]) { |
| 790 | // Convert Shoemake's Euler angle convention into 'apparent' rotation axes |
| 791 | // sequences, i.e. the alphabetic code (ZYX, ZYZ, etc.) indicates in what |
| 792 | // sequence rotations about different axes are applied |
| 793 | constexpr int i = T::kAxes[0]; |
| 794 | constexpr int j = (3 + (kIsParityOdd ? (i - 1) % 3 : (i + 1) % 3)) % 3; |
| 795 | constexpr int k = kIsProperEuler ? i : 3 ^ i ^ j; |
| 796 | constexpr auto kSeq = |
| 797 | kIsIntrinsic ? std::array{k, j, i} : std::array{i, j, k}; |
| 798 | |
| 799 | double aa_matrix[9]; |
| 800 | Eigen::Map<Eigen::Matrix3d, 0, Eigen::Stride<1, 3>> aa(aa_matrix); |
| 801 | aa.setIdentity(); |
| 802 | for (int i = 0; i < 3; ++i) { |
| 803 | Eigen::Vector3d angle_axis; |
| 804 | if constexpr (kIsIntrinsic) { |
| 805 | angle_axis = -angles[i] * Eigen::Vector3d::Unit(kSeq[i]); |
| 806 | } else { |
| 807 | angle_axis = angles[i] * Eigen::Vector3d::Unit(kSeq[i]); |
| 808 | } |
| 809 | Eigen::Matrix3d m; |
| 810 | AngleAxisToRotationMatrix(angle_axis.data(), m.data()); |
| 811 | aa = m * aa; |
| 812 | } |
| 813 | if constexpr (kIsIntrinsic) { |
| 814 | aa.transposeInPlace(); |
| 815 | } |
| 816 | |
| 817 | double ea_matrix[9]; |
| 818 | EulerAnglesToRotation<T>(angles, ea_matrix); |
| 819 | |
| 820 | EXPECT_THAT(aa_matrix, IsOrthonormal()); |
| 821 | EXPECT_THAT(ea_matrix, IsOrthonormal()); |
| 822 | EXPECT_THAT(ea_matrix, IsNear3x3Matrix(aa_matrix)); |
| 823 | } |
| 824 | }; |
| 825 | |
| 826 | using EulerSystemList = ::testing::Types<ExtrinsicXYZ, |
| 827 | ExtrinsicXYX, |
| 828 | ExtrinsicXZY, |
| 829 | ExtrinsicXZX, |
| 830 | ExtrinsicYZX, |
| 831 | ExtrinsicYZY, |
| 832 | ExtrinsicYXZ, |
| 833 | ExtrinsicYXY, |
| 834 | ExtrinsicZXY, |
| 835 | ExtrinsicZXZ, |
| 836 | ExtrinsicZYX, |
| 837 | ExtrinsicZYZ, |
| 838 | IntrinsicZYX, |
| 839 | IntrinsicXYX, |
| 840 | IntrinsicYZX, |
| 841 | IntrinsicXZX, |
| 842 | IntrinsicXZY, |
| 843 | IntrinsicYZY, |
| 844 | IntrinsicZXY, |
| 845 | IntrinsicYXY, |
| 846 | IntrinsicYXZ, |
| 847 | IntrinsicZXZ, |
| 848 | IntrinsicXYZ, |
| 849 | IntrinsicZYZ>; |
| 850 | TYPED_TEST_SUITE(GeneralEulerAngles, EulerSystemList); |
| 851 | |
| 852 | TYPED_TEST(GeneralEulerAngles, EulerAnglesToRotationMatrixAndBack) { |
| 853 | std::mt19937 prng; |
| 854 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
| 855 | for (int i = 0; i < kNumTrials; ++i) { |
| 856 | double euler[3]; |
| 857 | TestFixture::RandomEulerAngles(euler, prng); |
| 858 | |
| 859 | double matrix[9]; |
| 860 | double round_trip[3]; |
| 861 | EulerAnglesToRotation<TypeParam>(euler, matrix); |
| 862 | ASSERT_THAT(matrix, IsOrthonormal()); |
| 863 | RotationMatrixToEulerAngles<TypeParam>(matrix, round_trip); |
| 864 | for (int j = 0; j < 3; ++j) |
| 865 | ASSERT_NEAR(euler[j], round_trip[j], 128.0 * kLooseTolerance); |
| 866 | } |
| 867 | } |
| 868 | |
| 869 | // Check that the rotation matrix converted from euler angles is equivalent to |
| 870 | // product of three principal axis rotation matrices |
| 871 | // R_euler = R_a2(euler_2) * R_a1(euler_1) * R_a0(euler_0) |
| 872 | TYPED_TEST(GeneralEulerAngles, PrincipalRotationMatrixProduct) { |
| 873 | std::mt19937 prng; |
| 874 | double euler[3]; |
| 875 | for (int i = 0; i < kNumTrials; ++i) { |
| 876 | TestFixture::RandomEulerAngles(euler, prng); |
| 877 | TestFixture::CheckPrincipalRotationMatrixProduct(euler); |
| 878 | } |
| 879 | } |
| 880 | |
| 881 | // Gimbal lock (euler[1] == +/-pi) handling test. If a rotation matrix |
| 882 | // represents a gimbal-locked configuration, then converting this rotation |
| 883 | // matrix to euler angles and back must produce the same rotation matrix. |
| 884 | // |
| 885 | // From scipy/spatial/transform/test/test_rotation.py, but additionally covers |
| 886 | // gimbal lock handling for proper euler angles, which scipy appears to fail to |
| 887 | // do properly. |
| 888 | TYPED_TEST(GeneralEulerAngles, GimbalLocked) { |
| 889 | constexpr auto kBoundaryAngles = TestFixture::kIsProperEuler |
| 890 | ? std::array{0.0, kPi} |
| 891 | : std::array{-kPi / 2, kPi / 2}; |
| 892 | constexpr double gimbal_locked_configurations[4][3] = { |
| 893 | {0.78539816, kBoundaryAngles[1], 0.61086524}, |
| 894 | {0.61086524, kBoundaryAngles[0], 0.34906585}, |
| 895 | {0.61086524, kBoundaryAngles[1], 0.43633231}, |
| 896 | {0.43633231, kBoundaryAngles[0], 0.26179939}}; |
| 897 | double angle_estimates[3]; |
| 898 | double mat_expected[9]; |
| 899 | double mat_estimated[9]; |
| 900 | for (const auto& euler_angles : gimbal_locked_configurations) { |
| 901 | EulerAnglesToRotation<TypeParam>(euler_angles, mat_expected); |
| 902 | RotationMatrixToEulerAngles<TypeParam>(mat_expected, angle_estimates); |
| 903 | EulerAnglesToRotation<TypeParam>(angle_estimates, mat_estimated); |
| 904 | ASSERT_THAT(mat_expected, IsNear3x3Matrix(mat_estimated)); |
| 905 | } |
| 906 | } |
| 907 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 908 | // Tests using Jets for specific behavior involving auto differentiation |
| 909 | // near singularity points. |
| 910 | |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 911 | using J3 = Jet<double, 3>; |
| 912 | using J4 = Jet<double, 4>; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 913 | |
Sergey Sharybin | 54ba6c2 | 2017-12-23 18:18:24 +0100 | [diff] [blame] | 914 | namespace { |
| 915 | |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 916 | // Converts an array of N real numbers (doubles) to an array of jets |
| 917 | template <int N> |
| 918 | void ArrayToArrayOfJets(double const* const src, Jet<double, N>* dst) { |
| 919 | for (int i = 0; i < N; ++i) { |
| 920 | dst[i] = Jet<double, N>(src[i], i); |
| 921 | } |
| 922 | } |
| 923 | |
| 924 | // Generically initializes a Jet with type T and a N-dimensional dual part |
| 925 | // N is explicitly given (instead of inferred from sizeof...(Ts)) so that the |
| 926 | // dual part can be initialized from Eigen expressions |
| 927 | template <int N, typename T, typename... Ts> |
| 928 | Jet<T, N> MakeJet(T a, const T& v0, Ts&&... v) { |
| 929 | Jet<T, N> j; |
| 930 | j.a = a; // Real part |
| 931 | ((j.v << v0), ..., std::forward<Ts>(v)); // Fill dual part with N components |
| 932 | return j; |
| 933 | } |
| 934 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 935 | J3 MakeJ3(double a, double v0, double v1, double v2) { |
| 936 | J3 j; |
| 937 | j.a = a; |
| 938 | j.v[0] = v0; |
| 939 | j.v[1] = v1; |
| 940 | j.v[2] = v2; |
| 941 | return j; |
| 942 | } |
| 943 | |
| 944 | J4 MakeJ4(double a, double v0, double v1, double v2, double v3) { |
| 945 | J4 j; |
| 946 | j.a = a; |
| 947 | j.v[0] = v0; |
| 948 | j.v[1] = v1; |
| 949 | j.v[2] = v2; |
| 950 | j.v[3] = v3; |
| 951 | return j; |
| 952 | } |
| 953 | |
Sergey Sharybin | 54ba6c2 | 2017-12-23 18:18:24 +0100 | [diff] [blame] | 954 | } // namespace |
| 955 | |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 956 | // Use EXPECT_THAT(x, testing::PointWise(JetClose(prec), y); to achieve Jet |
| 957 | // array comparison |
| 958 | MATCHER_P(JetClose, relative_precision, "") { |
| 959 | using internal::IsClose; |
| 960 | using LHSJetType = std::remove_reference_t<std::tuple_element_t<0, arg_type>>; |
| 961 | using RHSJetType = std::remove_reference_t<std::tuple_element_t<1, arg_type>>; |
| 962 | |
| 963 | constexpr int kDualPartDimension = LHSJetType::DIMENSION; |
| 964 | static_assert( |
| 965 | kDualPartDimension == RHSJetType::DIMENSION, |
| 966 | "Can only compare Jets with dual parts having equal dimensions"); |
| 967 | auto&& [x, y] = arg; |
| 968 | double relative_error; |
| 969 | double absolute_error; |
| 970 | if (!IsClose( |
| 971 | x.a, y.a, relative_precision, &relative_error, &absolute_error)) { |
| 972 | *result_listener << "Real part mismatch: x.a = " << x.a |
| 973 | << " and y.a = " << y.a |
| 974 | << " where the relative error between them is " |
| 975 | << relative_error |
| 976 | << " and the absolute error between them is " |
| 977 | << absolute_error; |
Keir Mierle | 0149ce0 | 2016-03-16 11:09:53 -0700 | [diff] [blame] | 978 | return false; |
| 979 | } |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 980 | for (int i = 0; i < kDualPartDimension; i++) { |
| 981 | if (!IsClose(x.v[i], |
| 982 | y.v[i], |
| 983 | relative_precision, |
| 984 | &relative_error, |
| 985 | &absolute_error)) { |
| 986 | *result_listener << "Dual part mismatch: x.v[" << i << "] = " << x.v[i] |
| 987 | << " and y.v[" << i << "] = " << y.v[i] |
| 988 | << " where the relative error between them is " |
| 989 | << relative_error |
| 990 | << " and the absolute error between them is " |
| 991 | << absolute_error; |
Keir Mierle | 0149ce0 | 2016-03-16 11:09:53 -0700 | [diff] [blame] | 992 | return false; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 993 | } |
| 994 | } |
| 995 | return true; |
| 996 | } |
| 997 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 998 | // Log-10 of a value well below machine precision. |
Sameer Agarwal | 79a554f | 2023-01-13 11:37:27 -0800 | [diff] [blame] | 999 | static const int kSmallTinyCutoff = static_cast<int>( |
| 1000 | 2 * log(std::numeric_limits<double>::epsilon()) / log(10.0)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1001 | |
| 1002 | // Log-10 of a value just below values representable by double. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1003 | static const int kTinyZeroLimit = |
Alexander Ivanov | 53df5dd | 2023-01-11 16:51:38 +0000 | [diff] [blame] | 1004 | static_cast<int>(1 + log(std::numeric_limits<double>::min()) / log(10.0)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1005 | |
| 1006 | // Test that exact conversion works for small angles when jets are used. |
| 1007 | TEST(Rotation, SmallAngleAxisToQuaternionForJets) { |
| 1008 | // Examine small x rotations that are still large enough |
| 1009 | // to be well within the range represented by doubles. |
| 1010 | for (int i = -2; i >= kSmallTinyCutoff; i--) { |
| 1011 | double theta = pow(10.0, i); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1012 | J3 axis_angle[3] = {J3(theta, 0), J3(0, 1), J3(0, 2)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1013 | J3 quaternion[4]; |
| 1014 | J3 expected[4] = { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1015 | MakeJ3(cos(theta / 2), -sin(theta / 2) / 2, 0, 0), |
| 1016 | MakeJ3(sin(theta / 2), cos(theta / 2) / 2, 0, 0), |
| 1017 | MakeJ3(0, 0, sin(theta / 2) / theta, 0), |
| 1018 | MakeJ3(0, 0, 0, sin(theta / 2) / theta), |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1019 | }; |
| 1020 | AngleAxisToQuaternion(axis_angle, quaternion); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1021 | EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1022 | } |
| 1023 | } |
| 1024 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1025 | // Test that conversion works for very small angles when jets are used. |
| 1026 | TEST(Rotation, TinyAngleAxisToQuaternionForJets) { |
| 1027 | // Examine tiny x rotations that extend all the way to where |
| 1028 | // underflow occurs. |
| 1029 | for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { |
| 1030 | double theta = pow(10.0, i); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1031 | J3 axis_angle[3] = {J3(theta, 0), J3(0, 1), J3(0, 2)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1032 | J3 quaternion[4]; |
| 1033 | // To avoid loss of precision in the test itself, |
| 1034 | // a finite expansion is used here, which will |
| 1035 | // be exact up to machine precision for the test values used. |
| 1036 | J3 expected[4] = { |
| 1037 | MakeJ3(1.0, 0, 0, 0), |
| 1038 | MakeJ3(0, 0.5, 0, 0), |
| 1039 | MakeJ3(0, 0, 0.5, 0), |
| 1040 | MakeJ3(0, 0, 0, 0.5), |
| 1041 | }; |
| 1042 | AngleAxisToQuaternion(axis_angle, quaternion); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1043 | EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1044 | } |
| 1045 | } |
| 1046 | |
| 1047 | // Test that derivatives are correct for zero rotation. |
| 1048 | TEST(Rotation, ZeroAngleAxisToQuaternionForJets) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1049 | J3 axis_angle[3] = {J3(0, 0), J3(0, 1), J3(0, 2)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1050 | J3 quaternion[4]; |
| 1051 | J3 expected[4] = { |
| 1052 | MakeJ3(1.0, 0, 0, 0), |
| 1053 | MakeJ3(0, 0.5, 0, 0), |
| 1054 | MakeJ3(0, 0, 0.5, 0), |
| 1055 | MakeJ3(0, 0, 0, 0.5), |
| 1056 | }; |
| 1057 | AngleAxisToQuaternion(axis_angle, quaternion); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1058 | EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1059 | } |
| 1060 | |
| 1061 | // Test that exact conversion works for small angles. |
| 1062 | TEST(Rotation, SmallQuaternionToAngleAxisForJets) { |
| 1063 | // Examine small x rotations that are still large enough |
| 1064 | // to be well within the range represented by doubles. |
| 1065 | for (int i = -2; i >= kSmallTinyCutoff; i--) { |
| 1066 | double theta = pow(10.0, i); |
| 1067 | double s = sin(theta); |
| 1068 | double c = cos(theta); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1069 | J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1070 | J4 axis_angle[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1071 | // clang-format off |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1072 | J4 expected[3] = { |
Keir Mierle | 0149ce0 | 2016-03-16 11:09:53 -0700 | [diff] [blame] | 1073 | MakeJ4(2*theta, -2*s, 2*c, 0, 0), |
| 1074 | MakeJ4(0, 0, 0, 2*theta/s, 0), |
| 1075 | MakeJ4(0, 0, 0, 0, 2*theta/s), |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1076 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1077 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1078 | QuaternionToAngleAxis(quaternion, axis_angle); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1079 | EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1080 | } |
| 1081 | } |
| 1082 | |
| 1083 | // Test that conversion works for very small angles. |
| 1084 | TEST(Rotation, TinyQuaternionToAngleAxisForJets) { |
| 1085 | // Examine tiny x rotations that extend all the way to where |
| 1086 | // underflow occurs. |
| 1087 | for (int i = kSmallTinyCutoff; i >= kTinyZeroLimit; i--) { |
| 1088 | double theta = pow(10.0, i); |
| 1089 | double s = sin(theta); |
| 1090 | double c = cos(theta); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1091 | J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1092 | J4 axis_angle[3]; |
| 1093 | // To avoid loss of precision in the test itself, |
| 1094 | // a finite expansion is used here, which will |
| 1095 | // be exact up to machine precision for the test values used. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1096 | // clang-format off |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1097 | J4 expected[3] = { |
Keir Mierle | 0149ce0 | 2016-03-16 11:09:53 -0700 | [diff] [blame] | 1098 | MakeJ4(2*theta, -2*s, 2.0, 0, 0), |
| 1099 | MakeJ4(0, 0, 0, 2.0, 0), |
| 1100 | MakeJ4(0, 0, 0, 0, 2.0), |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1101 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1102 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1103 | QuaternionToAngleAxis(quaternion, axis_angle); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1104 | EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1105 | } |
| 1106 | } |
| 1107 | |
| 1108 | // Test that conversion works for no rotation. |
| 1109 | TEST(Rotation, ZeroQuaternionToAngleAxisForJets) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1110 | J4 quaternion[4] = {J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3)}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1111 | J4 axis_angle[3]; |
| 1112 | J4 expected[3] = { |
| 1113 | MakeJ4(0, 0, 2.0, 0, 0), |
| 1114 | MakeJ4(0, 0, 0, 2.0, 0), |
| 1115 | MakeJ4(0, 0, 0, 0, 2.0), |
| 1116 | }; |
| 1117 | QuaternionToAngleAxis(quaternion, axis_angle); |
Hs293Go | 96fdfd2 | 2023-04-22 02:48:39 -0400 | [diff] [blame] | 1118 | EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected)); |
| 1119 | } |
| 1120 | |
| 1121 | // The following 4 test cases cover the conversion of Euler Angles to rotation |
| 1122 | // matrices for Jets |
| 1123 | // |
| 1124 | // The dual parts (with dimension 3) of the resultant matrix of Jets contain the |
| 1125 | // derivative of each matrix element w.r.t. the input Euler Angles. In other |
| 1126 | // words, for each element in R = EulerAnglesToRotationMatrix(angles), we have |
| 1127 | // R_ij.v = jacobian(R_ij, angles) |
| 1128 | // |
| 1129 | // The test data (dual parts of the Jets) is generated by analytically |
| 1130 | // differentiating the formulas for Euler Angle to Rotation Matrix conversion |
| 1131 | |
| 1132 | // Test ZXY/312 Intrinsic Euler Angles to rotation matrix conversion using Jets |
| 1133 | // The two ZXY test cases specifically cover handling of Tait-Bryan angles |
| 1134 | // i.e. last axis of rotation is different from the first |
| 1135 | TEST(EulerAngles, Intrinsic312EulerSequenceToRotationMatrixForJets) { |
| 1136 | J3 euler_angles[3]; |
| 1137 | J3 rotation_matrix[9]; |
| 1138 | |
| 1139 | ArrayToArrayOfJets(sample_euler[0], euler_angles); |
| 1140 | EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix); |
| 1141 | { |
| 1142 | // clang-format off |
| 1143 | const J3 expected[] = { |
| 1144 | MakeJ3( 0.306186083320, -0.883883627842, -0.176776571821, -0.918558748402), // NOLINT |
| 1145 | MakeJ3(-0.249999816229, -0.433012359189, 0.433012832394, 0.000000000000), // NOLINT |
| 1146 | MakeJ3( 0.918558748402, 0.176776777947, 0.176776558880, 0.306186083320), // NOLINT |
| 1147 | MakeJ3( 0.883883627842, 0.306186083320, 0.306185986727, 0.176776777947), // NOLINT |
| 1148 | MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771, 0.000000000000), // NOLINT |
| 1149 | MakeJ3(-0.176776777947, 0.918558748402, -0.306185964313, 0.883883627842), // NOLINT |
| 1150 | MakeJ3(-0.353553128699, 0.000000000000, 0.612372616786, -0.353553102817), // NOLINT |
| 1151 | MakeJ3( 0.866025628186, 0.000000000000, 0.499999611325, 0.000000000000), // NOLINT |
| 1152 | MakeJ3( 0.353553102817, 0.000000000000, -0.612372571957, -0.353553128699) // NOLINT |
| 1153 | }; |
| 1154 | // clang-format on |
| 1155 | EXPECT_THAT(rotation_matrix, |
| 1156 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1157 | } |
| 1158 | |
| 1159 | ArrayToArrayOfJets(sample_euler[1], euler_angles); |
| 1160 | EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix); |
| 1161 | { |
| 1162 | // clang-format off |
| 1163 | const J3 expected[] = { |
| 1164 | MakeJ3( 0.533493553520, -0.808012821828, -0.124999913397, -0.808012821828), // NOLINT |
| 1165 | MakeJ3(-0.249999816229, -0.433012359189, 0.433012832394, 0.000000000000), // NOLINT |
| 1166 | MakeJ3( 0.808012821828, 0.399519181706, 0.216506188745, 0.533493553520), // NOLINT |
| 1167 | MakeJ3( 0.808012821828, 0.533493553520, 0.216506188745, 0.399519181706), // NOLINT |
| 1168 | MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771, 0.000000000000), // NOLINT |
| 1169 | MakeJ3(-0.399519181706, 0.808012821828, -0.374999697927, 0.808012821828), // NOLINT |
| 1170 | MakeJ3(-0.249999816229, 0.000000000000, 0.433012832394, -0.433012359189), // NOLINT |
| 1171 | MakeJ3( 0.866025628186, 0.000000000000, 0.499999611325, 0.000000000000), // NOLINT |
| 1172 | MakeJ3( 0.433012359189, 0.000000000000, -0.750000183771, -0.249999816229) // NOLINT |
| 1173 | }; |
| 1174 | // clang-format on |
| 1175 | EXPECT_THAT(rotation_matrix, |
| 1176 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1177 | } |
| 1178 | |
| 1179 | ArrayToArrayOfJets(sample_euler[2], euler_angles); |
| 1180 | EulerAnglesToRotation<IntrinsicZXY>(euler_angles, rotation_matrix); |
| 1181 | { |
| 1182 | // clang-format off |
| 1183 | const J3 expected[] = { |
| 1184 | MakeJ3( 0.047366781483, -0.659739427619, -0.530330235247, -0.789149143778), // NOLINT |
| 1185 | MakeJ3(-0.612372449483, -0.612372404654, 0.353553418477, 0.000000000000), // NOLINT |
| 1186 | MakeJ3( 0.789149143778, -0.435596057906, 0.306185986727, 0.047366781483), // NOLINT |
| 1187 | MakeJ3( 0.659739427619, 0.047366781483, 0.530330196424, -0.435596057906), // NOLINT |
| 1188 | MakeJ3( 0.612372404654, -0.612372449483, -0.353553392595, 0.000000000000), // NOLINT |
| 1189 | MakeJ3( 0.435596057906, 0.789149143778, -0.306185964313, 0.659739427619), // NOLINT |
| 1190 | MakeJ3(-0.750000183771, 0.000000000000, 0.433012832394, -0.433012359189), // NOLINT |
| 1191 | MakeJ3( 0.500000021132, 0.000000000000, 0.866025391584, 0.000000000000), // NOLINT |
| 1192 | MakeJ3( 0.433012359189, 0.000000000000, -0.249999816229, -0.750000183771) // NOLINT |
| 1193 | }; |
| 1194 | // clang-format on |
| 1195 | EXPECT_THAT(rotation_matrix, |
| 1196 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1197 | } |
| 1198 | } |
| 1199 | |
| 1200 | // Test ZXY/312 Extrinsic Euler Angles to rotation matrix conversion using Jets |
| 1201 | TEST(EulerAngles, Extrinsic312EulerSequenceToRotationMatrixForJets) { |
| 1202 | J3 euler_angles[3]; |
| 1203 | J3 rotation_matrix[9]; |
| 1204 | |
| 1205 | ArrayToArrayOfJets(sample_euler[0], euler_angles); |
| 1206 | EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix); |
| 1207 | { |
| 1208 | // clang-format off |
| 1209 | const J3 expected[] = { |
| 1210 | MakeJ3( 0.918558725988, 0.176776842652, 0.176776571821, -0.306186150563), // NOLINT |
| 1211 | MakeJ3( 0.176776842652, -0.918558725988, 0.306185986727, 0.883883614902), // NOLINT |
| 1212 | MakeJ3( 0.353553128699, 0.000000000000, -0.612372616786, 0.353553102817), // NOLINT |
| 1213 | MakeJ3( 0.249999816229, 0.433012359189, -0.433012832394, 0.000000000000), // NOLINT |
| 1214 | MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771, 0.000000000000), // NOLINT |
| 1215 | MakeJ3(-0.866025628186, 0.000000000000, -0.499999611325, 0.000000000000), // NOLINT |
| 1216 | MakeJ3(-0.306186150563, 0.883883614902, 0.176776558880, -0.918558725988), // NOLINT |
| 1217 | MakeJ3( 0.883883614902, 0.306186150563, 0.306185964313, -0.176776842652), // NOLINT |
| 1218 | MakeJ3( 0.353553102817, 0.000000000000, -0.612372571957, -0.353553128699) // NOLINT |
| 1219 | }; |
| 1220 | // clang-format on |
| 1221 | EXPECT_THAT(rotation_matrix, |
| 1222 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1223 | } |
| 1224 | |
| 1225 | ArrayToArrayOfJets(sample_euler[1], euler_angles); |
| 1226 | EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix); |
| 1227 | { |
| 1228 | // clang-format off |
| 1229 | const J3 expected[] = { |
| 1230 | MakeJ3( 0.966506404215, -0.058012606358, 0.124999913397, -0.058012606358), // NOLINT |
| 1231 | MakeJ3(-0.058012606358, -0.966506404215, 0.216506188745, 0.899519223971), // NOLINT |
| 1232 | MakeJ3( 0.249999816229, 0.000000000000, -0.433012832394, 0.433012359189), // NOLINT |
| 1233 | MakeJ3( 0.249999816229, 0.433012359189, -0.433012832394, 0.000000000000), // NOLINT |
| 1234 | MakeJ3( 0.433012359189, -0.249999816229, -0.750000183771, 0.000000000000), // NOLINT |
| 1235 | MakeJ3(-0.866025628186, 0.000000000000, -0.499999611325, 0.000000000000), // NOLINT |
| 1236 | MakeJ3(-0.058012606358, 0.899519223971, 0.216506188745, -0.966506404215), // NOLINT |
| 1237 | MakeJ3( 0.899519223971, 0.058012606358, 0.374999697927, 0.058012606358), // NOLINT |
| 1238 | MakeJ3( 0.433012359189, 0.000000000000, -0.750000183771, -0.249999816229) // NOLINT |
| 1239 | }; |
| 1240 | // clang-format on |
| 1241 | EXPECT_THAT(rotation_matrix, |
| 1242 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1243 | } |
| 1244 | |
| 1245 | ArrayToArrayOfJets(sample_euler[2], euler_angles); |
| 1246 | EulerAnglesToRotation<ExtrinsicZXY>(euler_angles, rotation_matrix); |
| 1247 | { |
| 1248 | // clang-format off |
| 1249 | const J3 expected[] = { |
| 1250 | MakeJ3( 0.659739424151, -0.047366829780, 0.530330235247, -0.435596000136), // NOLINT |
| 1251 | MakeJ3(-0.047366829780, -0.659739424151, 0.530330196424, 0.789149175666), // NOLINT |
| 1252 | MakeJ3( 0.750000183771, 0.000000000000, -0.433012832394, 0.433012359189), // NOLINT |
| 1253 | MakeJ3( 0.612372449483, 0.612372404654, -0.353553418477, 0.000000000000), // NOLINT |
| 1254 | MakeJ3( 0.612372404654, -0.612372449483, -0.353553392595, 0.000000000000), // NOLINT |
| 1255 | MakeJ3(-0.500000021132, 0.000000000000, -0.866025391584, 0.000000000000), // NOLINT |
| 1256 | MakeJ3(-0.435596000136, 0.789149175666, 0.306185986727, -0.659739424151), // NOLINT |
| 1257 | MakeJ3( 0.789149175666, 0.435596000136, 0.306185964313, 0.047366829780), // NOLINT |
| 1258 | MakeJ3( 0.433012359189, 0.000000000000, -0.249999816229, -0.750000183771) // NOLINT |
| 1259 | }; |
| 1260 | // clang-format on |
| 1261 | EXPECT_THAT(rotation_matrix, |
| 1262 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1263 | } |
| 1264 | } |
| 1265 | |
| 1266 | // Test ZXZ/313 Intrinsic Euler Angles to rotation matrix conversion using Jets |
| 1267 | // The two ZXZ test cases specifically cover handling of proper Euler Sequences |
| 1268 | // i.e. last axis of rotation is same as the first |
| 1269 | TEST(EulerAngles, Intrinsic313EulerSequenceToRotationMatrixForJets) { |
| 1270 | J3 euler_angles[3]; |
| 1271 | J3 rotation_matrix[9]; |
| 1272 | |
| 1273 | ArrayToArrayOfJets(sample_euler[0], euler_angles); |
| 1274 | EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix); |
| 1275 | { |
| 1276 | // clang-format off |
| 1277 | const J3 expected[] = { |
| 1278 | MakeJ3( 0.435595832833, -0.659739379323, 0.306186321334, -0.789149008363), // NOLINT |
| 1279 | MakeJ3(-0.789149008363, 0.047367454164, 0.306186298920, -0.435595832833), // NOLINT |
| 1280 | MakeJ3( 0.433012832394, 0.750000183771, 0.249999816229, 0.000000000000), // NOLINT |
| 1281 | MakeJ3( 0.659739379323, 0.435595832833, -0.530330235247, -0.047367454164), // NOLINT |
| 1282 | MakeJ3(-0.047367454164, -0.789149008363, -0.530330196424, -0.659739379323), // NOLINT |
| 1283 | MakeJ3(-0.750000183771, 0.433012832394, -0.433012359189, 0.000000000000), // NOLINT |
| 1284 | MakeJ3( 0.612372616786, 0.000000000000, 0.353553128699, 0.612372571957), // NOLINT |
| 1285 | MakeJ3( 0.612372571957, 0.000000000000, 0.353553102817, -0.612372616786), // NOLINT |
| 1286 | MakeJ3( 0.499999611325, 0.000000000000, -0.866025628186, 0.000000000000) // NOLINT |
| 1287 | }; |
| 1288 | // clang-format on |
| 1289 | EXPECT_THAT(rotation_matrix, |
| 1290 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1291 | } |
| 1292 | |
| 1293 | ArrayToArrayOfJets(sample_euler[1], euler_angles); |
| 1294 | EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix); |
| 1295 | { |
| 1296 | // clang-format off |
| 1297 | const J3 expected[] = { |
| 1298 | MakeJ3( 0.625000065470, -0.649518902838, 0.216506425348, -0.649518902838), // NOLINT |
| 1299 | MakeJ3(-0.649518902838, -0.124999676795, 0.375000107735, -0.625000065470), // NOLINT |
| 1300 | MakeJ3( 0.433012832394, 0.750000183771, 0.249999816229, 0.000000000000), // NOLINT |
| 1301 | MakeJ3( 0.649518902838, 0.625000065470, -0.375000107735, 0.124999676795), // NOLINT |
| 1302 | MakeJ3( 0.124999676795, -0.649518902838, -0.649519202838, -0.649518902838), // NOLINT |
| 1303 | MakeJ3(-0.750000183771, 0.433012832394, -0.433012359189, 0.000000000000), // NOLINT |
| 1304 | MakeJ3( 0.433012832394, 0.000000000000, 0.249999816229, 0.750000183771), // NOLINT |
| 1305 | MakeJ3( 0.750000183771, 0.000000000000, 0.433012359189, -0.433012832394), // NOLINT |
| 1306 | MakeJ3( 0.499999611325, 0.000000000000, -0.866025628186, 0.000000000000) // NOLINT |
| 1307 | }; |
| 1308 | // clang-format on |
| 1309 | EXPECT_THAT(rotation_matrix, |
| 1310 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1311 | } |
| 1312 | |
| 1313 | ArrayToArrayOfJets(sample_euler[2], euler_angles); |
| 1314 | EulerAnglesToRotation<IntrinsicZXZ>(euler_angles, rotation_matrix); |
| 1315 | { |
| 1316 | // clang-format off |
| 1317 | const J3 expected[] = { |
| 1318 | MakeJ3(-0.176777132430, -0.883883325124, 0.306186321334, -0.918558558685), // NOLINT |
| 1319 | MakeJ3(-0.918558558685, 0.306186652473, 0.176776571821, 0.176777132430), // NOLINT |
| 1320 | MakeJ3( 0.353553418477, 0.353553392595, 0.612372449483, 0.000000000000), // NOLINT |
| 1321 | MakeJ3( 0.883883325124, -0.176777132430, -0.306186298920, -0.306186652473), // NOLINT |
| 1322 | MakeJ3(-0.306186652473, -0.918558558685, -0.176776558880, -0.883883325124), // NOLINT |
| 1323 | MakeJ3(-0.353553392595, 0.353553418477, -0.612372404654, 0.000000000000), // NOLINT |
| 1324 | MakeJ3( 0.433012832394, 0.000000000000, 0.750000183771, 0.249999816229), // NOLINT |
| 1325 | MakeJ3( 0.249999816229, 0.000000000000, 0.433012359189, -0.433012832394), // NOLINT |
| 1326 | MakeJ3( 0.866025391584, 0.000000000000, -0.500000021132, 0.000000000000) // NOLINT |
| 1327 | }; |
| 1328 | // clang-format on |
| 1329 | EXPECT_THAT(rotation_matrix, |
| 1330 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1331 | } |
| 1332 | } |
| 1333 | |
| 1334 | // Test ZXZ/313 Extrinsic Euler Angles to rotation matrix conversion using Jets |
| 1335 | TEST(EulerAngles, Extrinsic313EulerSequenceToRotationMatrixForJets) { |
| 1336 | J3 euler_angles[3]; |
| 1337 | J3 rotation_matrix[9]; |
| 1338 | |
| 1339 | ArrayToArrayOfJets(sample_euler[0], euler_angles); |
| 1340 | EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix); |
| 1341 | { |
| 1342 | // clang-format off |
| 1343 | const J3 expected[] = { |
| 1344 | MakeJ3( 0.435595832833, -0.659739379323, 0.306186321334, -0.789149008363), // NOLINT |
| 1345 | MakeJ3(-0.659739379323, -0.435595832833, 0.530330235247, 0.047367454164), // NOLINT |
| 1346 | MakeJ3( 0.612372616786, 0.000000000000, 0.353553128699, 0.612372571957), // NOLINT |
| 1347 | MakeJ3( 0.789149008363, -0.047367454164, -0.306186298920, 0.435595832833), // NOLINT |
| 1348 | MakeJ3(-0.047367454164, -0.789149008363, -0.530330196424, -0.659739379323), // NOLINT |
| 1349 | MakeJ3(-0.612372571957, 0.000000000000, -0.353553102817, 0.612372616786), // NOLINT |
| 1350 | MakeJ3( 0.433012832394, 0.750000183771, 0.249999816229, 0.000000000000), // NOLINT |
| 1351 | MakeJ3( 0.750000183771, -0.433012832394, 0.433012359189, 0.000000000000), // NOLINT |
| 1352 | MakeJ3( 0.499999611325, 0.000000000000, -0.866025628186, 0.000000000000) // NOLINT |
| 1353 | }; |
| 1354 | // clang-format on |
| 1355 | EXPECT_THAT(rotation_matrix, |
| 1356 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1357 | } |
| 1358 | |
| 1359 | ArrayToArrayOfJets(sample_euler[1], euler_angles); |
| 1360 | EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix); |
| 1361 | { |
| 1362 | // clang-format off |
| 1363 | const J3 expected[] = { |
| 1364 | MakeJ3( 0.625000065470, -0.649518902838, 0.216506425348, -0.649518902838), // NOLINT |
| 1365 | MakeJ3(-0.649518902838, -0.625000065470, 0.375000107735, -0.124999676795), // NOLINT |
| 1366 | MakeJ3( 0.433012832394, 0.000000000000, 0.249999816229, 0.750000183771), // NOLINT |
| 1367 | MakeJ3( 0.649518902838, 0.124999676795, -0.375000107735, 0.625000065470), // NOLINT |
| 1368 | MakeJ3( 0.124999676795, -0.649518902838, -0.649519202838, -0.649518902838), // NOLINT |
| 1369 | MakeJ3(-0.750000183771, 0.000000000000, -0.433012359189, 0.433012832394), // NOLINT |
| 1370 | MakeJ3( 0.433012832394, 0.750000183771, 0.249999816229, 0.000000000000), // NOLINT |
| 1371 | MakeJ3( 0.750000183771, -0.433012832394, 0.433012359189, 0.000000000000), // NOLINT |
| 1372 | MakeJ3( 0.499999611325, 0.000000000000, -0.866025628186, 0.000000000000) // NOLINT |
| 1373 | }; |
| 1374 | // clang-format on |
| 1375 | EXPECT_THAT(rotation_matrix, |
| 1376 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1377 | } |
| 1378 | |
| 1379 | ArrayToArrayOfJets(sample_euler[2], euler_angles); |
| 1380 | EulerAnglesToRotation<ExtrinsicZXZ>(euler_angles, rotation_matrix); |
| 1381 | { |
| 1382 | // clang-format off |
| 1383 | const J3 expected[] = { |
| 1384 | MakeJ3(-0.176777132430, -0.883883325124, 0.306186321334, -0.918558558685), // NOLINT |
| 1385 | MakeJ3(-0.883883325124, 0.176777132430, 0.306186298920, 0.306186652473), // NOLINT |
| 1386 | MakeJ3( 0.433012832394, 0.000000000000, 0.750000183771, 0.249999816229), // NOLINT |
| 1387 | MakeJ3( 0.918558558685, -0.306186652473, -0.176776571821, -0.176777132430), // NOLINT |
| 1388 | MakeJ3(-0.306186652473, -0.918558558685, -0.176776558880, -0.883883325124), // NOLINT |
| 1389 | MakeJ3(-0.249999816229, 0.000000000000, -0.433012359189, 0.433012832394), // NOLINT |
| 1390 | MakeJ3( 0.353553418477, 0.353553392595, 0.612372449483, 0.000000000000), // NOLINT |
| 1391 | MakeJ3( 0.353553392595, -0.353553418477, 0.612372404654, 0.000000000000), // NOLINT |
| 1392 | MakeJ3( 0.866025391584, 0.000000000000, -0.500000021132, 0.000000000000) // NOLINT |
| 1393 | }; |
| 1394 | // clang-format on |
| 1395 | EXPECT_THAT(rotation_matrix, |
| 1396 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1397 | } |
| 1398 | } |
| 1399 | |
| 1400 | using J9 = Jet<double, 9>; |
| 1401 | |
| 1402 | // The following 4 tests Tests the conversion of rotation matrices to Euler |
| 1403 | // Angles for Jets. |
| 1404 | // |
| 1405 | // The dual parts (with dimension 9) of the resultant array of Jets contain the |
| 1406 | // derivative of each Euler angle w.r.t. each of the 9 elements of the rotation |
| 1407 | // matrix, or a 9-by-1 array formed from flattening the rotation matrix. In |
| 1408 | // other words, for each element in angles = RotationMatrixToEulerAngles(R), we |
| 1409 | // have angles.v = jacobian(angles, [R11 R12 R13 R21 ... R32 R33]); |
| 1410 | // |
| 1411 | // Note: the order of elements in v depend on row/column-wise flattening of |
| 1412 | // the rotation matrix |
| 1413 | // |
| 1414 | // The test data (dual parts of the Jets) is generated by analytically |
| 1415 | // differentiating the formulas for Rotation Matrix to Euler Angle conversion |
| 1416 | |
| 1417 | // clang-format off |
| 1418 | static double sample_matrices[][9] = { |
| 1419 | { 0.433012359189, 0.176776842652, 0.883883614902, 0.249999816229, 0.918558725988, -0.306186150563, -0.866025628186, 0.353553128699, 0.353553102817}, // NOLINT |
| 1420 | { 0.433012359189, -0.058012606358, 0.899519223971, 0.249999816229, 0.966506404215, -0.058012606358, -0.866025628186, 0.249999816229, 0.433012359189}, // NOLINT |
| 1421 | { 0.612372404654, -0.047366829780, 0.789149175666, 0.612372449483, 0.659739424151, -0.435596000136, -0.500000021132, 0.750000183771, 0.433012359189} // NOLINT |
| 1422 | }; |
| 1423 | // clang-format on |
| 1424 | |
| 1425 | // Test rotation matrix to ZXY/312 Intrinsic Euler Angles conversion using Jets |
| 1426 | // The two ZXY test cases specifically cover handling of Tait-Bryan angles |
| 1427 | // i.e. last axis of rotation is different from the first |
| 1428 | TEST(EulerAngles, RotationMatrixToIntrinsic312EulerSequenceForJets) { |
| 1429 | J9 euler_angles[3]; |
| 1430 | J9 rotation_matrix[9]; |
| 1431 | |
| 1432 | ArrayToArrayOfJets(sample_matrices[0], rotation_matrix); |
| 1433 | RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles); |
| 1434 | { |
| 1435 | // clang-format off |
| 1436 | const J9 expected[] = { |
| 1437 | MakeJet<9>(-0.190125743401, 0.000000000000, -1.049781178951, 0.000000000000, 0.000000000000, 0.202030634558, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1438 | MakeJet<9>( 0.361366843930, 0.000000000000, -0.066815309609, 0.000000000000, 0.000000000000, -0.347182270882, 0.000000000000, 0.000000000000, 0.935414445680, 0.000000000000), // NOLINT |
| 1439 | MakeJet<9>( 1.183200015636, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.404060603418, 0.000000000000, -0.989743365598) // NOLINT |
| 1440 | }; |
| 1441 | // clang-format on |
| 1442 | EXPECT_THAT(euler_angles, |
| 1443 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1444 | } |
| 1445 | |
| 1446 | ArrayToArrayOfJets(sample_matrices[1], rotation_matrix); |
| 1447 | RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles); |
| 1448 | { |
| 1449 | // clang-format off |
| 1450 | const J9 expected[] = { |
| 1451 | MakeJet<9>( 0.059951064811, 0.000000000000, -1.030940063452, 0.000000000000, 0.000000000000, -0.061880107384, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1452 | MakeJet<9>( 0.252680065344, 0.000000000000, 0.014978778808, 0.000000000000, 0.000000000000, -0.249550684831, 0.000000000000, 0.000000000000, 0.968245884001, 0.000000000000), // NOLINT |
| 1453 | MakeJet<9>( 1.107149138016, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.461879804532, 0.000000000000, -0.923760579526) // NOLINT |
| 1454 | }; |
| 1455 | // clang-format on |
| 1456 | EXPECT_THAT(euler_angles, |
| 1457 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1458 | } |
| 1459 | |
| 1460 | ArrayToArrayOfJets(sample_matrices[2], rotation_matrix); |
| 1461 | RotationMatrixToEulerAngles<IntrinsicZXY>(rotation_matrix, euler_angles); |
| 1462 | { |
| 1463 | // clang-format off |
| 1464 | const J9 expected[] = { |
| 1465 | MakeJet<9>( 0.071673287221, 0.000000000000, -1.507976776767, 0.000000000000, 0.000000000000, -0.108267107713, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1466 | MakeJet<9>( 0.848062356818, 0.000000000000, 0.053708966648, 0.000000000000, 0.000000000000, -0.748074610289, 0.000000000000, 0.000000000000, 0.661437619389, 0.000000000000), // NOLINT |
| 1467 | MakeJet<9>( 0.857072360427, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.989743158900, 0.000000000000, -1.142857911244) // NOLINT |
| 1468 | }; |
| 1469 | // clang-format on |
| 1470 | EXPECT_THAT(euler_angles, |
| 1471 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1472 | } |
| 1473 | } |
| 1474 | |
| 1475 | // Test rotation matrix to ZXY/312 Extrinsic Euler Angles conversion using Jets |
| 1476 | TEST(EulerAngles, RotationMatrixToExtrinsic312EulerSequenceForJets) { |
| 1477 | J9 euler_angles[3]; |
| 1478 | J9 rotation_matrix[9]; |
| 1479 | |
| 1480 | ArrayToArrayOfJets(sample_matrices[0], rotation_matrix); |
| 1481 | RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles); |
| 1482 | { |
| 1483 | // clang-format off |
| 1484 | const J9 expected[] = { |
| 1485 | MakeJet<9>( 0.265728912717, 0.000000000000, 0.000000000000, 0.000000000000, 1.013581996386, -0.275861853641, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1486 | MakeJet<9>( 0.311184173598, 0.000000000000, 0.000000000000, -0.284286741927, 0.000000000000, 0.000000000000, -0.951971659874, 0.000000000000, 0.000000000000, -0.113714586405), // NOLINT |
| 1487 | MakeJet<9>( 1.190290284357, 0.000000000000, 0.000000000000, 0.390127543992, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.975319806582) // NOLINT |
| 1488 | }; |
| 1489 | // clang-format on |
| 1490 | EXPECT_THAT(euler_angles, |
| 1491 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1492 | } |
| 1493 | |
| 1494 | ArrayToArrayOfJets(sample_matrices[1], rotation_matrix); |
| 1495 | RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles); |
| 1496 | { |
| 1497 | // clang-format off |
| 1498 | const J9 expected[] = { |
| 1499 | MakeJet<9>( 0.253115668605, 0.000000000000, 0.000000000000, 0.000000000000, 0.969770129215, -0.250844022378, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1500 | MakeJet<9>( 0.058045195612, 0.000000000000, 0.000000000000, -0.052271487648, 0.000000000000, 0.000000000000, -0.998315850572, 0.000000000000, 0.000000000000, -0.025162553041), // NOLINT |
| 1501 | MakeJet<9>( 1.122153748896, 0.000000000000, 0.000000000000, 0.434474567050, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.902556744846) // NOLINT |
| 1502 | }; |
| 1503 | // clang-format on |
| 1504 | EXPECT_THAT(euler_angles, |
| 1505 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1506 | } |
| 1507 | |
| 1508 | ArrayToArrayOfJets(sample_matrices[2], rotation_matrix); |
| 1509 | RotationMatrixToEulerAngles<ExtrinsicZXY>(rotation_matrix, euler_angles); |
| 1510 | { |
| 1511 | // clang-format off |
| 1512 | const J9 expected[] = { |
| 1513 | MakeJet<9>( 0.748180444286, 0.000000000000, 0.000000000000, 0.000000000000, 0.814235652244, -0.755776390750, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1514 | MakeJet<9>( 0.450700288478, 0.000000000000, 0.000000000000, -0.381884322045, 0.000000000000, 0.000000000000, -0.900142280234, 0.000000000000, 0.000000000000, -0.209542930950), // NOLINT |
| 1515 | MakeJet<9>( 1.068945699497, 0.000000000000, 0.000000000000, 0.534414175972, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.973950275281) // NOLINT |
| 1516 | }; |
| 1517 | // clang-format on |
| 1518 | EXPECT_THAT(euler_angles, |
| 1519 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1520 | } |
| 1521 | } |
| 1522 | |
| 1523 | // Test rotation matrix to ZXZ/313 Intrinsic Euler Angles conversion using Jets |
| 1524 | //// The two ZXZ test cases specifically cover handling of proper Euler |
| 1525 | /// Sequences |
| 1526 | // i.e. last axis of rotation is same as the first |
| 1527 | TEST(EulerAngles, RotationMatrixToIntrinsic313EulerSequenceForJets) { |
| 1528 | J9 euler_angles[3]; |
| 1529 | J9 rotation_matrix[9]; |
| 1530 | |
| 1531 | ArrayToArrayOfJets(sample_matrices[0], rotation_matrix); |
| 1532 | RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles); |
| 1533 | { |
| 1534 | // clang-format off |
| 1535 | const J9 expected[] = { |
| 1536 | MakeJet<9>( 1.237323270947, 0.000000000000, 0.000000000000, 0.349926947837, 0.000000000000, 0.000000000000, 1.010152467826, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1537 | MakeJet<9>( 1.209429510533, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.327326615680, 0.133630397662, -0.935414455462), // NOLINT |
| 1538 | MakeJet<9>(-1.183199990019, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.404060624546, 0.989743344897, 0.000000000000) // NOLINT |
| 1539 | }; |
| 1540 | // clang-format on |
| 1541 | EXPECT_THAT(euler_angles, |
| 1542 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1543 | } |
| 1544 | |
| 1545 | ArrayToArrayOfJets(sample_matrices[1], rotation_matrix); |
| 1546 | RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles); |
| 1547 | { |
| 1548 | // clang-format off |
| 1549 | const J9 expected[] = { |
| 1550 | MakeJet<9>( 1.506392616830, 0.000000000000, 0.000000000000, 0.071400104821, 0.000000000000, 0.000000000000, 1.107100178948, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1551 | MakeJet<9>( 1.122964310061, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.416024849727, 0.120095910090, -0.901387983495), // NOLINT |
| 1552 | MakeJet<9>(-1.289761690216, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.307691969119, 1.065877306886, 0.000000000000) // NOLINT |
| 1553 | }; |
| 1554 | // clang-format on |
| 1555 | EXPECT_THAT(euler_angles, |
| 1556 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1557 | } |
| 1558 | |
| 1559 | ArrayToArrayOfJets(sample_matrices[2], rotation_matrix); |
| 1560 | RotationMatrixToEulerAngles<IntrinsicZXZ>(rotation_matrix, euler_angles); |
| 1561 | { |
| 1562 | // clang-format off |
| 1563 | const J9 expected[] = { |
| 1564 | MakeJet<9>( 1.066432836578, 0.000000000000, 0.000000000000, 0.536117958181, 0.000000000000, 0.000000000000, 0.971260169116, 0.000000000000, 0.000000000000, 0.000000000000), // NOLINT |
| 1565 | MakeJet<9>( 1.122964310061, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.240192006893, 0.360288083393, -0.901387983495), // NOLINT |
| 1566 | MakeJet<9>(-0.588002509965, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.923076812076, 0.615384416607, 0.000000000000) // NOLINT |
| 1567 | }; |
| 1568 | // clang-format on |
| 1569 | EXPECT_THAT(euler_angles, |
| 1570 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1571 | } |
| 1572 | } |
| 1573 | |
| 1574 | // Test rotation matrix to ZXZ/313 Extrinsic Euler Angles conversion using Jets |
| 1575 | TEST(EulerAngles, RotationMatrixToExtrinsic313EulerSequenceForJets) { |
| 1576 | J9 euler_angles[3]; |
| 1577 | J9 rotation_matrix[9]; |
| 1578 | |
| 1579 | ArrayToArrayOfJets(sample_matrices[0], rotation_matrix); |
| 1580 | RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles); |
| 1581 | { |
| 1582 | // clang-format off |
| 1583 | const J9 expected[] = { |
| 1584 | MakeJet<9>(-1.183199990019, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.404060624546, 0.989743344897, 0.000000000000), // NOLINT |
| 1585 | MakeJet<9>( 1.209429510533, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.327326615680, 0.133630397662, -0.935414455462), // NOLINT |
| 1586 | MakeJet<9>( 1.237323270947, 0.000000000000, 0.000000000000, 0.349926947837, 0.000000000000, 0.000000000000, 1.010152467826, 0.000000000000, 0.000000000000, 0.000000000000) // NOLINT |
| 1587 | }; |
| 1588 | // clang-format on |
| 1589 | EXPECT_THAT(euler_angles, |
| 1590 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1591 | } |
| 1592 | |
| 1593 | ArrayToArrayOfJets(sample_matrices[1], rotation_matrix); |
| 1594 | RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles); |
| 1595 | { |
| 1596 | // clang-format off |
| 1597 | const J9 expected[] = { |
| 1598 | MakeJet<9>(-1.289761690216, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.307691969119, 1.065877306886, 0.000000000000), // NOLINT |
| 1599 | MakeJet<9>( 1.122964310061, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.416024849727, 0.120095910090, -0.901387983495), // NOLINT |
| 1600 | MakeJet<9>( 1.506392616830, 0.000000000000, 0.000000000000, 0.071400104821, 0.000000000000, 0.000000000000, 1.107100178948, 0.000000000000, 0.000000000000, 0.000000000000) // NOLINT |
| 1601 | }; |
| 1602 | // clang-format on |
| 1603 | EXPECT_THAT(euler_angles, |
| 1604 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1605 | } |
| 1606 | |
| 1607 | ArrayToArrayOfJets(sample_matrices[2], rotation_matrix); |
| 1608 | RotationMatrixToEulerAngles<ExtrinsicZXZ>(rotation_matrix, euler_angles); |
| 1609 | { |
| 1610 | // clang-format off |
| 1611 | const J9 expected[] = { |
| 1612 | MakeJet<9>(-0.588002509965, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.923076812076, 0.615384416607, 0.000000000000), // NOLINT |
| 1613 | MakeJet<9>( 1.122964310061, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, 0.000000000000, -0.240192006893, 0.360288083393, -0.901387983495), // NOLINT |
| 1614 | MakeJet<9>( 1.066432836578, 0.000000000000, 0.000000000000, 0.536117958181, 0.000000000000, 0.000000000000, 0.971260169116, 0.000000000000, 0.000000000000, 0.000000000000) // NOLINT |
| 1615 | }; |
| 1616 | // clang-format on |
| 1617 | EXPECT_THAT(euler_angles, |
| 1618 | testing::Pointwise(JetClose(kLooseTolerance), expected)); |
| 1619 | } |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1620 | } |
| 1621 | |
| 1622 | TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) { |
| 1623 | // Canned data generated in octave. |
| 1624 | double const q[4] = { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1625 | +0.1956830471754074, |
| 1626 | -0.0150618562474847, |
| 1627 | +0.7634572982788086, |
| 1628 | -0.3019454777240753, |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1629 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1630 | double const Q[3][3] = { |
| 1631 | // Scaled rotation matrix. |
| 1632 | {-0.6355194033477252, +0.0951730541682254, +0.3078870197911186}, |
| 1633 | {-0.1411693904792992, +0.5297609702153905, -0.4551502574482019}, |
| 1634 | {-0.2896955822708862, -0.4669396571547050, -0.4536309793389248}, |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1635 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1636 | double const R[3][3] = { |
| 1637 | // With unit rows and columns. |
| 1638 | {-0.8918859164053080, +0.1335655625725649, +0.4320876677394745}, |
| 1639 | {-0.1981166751680096, +0.7434648665444399, -0.6387564287225856}, |
| 1640 | {-0.4065578619806013, -0.6553016349046693, -0.6366242786393164}, |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1641 | }; |
| 1642 | |
| 1643 | // Compute R from q and compare to known answer. |
| 1644 | double Rq[3][3]; |
| 1645 | QuaternionToScaledRotation<double>(q, Rq[0]); |
| 1646 | ExpectArraysClose(9, Q[0], Rq[0], kTolerance); |
| 1647 | |
| 1648 | // Now do the same but compute R with normalization. |
| 1649 | QuaternionToRotation<double>(q, Rq[0]); |
| 1650 | ExpectArraysClose(9, R[0], Rq[0], kTolerance); |
| 1651 | } |
| 1652 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1653 | TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) { |
| 1654 | // Rotation defined by a unit quaternion. |
| 1655 | double const q[4] = { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1656 | +0.2318160216097109, |
| 1657 | -0.0178430356832060, |
| 1658 | +0.9044300776717159, |
| 1659 | -0.3576998641394597, |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1660 | }; |
| 1661 | double const p[3] = { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1662 | +0.11, |
| 1663 | -13.15, |
| 1664 | 1.17, |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1665 | }; |
| 1666 | |
| 1667 | double R[3 * 3]; |
| 1668 | QuaternionToRotation(q, R); |
| 1669 | |
| 1670 | double result1[3]; |
| 1671 | UnitQuaternionRotatePoint(q, p, result1); |
| 1672 | |
| 1673 | double result2[3]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1674 | VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3) * ConstVectorRef(p, 3); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1675 | ExpectArraysClose(3, result1, result2, kTolerance); |
| 1676 | } |
| 1677 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1678 | // Verify that (a * b) * c == a * (b * c). |
| 1679 | TEST(Quaternion, MultiplicationIsAssociative) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1680 | std::mt19937 prng; |
| 1681 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1682 | double a[4]; |
| 1683 | double b[4]; |
| 1684 | double c[4]; |
| 1685 | for (int i = 0; i < 4; ++i) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1686 | a[i] = uniform_distribution(prng); |
| 1687 | b[i] = uniform_distribution(prng); |
| 1688 | c[i] = uniform_distribution(prng); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1689 | } |
| 1690 | |
| 1691 | double ab[4]; |
| 1692 | double ab_c[4]; |
| 1693 | QuaternionProduct(a, b, ab); |
| 1694 | QuaternionProduct(ab, c, ab_c); |
| 1695 | |
| 1696 | double bc[4]; |
| 1697 | double a_bc[4]; |
| 1698 | QuaternionProduct(b, c, bc); |
| 1699 | QuaternionProduct(a, bc, a_bc); |
| 1700 | |
| 1701 | ASSERT_NEAR(ab_c[0], a_bc[0], kTolerance); |
| 1702 | ASSERT_NEAR(ab_c[1], a_bc[1], kTolerance); |
| 1703 | ASSERT_NEAR(ab_c[2], a_bc[2], kTolerance); |
| 1704 | ASSERT_NEAR(ab_c[3], a_bc[3], kTolerance); |
| 1705 | } |
| 1706 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1707 | TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1708 | std::mt19937 prng; |
| 1709 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1710 | double angle_axis[3]; |
| 1711 | double R[9]; |
| 1712 | double p[3]; |
| 1713 | double angle_axis_rotated_p[3]; |
| 1714 | double rotation_matrix_rotated_p[3]; |
| 1715 | |
| 1716 | for (int i = 0; i < 10000; ++i) { |
Keir Mierle | efe7ac6 | 2012-06-24 22:25:28 -0700 | [diff] [blame] | 1717 | double theta = (2.0 * i * 0.0011 - 1.0) * kPi; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1718 | for (int j = 0; j < 50; ++j) { |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1719 | for (int k = 0; k < 3; ++k) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1720 | angle_axis[k] = uniform_distribution(prng); |
| 1721 | p[k] = uniform_distribution(prng); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1722 | } |
| 1723 | |
Sergiu Deitsch | cb6b306 | 2023-01-05 21:37:36 +0100 | [diff] [blame] | 1724 | const double inv_norm = |
| 1725 | theta / std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 1726 | for (double& angle_axi : angle_axis) { |
| 1727 | angle_axi *= inv_norm; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1728 | } |
| 1729 | |
| 1730 | AngleAxisToRotationMatrix(angle_axis, R); |
| 1731 | rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; |
| 1732 | rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; |
| 1733 | rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; |
| 1734 | |
| 1735 | AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); |
| 1736 | for (int k = 0; k < 3; ++k) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1737 | // clang-format off |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1738 | EXPECT_NEAR(rotation_matrix_rotated_p[k], |
| 1739 | angle_axis_rotated_p[k], |
| 1740 | kTolerance) << "p: " << p[0] |
| 1741 | << " " << p[1] |
| 1742 | << " " << p[2] |
| 1743 | << " angle_axis: " << angle_axis[0] |
| 1744 | << " " << angle_axis[1] |
| 1745 | << " " << angle_axis[2]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1746 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1747 | } |
| 1748 | } |
| 1749 | } |
| 1750 | } |
| 1751 | |
Sameer Agarwal | 79a554f | 2023-01-13 11:37:27 -0800 | [diff] [blame] | 1752 | TEST(Quaternion, UnitQuaternion) { |
| 1753 | using Jet = ceres::Jet<double, 4>; |
| 1754 | std::array<Jet, 4> quaternion = { |
| 1755 | Jet(1.0, 0), Jet(0.0, 1), Jet(0.0, 2), Jet(0.0, 3)}; |
| 1756 | std::array<Jet, 3> point = {Jet(0.0), Jet(0.0), Jet(0.0)}; |
Sameer Agarwal | c4ba975 | 2023-01-14 06:08:10 -0800 | [diff] [blame] | 1757 | std::array<Jet, 3> rotated_point; |
Sameer Agarwal | 79a554f | 2023-01-13 11:37:27 -0800 | [diff] [blame] | 1758 | QuaternionRotatePoint(quaternion.data(), point.data(), rotated_point.data()); |
Sameer Agarwal | 79a554f | 2023-01-13 11:37:27 -0800 | [diff] [blame] | 1759 | for (int i = 0; i < 3; ++i) { |
| 1760 | EXPECT_EQ(rotated_point[i], point[i]); |
| 1761 | EXPECT_FALSE(rotated_point[i].v.array().isNaN().any()); |
| 1762 | } |
| 1763 | } |
| 1764 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1765 | TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1766 | std::mt19937 prng; |
| 1767 | std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0}; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1768 | double angle_axis[3]; |
| 1769 | double R[9]; |
| 1770 | double p[3]; |
| 1771 | double angle_axis_rotated_p[3]; |
| 1772 | double rotation_matrix_rotated_p[3]; |
| 1773 | |
| 1774 | for (int i = 0; i < 10000; ++i) { |
| 1775 | double norm2 = 0.0; |
| 1776 | for (int k = 0; k < 3; ++k) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1777 | angle_axis[k] = uniform_distribution(prng); |
| 1778 | p[k] = uniform_distribution(prng); |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1779 | norm2 = angle_axis[k] * angle_axis[k]; |
| 1780 | } |
| 1781 | |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1782 | double theta = (2.0 * i * 0.0001 - 1.0) * 1e-16; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1783 | const double inv_norm = theta / sqrt(norm2); |
Sergiu Deitsch | c8658c8 | 2022-02-20 02:22:17 +0100 | [diff] [blame] | 1784 | for (double& angle_axi : angle_axis) { |
| 1785 | angle_axi *= inv_norm; |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1786 | } |
| 1787 | |
| 1788 | AngleAxisToRotationMatrix(angle_axis, R); |
| 1789 | rotation_matrix_rotated_p[0] = R[0] * p[0] + R[3] * p[1] + R[6] * p[2]; |
| 1790 | rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2]; |
| 1791 | rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2]; |
| 1792 | |
| 1793 | AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p); |
| 1794 | for (int k = 0; k < 3; ++k) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1795 | // clang-format off |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1796 | EXPECT_NEAR(rotation_matrix_rotated_p[k], |
| 1797 | angle_axis_rotated_p[k], |
| 1798 | kTolerance) << "p: " << p[0] |
| 1799 | << " " << p[1] |
| 1800 | << " " << p[2] |
| 1801 | << " angle_axis: " << angle_axis[0] |
| 1802 | << " " << angle_axis[1] |
| 1803 | << " " << angle_axis[2]; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1804 | // clang-format on |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1805 | } |
| 1806 | } |
| 1807 | } |
| 1808 | |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1809 | TEST(MatrixAdapter, RowMajor3x3ReturnTypeAndAccessIsCorrect) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1810 | double array[9] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; |
| 1811 | const float const_array[9] = { |
| 1812 | 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f}; |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1813 | MatrixAdapter<double, 3, 1> A = RowMajorAdapter3x3(array); |
| 1814 | MatrixAdapter<const float, 3, 1> B = RowMajorAdapter3x3(const_array); |
| 1815 | |
| 1816 | for (int i = 0; i < 3; ++i) { |
| 1817 | for (int j = 0; j < 3; ++j) { |
| 1818 | // The values are integers from 1 to 9, so equality tests are appropriate |
| 1819 | // even for float and double values. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1820 | EXPECT_EQ(A(i, j), array[3 * i + j]); |
| 1821 | EXPECT_EQ(B(i, j), const_array[3 * i + j]); |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1822 | } |
| 1823 | } |
| 1824 | } |
| 1825 | |
| 1826 | TEST(MatrixAdapter, ColumnMajor3x3ReturnTypeAndAccessIsCorrect) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1827 | double array[9] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; |
| 1828 | const float const_array[9] = { |
| 1829 | 1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f}; |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1830 | MatrixAdapter<double, 1, 3> A = ColumnMajorAdapter3x3(array); |
| 1831 | MatrixAdapter<const float, 1, 3> B = ColumnMajorAdapter3x3(const_array); |
| 1832 | |
| 1833 | for (int i = 0; i < 3; ++i) { |
| 1834 | for (int j = 0; j < 3; ++j) { |
Sameer Agarwal | 509f68c | 2013-02-20 01:39:03 -0800 | [diff] [blame] | 1835 | // The values are integers from 1 to 9, so equality tests are |
| 1836 | // appropriate even for float and double values. |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1837 | EXPECT_EQ(A(i, j), array[3 * j + i]); |
| 1838 | EXPECT_EQ(B(i, j), const_array[3 * j + i]); |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1839 | } |
| 1840 | } |
| 1841 | } |
| 1842 | |
| 1843 | TEST(MatrixAdapter, RowMajor2x4IsCorrect) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1844 | const int expected[8] = {1, 2, 3, 4, 5, 6, 7, 8}; |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1845 | int array[8]; |
| 1846 | MatrixAdapter<int, 4, 1> M(array); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1847 | // clang-format off |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1848 | M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; |
| 1849 | M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1850 | // clang-format on |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1851 | for (int k = 0; k < 8; ++k) { |
| 1852 | EXPECT_EQ(array[k], expected[k]); |
| 1853 | } |
| 1854 | } |
| 1855 | |
| 1856 | TEST(MatrixAdapter, ColumnMajor2x4IsCorrect) { |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1857 | const int expected[8] = {1, 5, 2, 6, 3, 7, 4, 8}; |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1858 | int array[8]; |
| 1859 | MatrixAdapter<int, 1, 2> M(array); |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1860 | // clang-format off |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1861 | M(0, 0) = 1; M(0, 1) = 2; M(0, 2) = 3; M(0, 3) = 4; |
| 1862 | M(1, 0) = 5; M(1, 1) = 6; M(1, 2) = 7; M(1, 3) = 8; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1863 | // clang-format on |
Keir Mierle | 3e2c4ef | 2013-02-17 12:37:55 -0800 | [diff] [blame] | 1864 | for (int k = 0; k < 8; ++k) { |
| 1865 | EXPECT_EQ(array[k], expected[k]); |
| 1866 | } |
| 1867 | } |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1868 | |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 1869 | TEST(RotationMatrixToAngleAxis, NearPiExampleOneFromTobiasStrauss) { |
| 1870 | // Example from Tobias Strauss |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1871 | // clang-format off |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 1872 | const double rotation_matrix[] = { |
| 1873 | -0.999807135425239, -0.0128154391194470, -0.0148814136745799, |
| 1874 | -0.0128154391194470, -0.148441438622958, 0.988838158557669, |
| 1875 | -0.0148814136745799, 0.988838158557669, 0.148248574048196 |
| 1876 | }; |
Nikolaus Demmel | 7b8f675 | 2020-09-20 21:45:24 +0200 | [diff] [blame] | 1877 | // clang-format on |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 1878 | |
| 1879 | double angle_axis[3]; |
| 1880 | RotationMatrixToAngleAxis(RowMajorAdapter3x3(rotation_matrix), angle_axis); |
| 1881 | double round_trip[9]; |
| 1882 | AngleAxisToRotationMatrix(angle_axis, RowMajorAdapter3x3(round_trip)); |
| 1883 | EXPECT_THAT(rotation_matrix, IsNear3x3Matrix(round_trip)); |
| 1884 | } |
| 1885 | |
Sergey Sharybin | 54ba6c2 | 2017-12-23 18:18:24 +0100 | [diff] [blame] | 1886 | static void CheckRotationMatrixToAngleAxisRoundTrip(const double theta, |
| 1887 | const double phi, |
| 1888 | const double angle) { |
Sameer Agarwal | cd358c7 | 2014-10-29 17:31:40 -0700 | [diff] [blame] | 1889 | double angle_axis[3]; |
| 1890 | angle_axis[0] = angle * sin(phi) * cos(theta); |
| 1891 | angle_axis[1] = angle * sin(phi) * sin(theta); |
| 1892 | angle_axis[2] = angle * cos(phi); |
| 1893 | |
| 1894 | double rotation_matrix[9]; |
| 1895 | AngleAxisToRotationMatrix(angle_axis, rotation_matrix); |
| 1896 | |
| 1897 | double angle_axis_round_trip[3]; |
| 1898 | RotationMatrixToAngleAxis(rotation_matrix, angle_axis_round_trip); |
| 1899 | EXPECT_THAT(angle_axis_round_trip, IsNearAngleAxis(angle_axis)); |
| 1900 | } |
| 1901 | |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 1902 | TEST(RotationMatrixToAngleAxis, ExhaustiveRoundTrip) { |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1903 | constexpr double kMaxSmallAngle = 1e-8; |
| 1904 | std::mt19937 prng; |
| 1905 | std::uniform_real_distribution<double> uniform_distribution1{ |
| 1906 | kPi - kMaxSmallAngle, kPi}; |
| 1907 | std::uniform_real_distribution<double> uniform_distribution2{ |
| 1908 | -1.0, 2.0 * kMaxSmallAngle - 1.0}; |
Sameer Agarwal | 993622e | 2014-11-14 07:31:31 -0800 | [diff] [blame] | 1909 | const int kNumSteps = 1000; |
| 1910 | for (int i = 0; i < kNumSteps; ++i) { |
| 1911 | const double theta = static_cast<double>(i) / kNumSteps * 2.0 * kPi; |
| 1912 | for (int j = 0; j < kNumSteps; ++j) { |
| 1913 | const double phi = static_cast<double>(j) / kNumSteps * kPi; |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 1914 | // Rotations of angle Pi. |
| 1915 | CheckRotationMatrixToAngleAxisRoundTrip(theta, phi, kPi); |
| 1916 | // Rotation of angle approximately Pi. |
Sameer Agarwal | 0484fab | 2014-11-13 23:08:30 -0800 | [diff] [blame] | 1917 | CheckRotationMatrixToAngleAxisRoundTrip( |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1918 | theta, phi, uniform_distribution1(prng)); |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 1919 | // Rotations of angle approximately zero. |
Sameer Agarwal | 0484fab | 2014-11-13 23:08:30 -0800 | [diff] [blame] | 1920 | CheckRotationMatrixToAngleAxisRoundTrip( |
Sergiu Deitsch | f1dfac8 | 2022-08-08 21:06:22 +0200 | [diff] [blame] | 1921 | theta, phi, uniform_distribution2(prng)); |
Sameer Agarwal | 730aa53 | 2014-11-13 15:51:10 -0800 | [diff] [blame] | 1922 | } |
| 1923 | } |
| 1924 | } |
| 1925 | |
Keir Mierle | 8ebb073 | 2012-04-30 23:09:08 -0700 | [diff] [blame] | 1926 | } // namespace internal |
| 1927 | } // namespace ceres |