1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <gtest/gtest.h>
18
19 #include "media/QuaternionUtil.h"
20 #include "TestUtil.h"
21
22 using Eigen::Quaternionf;
23 using Eigen::Vector3f;
24
25 namespace android {
26 namespace media {
27 namespace {
28
TEST(QuaternionUtil,RotationVectorToQuaternion)29 TEST(QuaternionUtil, RotationVectorToQuaternion) {
30 // 90 degrees around Z.
31 Vector3f rot = {0, 0, M_PI_2};
32 Quaternionf quat = rotationVectorToQuaternion(rot);
33 ASSERT_EQ(quat * Vector3f(1, 0, 0), Vector3f(0, 1, 0));
34 ASSERT_EQ(quat * Vector3f(0, 1, 0), Vector3f(-1, 0, 0));
35 ASSERT_EQ(quat * Vector3f(0, 0, 1), Vector3f(0, 0, 1));
36 }
37
TEST(QuaternionUtil,QuaternionToRotationVector)38 TEST(QuaternionUtil, QuaternionToRotationVector) {
39 Quaternionf quat = Quaternionf::FromTwoVectors(Vector3f(1, 0, 0), Vector3f(0, 1, 0));
40 Vector3f rot = quaternionToRotationVector(quat);
41 ASSERT_EQ(rot, Vector3f(0, 0, M_PI_2));
42 }
43
TEST(QuaternionUtil,RoundTripFromQuaternion)44 TEST(QuaternionUtil, RoundTripFromQuaternion) {
45 Quaternionf quaternion = Quaternionf::UnitRandom();
46 EXPECT_EQ(quaternion, rotationVectorToQuaternion(quaternionToRotationVector(quaternion)));
47 }
48
TEST(QuaternionUtil,RoundTripFromVector)49 TEST(QuaternionUtil, RoundTripFromVector) {
50 Vector3f vec{0.1, 0.2, 0.3};
51 EXPECT_EQ(vec, quaternionToRotationVector(rotationVectorToQuaternion(vec)));
52 }
53
54 // Float precision necessitates this precision (1e-4f fails)
55 constexpr float NEAR = 1e-3f;
56
TEST(QuaternionUtil,quaternionToAngles_basic)57 TEST(QuaternionUtil, quaternionToAngles_basic) {
58 float pitch, roll, yaw;
59
60 // angles as reported.
61 // choose 11 angles between -M_PI / 2 to M_PI / 2
62 for (int step = -5; step <= 5; ++step) {
63 const float angle = M_PI * step * 0.1f;
64
65 quaternionToAngles(rotationVectorToQuaternion({angle, 0.f, 0.f}), &pitch, &roll, &yaw);
66 EXPECT_NEAR(angle, pitch, NEAR);
67 EXPECT_NEAR(0.f, roll, NEAR);
68 EXPECT_NEAR(0.f, yaw, NEAR);
69
70 quaternionToAngles(rotationVectorToQuaternion({0.f, angle, 0.f}), &pitch, &roll, &yaw);
71 EXPECT_NEAR(0.f, pitch, NEAR);
72 EXPECT_NEAR(angle, roll, NEAR);
73 EXPECT_NEAR(0.f, yaw, NEAR);
74
75 quaternionToAngles(rotationVectorToQuaternion({0.f, 0.f, angle}), &pitch, &roll, &yaw);
76 EXPECT_NEAR(0.f, pitch, NEAR);
77 EXPECT_NEAR(0.f, roll, NEAR);
78 EXPECT_NEAR(angle, yaw, NEAR);
79 }
80
81 // Generates a debug string
82 const std::string s = quaternionToAngles<true /* DEBUG */>(
83 rotationVectorToQuaternion({M_PI, 0.f, 0.f}), &pitch, &roll, &yaw);
84 ASSERT_FALSE(s.empty());
85 }
86
TEST(QuaternionUtil,quaternionToAngles_zaxis)87 TEST(QuaternionUtil, quaternionToAngles_zaxis) {
88 float pitch, roll, yaw;
89
90 for (int rot_step = -10; rot_step <= 10; ++rot_step) {
91 const float rot_angle = M_PI * rot_step * 0.1f;
92 // pitch independent of world Z rotation
93
94 // We don't test the boundaries of pitch +-M_PI/2 as roll can become
95 // degenerate and atan(0, 0) may report 0, PI, or -PI.
96 for (int step = -4; step <= 4; ++step) {
97 const float angle = M_PI * step * 0.1f;
98 auto q = rotationVectorToQuaternion({angle, 0.f, 0.f});
99 auto world_z = rotationVectorToQuaternion({0.f, 0.f, rot_angle});
100
101 // Sequential active rotations (on world frame) compose as R_2 * R_1.
102 quaternionToAngles(world_z * q, &pitch, &roll, &yaw);
103
104 EXPECT_NEAR(angle, pitch, NEAR);
105 EXPECT_NEAR(0.f, roll, NEAR);
106 }
107
108 // roll independent of world Z rotation
109 for (int step = -5; step <= 5; ++step) {
110 const float angle = M_PI * step * 0.1f;
111 auto q = rotationVectorToQuaternion({0.f, angle, 0.f});
112 auto world_z = rotationVectorToQuaternion({0.f, 0.f, rot_angle});
113
114 // Sequential active rotations (on world frame) compose as R_2 * R_1.
115 quaternionToAngles(world_z * q, &pitch, &roll, &yaw);
116
117 EXPECT_NEAR(0.f, pitch, NEAR);
118 EXPECT_NEAR(angle, roll, NEAR);
119
120 // Convert extrinsic (world-based) active rotations to a sequence of
121 // intrinsic rotations (each rotation based off of previous rotation
122 // frame).
123 //
124 // R_1 * R_intrinsic = R_extrinsic * R_1
125 // implies
126 // R_intrinsic = (R_1)^-1 R_extrinsic R_1
127 //
128 auto world_z_intrinsic = rotationVectorToQuaternion(
129 q.inverse() * Vector3f(0.f, 0.f, rot_angle));
130
131 // Sequential intrinsic rotations compose as R_1 * R_2.
132 quaternionToAngles(q * world_z_intrinsic, &pitch, &roll, &yaw);
133
134 EXPECT_NEAR(0.f, pitch, NEAR);
135 EXPECT_NEAR(angle, roll, NEAR);
136 }
137 }
138 }
139
140 } // namespace
141 } // namespace media
142 } // namespace android
143