blob: ffa5ff39041b425b69e06b2e1ebc982a9b180f0d [file] [log] [blame]
Keir Mierle8ebb0732012-04-30 23:09:08 -07001// Ceres Solver - A fast non-linear least squares minimizer
Sergiu Deitschcb6b3062023-01-05 21:37:36 +01002// Copyright 2023 Google Inc. All rights reserved.
Keir Mierle7492b0d2015-03-17 22:30:16 -07003// http://ceres-solver.org/
Keir Mierle8ebb0732012-04-30 23:09:08 -07004//
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 Demmel7b8f6752020-09-20 21:45:24 +020031#include "ceres/rotation.h"
32
Sameer Agarwal749a4422023-01-16 07:38:05 -080033#include <algorithm>
Sameer Agarwal79a554f2023-01-13 11:37:27 -080034#include <array>
Keir Mierle8ebb0732012-04-30 23:09:08 -070035#include <cmath>
36#include <limits>
Sergiu Deitschf1dfac82022-08-08 21:06:22 +020037#include <random>
Keir Mierle8ebb0732012-04-30 23:09:08 -070038#include <string>
Sameer Agarwald340f812023-04-10 16:43:30 -070039#include <utility>
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +020040
Sameer Agarwal0a53aa92024-07-07 10:24:18 -070041#include "absl/log/log.h"
hs293go2b89ce62021-11-23 15:39:14 -050042#include "ceres/constants.h"
Keir Mierle8ebb0732012-04-30 23:09:08 -070043#include "ceres/internal/eigen.h"
hs293go2b89ce62021-11-23 15:39:14 -050044#include "ceres/internal/euler_angles.h"
Sergiu Deitschf90833f2022-02-07 23:43:19 +010045#include "ceres/internal/export.h"
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +020046#include "ceres/is_close.h"
Keir Mierle8ebb0732012-04-30 23:09:08 -070047#include "ceres/jet.h"
Sameer Agarwal0beab862012-08-13 15:12:01 -070048#include "ceres/stringprintf.h"
49#include "ceres/test_util.h"
Sameer Agarwal0beab862012-08-13 15:12:01 -070050#include "gmock/gmock.h"
51#include "gtest/gtest.h"
Keir Mierle8ebb0732012-04-30 23:09:08 -070052
53namespace ceres {
54namespace internal {
55
Sergiu Deitsch4519b8d2023-10-04 22:45:42 +020056inline constexpr double kPi = constants::pi;
Keir Mierleefe7ac62012-06-24 22:25:28 -070057const double kHalfSqrt2 = 0.707106781186547524401;
58
Keir Mierle8ebb0732012-04-30 23:09:08 -070059// A tolerance value for floating-point comparisons.
Alexander Ivanov53df5dd2023-01-11 16:51:38 +000060static double const kTolerance = std::numeric_limits<double>::epsilon() * 10;
Keir Mierle8ebb0732012-04-30 23:09:08 -070061
Keir Mierle3e2c4ef2013-02-17 12:37:55 -080062// Looser tolerance used for numerically unstable conversions.
Keir Mierleefe7ac62012-06-24 22:25:28 -070063static double const kLooseTolerance = 1e-9;
Keir Mierle8ebb0732012-04-30 23:09:08 -070064
65// Use as:
66// double quaternion[4];
67// EXPECT_THAT(quaternion, IsNormalizedQuaternion());
68MATCHER(IsNormalizedQuaternion, "") {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +020069 double norm2 =
70 arg[0] * arg[0] + arg[1] * arg[1] + arg[2] * arg[2] + arg[3] * arg[3];
Keir Mierle8ebb0732012-04-30 23:09:08 -070071 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));
83MATCHER_P(IsNearQuaternion, expected, "") {
Sameer Agarwal383c04f2012-08-17 10:14:04 -070084 // Quaternions are equivalent upto a sign change. So we will compare
85 // both signs before declaring failure.
Sergiu Deitschf0851662022-02-17 00:56:27 +010086 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 Mierle8ebb0732012-04-30 23:09:08 -070089 for (int i = 0; i < 4; i++) {
90 if (fabs(arg[i] - expected[i]) > kTolerance) {
Sergiu Deitschf0851662022-02-17 00:56:27 +010091 is_near = false;
Sameer Agarwal383c04f2012-08-17 10:14:04 -070092 break;
Keir Mierle8ebb0732012-04-30 23:09:08 -070093 }
94 }
95
Sergiu Deitschf0851662022-02-17 00:56:27 +010096 if (is_near) {
Sameer Agarwal383c04f2012-08-17 10:14:04 -070097 return true;
98 }
99
Sergiu Deitschf0851662022-02-17 00:56:27 +0100100 is_near = true;
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700101 for (int i = 0; i < 4; i++) {
102 if (fabs(arg[i] + expected[i]) > kTolerance) {
Sergiu Deitschf0851662022-02-17 00:56:27 +0100103 is_near = false;
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700104 break;
105 }
106 }
107
Sergiu Deitschf0851662022-02-17 00:56:27 +0100108 if (is_near) {
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700109 return true;
110 }
111
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200112 // clang-format off
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700113 *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 Demmel7b8f6752020-09-20 21:45:24 +0200123 // clang-format on
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700124 return false;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700125}
126
127// Use as:
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700128// double expected_axis_angle[3];
129// double actual_axis_angle[3];
Keir Mierle8ebb0732012-04-30 23:09:08 -0700130// EXPECT_THAT(actual_axis_angle, IsNearAngleAxis(expected_axis_angle));
131MATCHER_P(IsNearAngleAxis, expected, "") {
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700132 Eigen::Vector3d a(arg[0], arg[1], arg[2]);
133 Eigen::Vector3d e(expected[0], expected[1], expected[2]);
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700134 const double e_norm = e.norm();
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700135
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000136 double delta_norm = std::numeric_limits<double>::max();
Sameer Agarwal730aa532014-11-13 15:51:10 -0800137 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 Ivanov53df5dd2023-01-11 16:51:38 +0000141 delta_norm = std::min((a - e).norm(), (a + e).norm()) / e_norm;
Sameer Agarwal730aa532014-11-13 15:51:10 -0800142 } 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 Agarwalcd358c72014-10-29 17:31:40 -0700150 return true;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700151 }
152
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200153 // clang-format off
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700154 *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 Demmel7b8f6752020-09-20 21:45:24 +0200162 // clang-format on
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700163 return false;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700164}
165
166// Use as:
167// double matrix[9];
168// EXPECT_THAT(matrix, IsOrthonormal());
169MATCHER(IsOrthonormal, "") {
Keir Mierle8ebb0732012-04-30 23:09:08 -0700170 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));
193MATCHER_P(IsNear3x3Matrix, expected, "") {
Keir Mierle8ebb0732012-04-30 23:09:08 -0700194 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.
205TEST(Rotation, ZeroAngleAxisToQuaternion) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200206 double axis_angle[3] = {0, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700207 double quaternion[4];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200208 double expected[4] = {1, 0, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700209 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.
215TEST(Rotation, SmallAngleAxisToQuaternion) {
216 // Small, finite value to test.
217 double theta = 1.0e-2;
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200218 double axis_angle[3] = {theta, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700219 double quaternion[4];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200220 double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700221 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.
227TEST(Rotation, TinyAngleAxisToQuaternion) {
228 // Very small value that could potentially cause underflow.
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000229 double theta = pow(std::numeric_limits<double>::min(), 0.75);
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200230 double axis_angle[3] = {theta, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700231 double quaternion[4];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200232 double expected[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700233 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.
239TEST(Rotation, XRotationToQuaternion) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200240 double axis_angle[3] = {kPi / 2, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700241 double quaternion[4];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200242 double expected[4] = {kHalfSqrt2, kHalfSqrt2, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700243 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.
249TEST(Rotation, UnitQuaternionToAngleAxis) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200250 double quaternion[4] = {1, 0, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700251 double axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200252 double expected[3] = {0, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700253 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.
258TEST(Rotation, YRotationQuaternionToAngleAxis) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200259 double quaternion[4] = {0, 0, 1, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700260 double axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200261 double expected[3] = {0, kPi, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700262 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.
268TEST(Rotation, ZRotationQuaternionToAngleAxis) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200269 double quaternion[4] = {sqrt(3) / 2, 0, 0, 0.5};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700270 double axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200271 double expected[3] = {0, 0, kPi / 3};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700272 QuaternionToAngleAxis(quaternion, axis_angle);
273 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
274}
275
276// Test that exact conversion works for small angles.
277TEST(Rotation, SmallQuaternionToAngleAxis) {
278 // Small, finite value to test.
279 double theta = 1.0e-2;
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200280 double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700281 double axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200282 double expected[3] = {theta, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700283 QuaternionToAngleAxis(quaternion, axis_angle);
284 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
285}
286
287// Test that approximate conversion works for very small angles.
288TEST(Rotation, TinyQuaternionToAngleAxis) {
289 // Very small value that could potentially cause underflow.
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000290 double theta = pow(std::numeric_limits<double>::min(), 0.75);
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200291 double quaternion[4] = {cos(theta / 2), sin(theta / 2.0), 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700292 double axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200293 double expected[3] = {theta, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700294 QuaternionToAngleAxis(quaternion, axis_angle);
295 EXPECT_THAT(axis_angle, IsNearAngleAxis(expected));
296}
297
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700298TEST(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 Deitschcb6b3062023-01-05 21:37:36 +0100309 const double angle = std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
Sameer Agarwal383c04f2012-08-17 10:14:04 -0700310 EXPECT_LE(angle, kPi);
311}
312
Sameer Agarwal57cf20a2020-04-21 10:10:01 -0700313static constexpr int kNumTrials = 10000;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700314
315// Takes a bunch of random axis/angle values, converts them to quaternions,
316// and back again.
317TEST(Rotation, AngleAxisToQuaterionAndBack) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200318 std::mt19937 prng;
319 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700320 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 Deitschc8658c82022-02-20 02:22:17 +0100325 for (double& coeff : axis_angle) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200326 coeff = uniform_distribution(prng);
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100327 norm += coeff * coeff;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700328 }
329 norm = sqrt(norm);
330
331 // Angle in [-pi, pi).
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200332 double theta = uniform_distribution(
333 prng, std::uniform_real_distribution<double>::param_type{-kPi, kPi});
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100334 for (double& coeff : axis_angle) {
335 coeff = coeff * theta / norm;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700336 }
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.
352TEST(Rotation, QuaterionToAngleAxisAndBack) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200353 std::mt19937 prng;
354 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700355 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 Deitschc8658c82022-02-20 02:22:17 +0100359 for (double& coeff : quaternion) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200360 coeff = uniform_distribution(prng);
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100361 norm += coeff * coeff;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700362 }
363 norm = sqrt(norm);
364
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100365 for (double& coeff : quaternion) {
366 coeff = coeff / norm;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700367 }
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.
379TEST(Rotation, ZeroAngleAxisToRotationMatrix) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200380 double axis_angle[3] = {0, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700381 double matrix[9];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200382 double expected[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700383 AngleAxisToRotationMatrix(axis_angle, matrix);
384 EXPECT_THAT(matrix, IsOrthonormal());
385 EXPECT_THAT(matrix, IsNear3x3Matrix(expected));
386}
387
388TEST(Rotation, NearZeroAngleAxisToRotationMatrix) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200389 double axis_angle[3] = {1e-24, 2e-24, 3e-24};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700390 double matrix[9];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200391 double expected[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700392 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.
398TEST(Rotation, XRotationToRotationMatrix) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200399 double axis_angle[3] = {kPi / 2, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700400 double matrix[9];
401 // The rotation matrices are stored column-major.
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200402 double expected[9] = {1, 0, 0, 0, 0, 1, 0, -1, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700403 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.
413TEST(Rotation, YRotationToRotationMatrix) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200414 double axis_angle[3] = {0, kPi, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700415 double matrix[9];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200416 double expected[9] = {-1, 0, 0, 0, 1, 0, 0, 0, -1};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700417 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
426TEST(Rotation, NearPiAngleAxisRoundTrip) {
427 double in_axis_angle[3];
428 double matrix[9];
429 double out_axis_angle[3];
430
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200431 std::mt19937 prng;
432 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700433 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 Deitschc8658c82022-02-20 02:22:17 +0100437 for (double& coeff : in_axis_angle) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200438 coeff = uniform_distribution(prng);
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100439 norm += coeff * coeff;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700440 }
441 norm = sqrt(norm);
442
443 // Angle in [pi - kMaxSmallAngle, pi).
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200444 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 Mierle8ebb0732012-04-30 23:09:08 -0700449
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100450 for (double& coeff : in_axis_angle) {
451 coeff *= (theta / norm);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700452 }
453 AngleAxisToRotationMatrix(in_axis_angle, matrix);
454 RotationMatrixToAngleAxis(matrix, out_axis_angle);
Sameer Agarwalcd358c72014-10-29 17:31:40 -0700455 EXPECT_THAT(in_axis_angle, IsNearAngleAxis(out_axis_angle));
Keir Mierle8ebb0732012-04-30 23:09:08 -0700456 }
457}
458
459TEST(Rotation, AtPiAngleAxisRoundTrip) {
Keir Mierleefe7ac62012-06-24 22:25:28 -0700460 // A rotation of kPi about the X axis;
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200461 // clang-format off
Sameer Agarwal57cf20a2020-04-21 10:10:01 -0700462 static constexpr double kMatrix[3][3] = {
Keir Mierle8ebb0732012-04-30 23:09:08 -0700463 {1.0, 0.0, 0.0},
464 {0.0, -1.0, 0.0},
465 {0.0, 0.0, -1.0}
466 };
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200467 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -0700468
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 Demmel7b8f6752020-09-20 21:45:24 +0200472 for (int i = 0; i < 3; ++i, ++k) {
473 in_matrix[k] = kMatrix[i][j];
474 }
Keir Mierle8ebb0732012-04-30 23:09:08 -0700475 }
476
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200477 const double expected_axis_angle[3] = {kPi, 0, 0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700478
479 double out_matrix[9];
480 double axis_angle[3];
481 RotationMatrixToAngleAxis(in_matrix, axis_angle);
482 AngleAxisToRotationMatrix(axis_angle, out_matrix);
483
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200484 LOG(INFO) << "AngleAxis = " << axis_angle[0] << " " << axis_angle[1] << " "
485 << axis_angle[2];
Keir Mierleefe7ac62012-06-24 22:25:28 -0700486 LOG(INFO) << "Expected AngleAxis = " << kPi << " 0 0";
Keir Mierle8ebb0732012-04-30 23:09:08 -0700487 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 Agarwalcd358c72014-10-29 17:31:40 -0700494 LOG(INFO) << "EXPECTED | ACTUAL";
Keir Mierle8ebb0732012-04-30 23:09:08 -0700495 for (int i = 0; i < 3; ++i) {
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000496 std::string line;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700497 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.
513TEST(Rotation, ZRotationToRotationMatrix) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200514 double axis_angle[3] = {0, 0, kPi / 3};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700515 double matrix[9];
516 // This is laid-out row-major on the screen but is actually stored
517 // column-major.
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +0200518 // clang-format off
Keir Mierle8ebb0732012-04-30 23:09:08 -0700519 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 Demmel7b8f6752020-09-20 21:45:24 +0200522 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -0700523 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.
533TEST(Rotation, AngleAxisToRotationMatrixAndBack) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200534 std::mt19937 prng;
535 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700536 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 Deitschc8658c82022-02-20 02:22:17 +0100541 for (double& i : axis_angle) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200542 i = uniform_distribution(prng);
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100543 norm += i * i;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700544 }
545 norm = sqrt(norm);
546
547 // Angle in [-pi, pi).
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200548 double theta = uniform_distribution(
549 prng, std::uniform_real_distribution<double>::param_type{-kPi, kPi});
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100550 for (double& i : axis_angle) {
551 i = i * theta / norm;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700552 }
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 Agarwal21d6a992013-10-25 10:20:24 -0700566// Takes a bunch of random axis/angle values near zero, converts them
567// to rotation matrices, and back again.
568TEST(Rotation, AngleAxisToRotationMatrixAndBackNearZero) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200569 std::mt19937 prng;
570 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Sameer Agarwal21d6a992013-10-25 10:20:24 -0700571 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 Deitschc8658c82022-02-20 02:22:17 +0100576 for (double& i : axis_angle) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200577 i = uniform_distribution(prng);
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100578 norm += i * i;
Sameer Agarwal21d6a992013-10-25 10:20:24 -0700579 }
580 norm = sqrt(norm);
581
582 // Tiny theta.
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200583 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 Deitschc8658c82022-02-20 02:22:17 +0100588 for (double& i : axis_angle) {
589 i = i * theta / norm;
Sameer Agarwal21d6a992013-10-25 10:20:24 -0700590 }
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 Demmel7b8f6752020-09-20 21:45:24 +0200599 EXPECT_NEAR(
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000600 round_trip[i], axis_angle[i], std::numeric_limits<double>::epsilon());
Sameer Agarwal21d6a992013-10-25 10:20:24 -0700601 }
602 }
603}
604
Keir Mierle8ebb0732012-04-30 23:09:08 -0700605// Transposes a 3x3 matrix.
606static void Transpose3x3(double m[9]) {
Alexander Ivanov53df5dd2023-01-11 16:51:38 +0000607 std::swap(m[1], m[3]);
608 std::swap(m[2], m[6]);
609 std::swap(m[5], m[7]);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700610}
611
612// Convert Euler angles from radians to degrees.
Keir Mierlef4ba28d2016-03-16 11:35:28 -0700613static void ToDegrees(double euler_angles[3]) {
614 for (int i = 0; i < 3; ++i) {
615 euler_angles[i] *= 180.0 / kPi;
616 }
Keir Mierle8ebb0732012-04-30 23:09:08 -0700617}
618
619// Compare the 3x3 rotation matrices produced by the axis-angle
620// rotation 'aa' and the Euler angle rotation 'ea' (in radians).
621static 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.
638TEST(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 Demmel7b8f6752020-09-20 21:45:24 +0200643 if ((x != 0) + (y != 0) + (z != 0) > 1) continue;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700644 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 Agarwal0a53aa92024-07-07 10:24:18 -0700651 ASSERT_EQ(7, n_tests);
Keir Mierle8ebb0732012-04-30 23:09:08 -0700652}
653
654// Test that a random rotation produces an orthonormal rotation
655// matrix.
656TEST(EulerAnglesToRotationMatrix, IsOrthonormal) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200657 std::mt19937 prng;
658 std::uniform_real_distribution<double> uniform_distribution{-180.0, 180.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -0700659 for (int trial = 0; trial < kNumTrials; ++trial) {
Keir Mierlef4ba28d2016-03-16 11:35:28 -0700660 double euler_angles_degrees[3];
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100661 for (double& euler_angles_degree : euler_angles_degrees) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +0200662 euler_angles_degree = uniform_distribution(prng);
Keir Mierlef4ba28d2016-03-16 11:35:28 -0700663 }
664 double rotation_matrix[9];
665 EulerAnglesToRotationMatrix(euler_angles_degrees, 3, rotation_matrix);
666 EXPECT_THAT(rotation_matrix, IsOrthonormal());
Keir Mierle8ebb0732012-04-30 23:09:08 -0700667 }
668}
669
hs293go2b89ce62021-11-23 15:39:14 -0500670static 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
676TEST(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
699TEST(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
722TEST(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
744TEST(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
764template <typename T>
765struct 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
826using 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>;
850TYPED_TEST_SUITE(GeneralEulerAngles, EulerSystemList);
851
852TYPED_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)
872TYPED_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.
888TYPED_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 Mierle8ebb0732012-04-30 23:09:08 -0700908// Tests using Jets for specific behavior involving auto differentiation
909// near singularity points.
910
Sergiu Deitschc8658c82022-02-20 02:22:17 +0100911using J3 = Jet<double, 3>;
912using J4 = Jet<double, 4>;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700913
Sergey Sharybin54ba6c22017-12-23 18:18:24 +0100914namespace {
915
Hs293Go96fdfd22023-04-22 02:48:39 -0400916// Converts an array of N real numbers (doubles) to an array of jets
917template <int N>
918void 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
927template <int N, typename T, typename... Ts>
928Jet<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 Mierle8ebb0732012-04-30 23:09:08 -0700935J3 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
944J4 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 Sharybin54ba6c22017-12-23 18:18:24 +0100954} // namespace
955
Hs293Go96fdfd22023-04-22 02:48:39 -0400956// Use EXPECT_THAT(x, testing::PointWise(JetClose(prec), y); to achieve Jet
957// array comparison
958MATCHER_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 Mierle0149ce02016-03-16 11:09:53 -0700978 return false;
979 }
Hs293Go96fdfd22023-04-22 02:48:39 -0400980 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 Mierle0149ce02016-03-16 11:09:53 -0700992 return false;
Keir Mierle8ebb0732012-04-30 23:09:08 -0700993 }
994 }
995 return true;
996}
997
Keir Mierle8ebb0732012-04-30 23:09:08 -0700998// Log-10 of a value well below machine precision.
Sameer Agarwal79a554f2023-01-13 11:37:27 -0800999static const int kSmallTinyCutoff = static_cast<int>(
1000 2 * log(std::numeric_limits<double>::epsilon()) / log(10.0));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001001
1002// Log-10 of a value just below values representable by double.
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001003static const int kTinyZeroLimit =
Alexander Ivanov53df5dd2023-01-11 16:51:38 +00001004 static_cast<int>(1 + log(std::numeric_limits<double>::min()) / log(10.0));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001005
1006// Test that exact conversion works for small angles when jets are used.
1007TEST(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 Demmel7b8f6752020-09-20 21:45:24 +02001012 J3 axis_angle[3] = {J3(theta, 0), J3(0, 1), J3(0, 2)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001013 J3 quaternion[4];
1014 J3 expected[4] = {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001015 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 Mierle8ebb0732012-04-30 23:09:08 -07001019 };
1020 AngleAxisToQuaternion(axis_angle, quaternion);
Hs293Go96fdfd22023-04-22 02:48:39 -04001021 EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001022 }
1023}
1024
Keir Mierle8ebb0732012-04-30 23:09:08 -07001025// Test that conversion works for very small angles when jets are used.
1026TEST(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 Demmel7b8f6752020-09-20 21:45:24 +02001031 J3 axis_angle[3] = {J3(theta, 0), J3(0, 1), J3(0, 2)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001032 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);
Hs293Go96fdfd22023-04-22 02:48:39 -04001043 EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001044 }
1045}
1046
1047// Test that derivatives are correct for zero rotation.
1048TEST(Rotation, ZeroAngleAxisToQuaternionForJets) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001049 J3 axis_angle[3] = {J3(0, 0), J3(0, 1), J3(0, 2)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001050 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);
Hs293Go96fdfd22023-04-22 02:48:39 -04001058 EXPECT_THAT(quaternion, testing::Pointwise(JetClose(kTolerance), expected));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001059}
1060
1061// Test that exact conversion works for small angles.
1062TEST(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 Demmel7b8f6752020-09-20 21:45:24 +02001069 J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001070 J4 axis_angle[3];
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001071 // clang-format off
Keir Mierle8ebb0732012-04-30 23:09:08 -07001072 J4 expected[3] = {
Keir Mierle0149ce02016-03-16 11:09:53 -07001073 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 Mierle8ebb0732012-04-30 23:09:08 -07001076 };
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001077 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -07001078 QuaternionToAngleAxis(quaternion, axis_angle);
Hs293Go96fdfd22023-04-22 02:48:39 -04001079 EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001080 }
1081}
1082
1083// Test that conversion works for very small angles.
1084TEST(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 Demmel7b8f6752020-09-20 21:45:24 +02001091 J4 quaternion[4] = {J4(c, 0), J4(s, 1), J4(0, 2), J4(0, 3)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001092 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 Demmel7b8f6752020-09-20 21:45:24 +02001096 // clang-format off
Keir Mierle8ebb0732012-04-30 23:09:08 -07001097 J4 expected[3] = {
Keir Mierle0149ce02016-03-16 11:09:53 -07001098 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 Mierle8ebb0732012-04-30 23:09:08 -07001101 };
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001102 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -07001103 QuaternionToAngleAxis(quaternion, axis_angle);
Hs293Go96fdfd22023-04-22 02:48:39 -04001104 EXPECT_THAT(axis_angle, testing::Pointwise(JetClose(kTolerance), expected));
Keir Mierle8ebb0732012-04-30 23:09:08 -07001105 }
1106}
1107
1108// Test that conversion works for no rotation.
1109TEST(Rotation, ZeroQuaternionToAngleAxisForJets) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001110 J4 quaternion[4] = {J4(1, 0), J4(0, 1), J4(0, 2), J4(0, 3)};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001111 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);
Hs293Go96fdfd22023-04-22 02:48:39 -04001118 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
1135TEST(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
1201TEST(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
1269TEST(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
1335TEST(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
1400using 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
1418static 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
1428TEST(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
1476TEST(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
1527TEST(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
1575TEST(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 Mierle8ebb0732012-04-30 23:09:08 -07001620}
1621
1622TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrixCanned) {
1623 // Canned data generated in octave.
1624 double const q[4] = {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001625 +0.1956830471754074,
1626 -0.0150618562474847,
1627 +0.7634572982788086,
1628 -0.3019454777240753,
Keir Mierle8ebb0732012-04-30 23:09:08 -07001629 };
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001630 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 Mierle8ebb0732012-04-30 23:09:08 -07001635 };
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001636 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 Mierle8ebb0732012-04-30 23:09:08 -07001641 };
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 Mierle8ebb0732012-04-30 23:09:08 -07001653TEST(Quaternion, RotatePointGivesSameAnswerAsRotationByMatrix) {
1654 // Rotation defined by a unit quaternion.
1655 double const q[4] = {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001656 +0.2318160216097109,
1657 -0.0178430356832060,
1658 +0.9044300776717159,
1659 -0.3576998641394597,
Keir Mierle8ebb0732012-04-30 23:09:08 -07001660 };
1661 double const p[3] = {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001662 +0.11,
1663 -13.15,
1664 1.17,
Keir Mierle8ebb0732012-04-30 23:09:08 -07001665 };
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 Demmel7b8f6752020-09-20 21:45:24 +02001674 VectorRef(result2, 3) = ConstMatrixRef(R, 3, 3) * ConstVectorRef(p, 3);
Keir Mierle8ebb0732012-04-30 23:09:08 -07001675 ExpectArraysClose(3, result1, result2, kTolerance);
1676}
1677
Keir Mierle8ebb0732012-04-30 23:09:08 -07001678// Verify that (a * b) * c == a * (b * c).
1679TEST(Quaternion, MultiplicationIsAssociative) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001680 std::mt19937 prng;
1681 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001682 double a[4];
1683 double b[4];
1684 double c[4];
1685 for (int i = 0; i < 4; ++i) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001686 a[i] = uniform_distribution(prng);
1687 b[i] = uniform_distribution(prng);
1688 c[i] = uniform_distribution(prng);
Keir Mierle8ebb0732012-04-30 23:09:08 -07001689 }
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 Mierle8ebb0732012-04-30 23:09:08 -07001707TEST(AngleAxis, RotatePointGivesSameAnswerAsRotationMatrix) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001708 std::mt19937 prng;
1709 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001710 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 Mierleefe7ac62012-06-24 22:25:28 -07001717 double theta = (2.0 * i * 0.0011 - 1.0) * kPi;
Keir Mierle8ebb0732012-04-30 23:09:08 -07001718 for (int j = 0; j < 50; ++j) {
Keir Mierle8ebb0732012-04-30 23:09:08 -07001719 for (int k = 0; k < 3; ++k) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001720 angle_axis[k] = uniform_distribution(prng);
1721 p[k] = uniform_distribution(prng);
Keir Mierle8ebb0732012-04-30 23:09:08 -07001722 }
1723
Sergiu Deitschcb6b3062023-01-05 21:37:36 +01001724 const double inv_norm =
1725 theta / std::hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
Sergiu Deitschc8658c82022-02-20 02:22:17 +01001726 for (double& angle_axi : angle_axis) {
1727 angle_axi *= inv_norm;
Keir Mierle8ebb0732012-04-30 23:09:08 -07001728 }
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 Demmel7b8f6752020-09-20 21:45:24 +02001737 // clang-format off
Keir Mierle8ebb0732012-04-30 23:09:08 -07001738 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 Demmel7b8f6752020-09-20 21:45:24 +02001746 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -07001747 }
1748 }
1749 }
1750}
1751
Sameer Agarwal79a554f2023-01-13 11:37:27 -08001752TEST(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 Agarwalc4ba9752023-01-14 06:08:10 -08001757 std::array<Jet, 3> rotated_point;
Sameer Agarwal79a554f2023-01-13 11:37:27 -08001758 QuaternionRotatePoint(quaternion.data(), point.data(), rotated_point.data());
Sameer Agarwal79a554f2023-01-13 11:37:27 -08001759 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 Mierle8ebb0732012-04-30 23:09:08 -07001765TEST(AngleAxis, NearZeroRotatePointGivesSameAnswerAsRotationMatrix) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001766 std::mt19937 prng;
1767 std::uniform_real_distribution<double> uniform_distribution{-1.0, 1.0};
Keir Mierle8ebb0732012-04-30 23:09:08 -07001768 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 Deitschf1dfac82022-08-08 21:06:22 +02001777 angle_axis[k] = uniform_distribution(prng);
1778 p[k] = uniform_distribution(prng);
Keir Mierle8ebb0732012-04-30 23:09:08 -07001779 norm2 = angle_axis[k] * angle_axis[k];
1780 }
1781
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001782 double theta = (2.0 * i * 0.0001 - 1.0) * 1e-16;
Keir Mierle8ebb0732012-04-30 23:09:08 -07001783 const double inv_norm = theta / sqrt(norm2);
Sergiu Deitschc8658c82022-02-20 02:22:17 +01001784 for (double& angle_axi : angle_axis) {
1785 angle_axi *= inv_norm;
Keir Mierle8ebb0732012-04-30 23:09:08 -07001786 }
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 Demmel7b8f6752020-09-20 21:45:24 +02001795 // clang-format off
Keir Mierle8ebb0732012-04-30 23:09:08 -07001796 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 Demmel7b8f6752020-09-20 21:45:24 +02001804 // clang-format on
Keir Mierle8ebb0732012-04-30 23:09:08 -07001805 }
1806 }
1807}
1808
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001809TEST(MatrixAdapter, RowMajor3x3ReturnTypeAndAccessIsCorrect) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001810 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 Mierle3e2c4ef2013-02-17 12:37:55 -08001813 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 Demmel7b8f6752020-09-20 21:45:24 +02001820 EXPECT_EQ(A(i, j), array[3 * i + j]);
1821 EXPECT_EQ(B(i, j), const_array[3 * i + j]);
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001822 }
1823 }
1824}
1825
1826TEST(MatrixAdapter, ColumnMajor3x3ReturnTypeAndAccessIsCorrect) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001827 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 Mierle3e2c4ef2013-02-17 12:37:55 -08001830 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 Agarwal509f68c2013-02-20 01:39:03 -08001835 // The values are integers from 1 to 9, so equality tests are
1836 // appropriate even for float and double values.
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001837 EXPECT_EQ(A(i, j), array[3 * j + i]);
1838 EXPECT_EQ(B(i, j), const_array[3 * j + i]);
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001839 }
1840 }
1841}
1842
1843TEST(MatrixAdapter, RowMajor2x4IsCorrect) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001844 const int expected[8] = {1, 2, 3, 4, 5, 6, 7, 8};
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001845 int array[8];
1846 MatrixAdapter<int, 4, 1> M(array);
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001847 // clang-format off
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001848 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 Demmel7b8f6752020-09-20 21:45:24 +02001850 // clang-format on
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001851 for (int k = 0; k < 8; ++k) {
1852 EXPECT_EQ(array[k], expected[k]);
1853 }
1854}
1855
1856TEST(MatrixAdapter, ColumnMajor2x4IsCorrect) {
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001857 const int expected[8] = {1, 5, 2, 6, 3, 7, 4, 8};
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001858 int array[8];
1859 MatrixAdapter<int, 1, 2> M(array);
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001860 // clang-format off
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001861 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 Demmel7b8f6752020-09-20 21:45:24 +02001863 // clang-format on
Keir Mierle3e2c4ef2013-02-17 12:37:55 -08001864 for (int k = 0; k < 8; ++k) {
1865 EXPECT_EQ(array[k], expected[k]);
1866 }
1867}
Keir Mierle8ebb0732012-04-30 23:09:08 -07001868
Sameer Agarwalcd358c72014-10-29 17:31:40 -07001869TEST(RotationMatrixToAngleAxis, NearPiExampleOneFromTobiasStrauss) {
1870 // Example from Tobias Strauss
Nikolaus Demmel7b8f6752020-09-20 21:45:24 +02001871 // clang-format off
Sameer Agarwalcd358c72014-10-29 17:31:40 -07001872 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 Demmel7b8f6752020-09-20 21:45:24 +02001877 // clang-format on
Sameer Agarwalcd358c72014-10-29 17:31:40 -07001878
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 Sharybin54ba6c22017-12-23 18:18:24 +01001886static void CheckRotationMatrixToAngleAxisRoundTrip(const double theta,
1887 const double phi,
1888 const double angle) {
Sameer Agarwalcd358c72014-10-29 17:31:40 -07001889 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 Agarwal730aa532014-11-13 15:51:10 -08001902TEST(RotationMatrixToAngleAxis, ExhaustiveRoundTrip) {
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001903 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 Agarwal993622e2014-11-14 07:31:31 -08001909 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 Agarwal730aa532014-11-13 15:51:10 -08001914 // Rotations of angle Pi.
1915 CheckRotationMatrixToAngleAxisRoundTrip(theta, phi, kPi);
1916 // Rotation of angle approximately Pi.
Sameer Agarwal0484fab2014-11-13 23:08:30 -08001917 CheckRotationMatrixToAngleAxisRoundTrip(
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001918 theta, phi, uniform_distribution1(prng));
Sameer Agarwal730aa532014-11-13 15:51:10 -08001919 // Rotations of angle approximately zero.
Sameer Agarwal0484fab2014-11-13 23:08:30 -08001920 CheckRotationMatrixToAngleAxisRoundTrip(
Sergiu Deitschf1dfac82022-08-08 21:06:22 +02001921 theta, phi, uniform_distribution2(prng));
Sameer Agarwal730aa532014-11-13 15:51:10 -08001922 }
1923 }
1924}
1925
Keir Mierle8ebb0732012-04-30 23:09:08 -07001926} // namespace internal
1927} // namespace ceres