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 #pragma once
17
18 #include <android-base/stringprintf.h>
19 #include <Eigen/Geometry>
20 #include <media/Pose.h>
21
22 namespace android {
23 namespace media {
24
25 /**
26 * Converts a rotation vector to an equivalent quaternion.
27 * The rotation vector is given as a 3-vector whose direction represents the rotation axis and its
28 * magnitude the rotation angle (in radians) around that axis.
29 */
30 Eigen::Quaternionf rotationVectorToQuaternion(const Eigen::Vector3f& rotationVector);
31
32 /**
33 * Converts a quaternion to an equivalent rotation vector.
34 * The rotation vector is given as a 3-vector whose direction represents the rotation axis and its
35 * magnitude the rotation angle (in radians) around that axis.
36 */
37 Eigen::Vector3f quaternionToRotationVector(const Eigen::Quaternionf& quaternion);
38
39 /**
40 * Returns a quaternion representing a rotation around the X-axis with the given amount (in
41 * radians).
42 */
43 Eigen::Quaternionf rotateX(float angle);
44
45 /**
46 * Returns a quaternion representing a rotation around the Y-axis with the given amount (in
47 * radians).
48 */
49 Eigen::Quaternionf rotateY(float angle);
50
51 /**
52 * Returns a quaternion representing a rotation around the Z-axis with the given amount (in
53 * radians).
54 */
55 Eigen::Quaternionf rotateZ(float angle);
56
57 /**
58 * Compute separate roll, pitch, and yaw angles from a quaternion
59 *
60 * The roll, pitch, and yaw follow standard 3DOF virtual reality definitions
61 * with angles increasing counter-clockwise by the right hand rule.
62 *
63 * https://en.wikipedia.org/wiki/Six_degrees_of_freedom
64 *
65 * The roll, pitch, and yaw angles are calculated separately from the device frame
66 * rotation from the world frame. This is not to be confused with the
67 * intrinsic Euler xyz roll, pitch, yaw 'nautical' angles.
68 *
69 * The input quarternion is the active rotation that transforms the
70 * World/Stage frame to the Head/Screen frame.
71 *
72 * The input quaternion may come from two principal sensors: DEVICE and HEADSET
73 * and are interpreted as below.
74 *
75 * DEVICE SENSOR
76 *
77 * Android sensor stack assumes device coordinates along the x/y axis.
78 *
79 * https://developer.android.com/reference/android/hardware/SensorEvent#sensor.type_rotation_vector:
80 *
81 * Looking down from the clouds. Android Device coordinate system (not used)
82 * DEVICE --> X (Y goes through top speaker towards the observer)
83 * | Z
84 * V
85 * USER
86 *
87 * Internally within this library, we transform the device sensor coordinate
88 * system by rotating the coordinate system around the X axis by -M_PI/2.
89 * This aligns the device coordinate system to match that of the
90 * Head Tracking sensor (see below), should the user be facing the device in
91 * natural (phone == portrait, tablet == ?) orientation.
92 *
93 * Looking down from the clouds. Spatializer device frame.
94 * Y
95 * ^
96 * |
97 * DEVICE --> X (Z goes through top of the DEVICE towards the observer)
98 *
99 * USER
100 *
101 * The reference world frame is the device in vertical
102 * natural (phone == portrait) orientation with the top pointing straight
103 * up from the ground and the front-to-back direction facing north.
104 * The world frame is presumed locally fixed by magnetic and gravitational reference.
105 *
106 * HEADSET SENSOR
107 * https://developer.android.com/reference/android/hardware/SensorEvent#sensor.type_head_tracker:
108 *
109 * Looking down from the clouds. Headset frame.
110 * Y
111 * ^
112 * |
113 * USER ---> X
114 * (Z goes through the top of the USER head towards the observer)
115 *
116 * The Z axis goes from the neck to the top of the head, the X axis goes
117 * from the left ear to the right ear, the Y axis goes from the back of the
118 * head through the nose.
119 *
120 * Typically for a headset sensor, the X and Y axes have some arbitrary fixed
121 * reference.
122 *
123 * ROLL
124 * Roll is the counter-clockwise L/R motion around the Y axis (hence ZX plane).
125 * The right hand convention means the plane is ZX not XZ.
126 * This can be considered the azimuth angle in spherical coordinates
127 * with Pitch being the elevation angle.
128 *
129 * Roll has a range of -M_PI to M_PI radians.
130 *
131 * Rolling a device changes between portrait and landscape
132 * modes, and for L/R speakers will limit the amount of crosstalk cancellation.
133 * Roll increases as the device (if vertical like a coin) rolls from left to right.
134 *
135 * By this definition, Roll is less accurate when the device is flat
136 * on a table rather than standing on edge.
137 * When perfectly flat on the table, roll may report as 0, M_PI, or -M_PI
138 * due ambiguity / degeneracy of atan(0, 0) in this case (the device Y axis aligns with
139 * the world Z axis), but exactly flat rarely occurs.
140 *
141 * Roll for a headset is the angle the head is inclined to the right side
142 * (like sleeping).
143 *
144 * PITCH
145 * Pitch is the Surface normal Y deviation (along the Z axis away from the earth).
146 * This can be considered the elevation angle in spherical coordinates using
147 * Roll as the azimuth angle.
148 *
149 * Pitch for a device determines whether the device is "upright" or lying
150 * flat on the table (i.e. surface normal). Pitch is 0 when upright, decreases
151 * as the device top moves away from the user to -M_PI/2 when lying down face up.
152 * Pitch increases from 0 to M_PI/2 when the device tilts towards the user, and is
153 * M_PI/2 degrees when face down.
154 *
155 * Pitch for a headset is the user tilting the head/chin up or down,
156 * like nodding.
157 *
158 * Pitch has a range of -M_PI/2, M_PI/2 radians.
159 *
160 * YAW
161 * Yaw is the rotational component along the earth's XY tangential plane,
162 * where the Z axis points radially away from the earth.
163 *
164 * Yaw has a range of -M_PI to M_PI radians. If used for azimuth angle in
165 * spherical coordinates, the elevation angle may be derived from the Z axis.
166 *
167 * A positive increase means the phone is rotating from right to left
168 * when considered flat on the table.
169 * (headset: the user is rotating their head to look left).
170 * If left speaker or right earbud is pointing straight up or down,
171 * this value is imprecise and Pitch or Roll is a more useful measure.
172 *
173 * Yaw for a device is like spinning a vertical device along the axis of
174 * gravity, like spinning a coin. Yaw increases as the coin / device
175 * spins from right to left, rotating around the Z axis.
176 *
177 * Yaw for a headset is the user turning the head to look left or right
178 * like shaking the head for no. Yaw is the primary angle for a binaural
179 * head tracking device.
180 *
181 * @param q input active rotation Eigen quaternion.
182 * @param pitch output set to pitch if not nullptr
183 * @param roll output set to roll if not nullptr
184 * @param yaw output set to yaw if not nullptr
185 * @return (DEBUG==true) a debug string with intermediate transformation matrix
186 * interpreted as the unit basis vectors.
187 */
188
189 // DEBUG returns a debug string for analysis.
190 // We save unneeded rotation matrix computation by keeping the DEBUG option constexpr.
191 template <bool DEBUG = false>
quaternionToAngles(const Eigen::Quaternionf & q,float * pitch,float * roll,float * yaw)192 auto quaternionToAngles(const Eigen::Quaternionf& q, float *pitch, float *roll, float *yaw) {
193 /*
194 * The quaternion here is the active rotation that transforms from the world frame
195 * to the device frame: the observer remains in the world frame,
196 * and the device (frame) moves.
197 *
198 * We use this to map device coordinates to world coordinates.
199 *
200 * Device: We transform the device right speaker (X == 1), top speaker (Z == 1),
201 * and surface inwards normal (Y == 1) positions to the world frame.
202 *
203 * Headset: We transform the headset right bud (X == 1), top (Z == 1) and
204 * nose normal (Y == 1) positions to the world frame.
205 *
206 * This is the same as the world frame coordinates of the
207 * unit device vector in the X dimension (ux),
208 * unit device vector in the Y dimension (uy),
209 * unit device vector in the Z dimension (uz).
210 *
211 * Rather than doing the rotation on unit vectors individually,
212 * one can simply use the columns of the rotation matrix of
213 * the world-to-body quaternion, so the computation is exceptionally fast.
214 *
215 * Furthermore, Eigen inlines the "toRotationMatrix" method
216 * and we rely on unused expression removal for efficiency
217 * and any elements not used should not be computed.
218 *
219 * Side note: For applying a rotation to several points,
220 * it is more computationally efficient to extract and
221 * use the rotation matrix form than the quaternion.
222 * So use of the rotation matrix is good for many reasons.
223 */
224 const auto rotation = q.toRotationMatrix();
225
226 /*
227 * World location of unit vector right speaker assuming the phone is situated
228 * natural (phone == portrait) mode.
229 * (headset: right bud).
230 *
231 * auto ux = q.rotation() * Eigen::Vector3f{1.f, 0.f, 0.f};
232 * = rotation.col(0);
233 */
234 [[maybe_unused]] const auto ux_0 = rotation.coeff(0, 0);
235 [[maybe_unused]] const auto ux_1 = rotation.coeff(1, 0);
236 [[maybe_unused]] const auto ux_2 = rotation.coeff(2, 0);
237
238 [[maybe_unused]] std::string coordinates;
239 if constexpr (DEBUG) {
240 base::StringAppendF(&coordinates, "ux: %f %f %f", ux_0, ux_1, ux_2);
241 }
242
243 /*
244 * World location of screen-inwards normal assuming the phone is situated
245 * in natural (phone == portrait) mode.
246 * (headset: user nose).
247 *
248 * auto uy = q.rotation() * Eigen::Vector3f{0.f, 1.f, 0.f};
249 * = rotation.col(1);
250 */
251 [[maybe_unused]] const auto uy_0 = rotation.coeff(0, 1);
252 [[maybe_unused]] const auto uy_1 = rotation.coeff(1, 1);
253 [[maybe_unused]] const auto uy_2 = rotation.coeff(2, 1);
254 if constexpr (DEBUG) {
255 base::StringAppendF(&coordinates, "uy: %f %f %f", uy_0, uy_1, uy_2);
256 }
257
258 /*
259 * World location of unit vector top speaker.
260 * (headset: top of head).
261 * auto uz = q.rotation() * Eigen::Vector3f{0.f, 0.f, 1.f};
262 * = rotation.col(2);
263 */
264 [[maybe_unused]] const auto uz_0 = rotation.coeff(0, 2);
265 [[maybe_unused]] const auto uz_1 = rotation.coeff(1, 2);
266 [[maybe_unused]] const auto uz_2 = rotation.coeff(2, 2);
267 if constexpr (DEBUG) {
268 base::StringAppendF(&coordinates, "uz: %f %f %f", uz_0, uz_1, uz_2);
269 }
270
271 // pitch computed from nose world Z coordinate;
272 // hence independent of rotation around world Z.
273 if (pitch != nullptr) {
274 *pitch = asin(std::clamp(uy_2, -1.f, 1.f));
275 }
276
277 // roll computed from head/right world Z coordinate;
278 // hence independent of rotation around world Z.
279 if (roll != nullptr) {
280 // atan2 takes care of implicit scale normalization of Z, X.
281 *roll = -atan2(ux_2, uz_2);
282 }
283
284 // yaw computed from right ear angle projected onto world XY plane
285 // where world Z == 0. This is the rotation around world Z.
286 if (yaw != nullptr) {
287 // atan2 takes care of implicit scale normalization of X, Y.
288 *yaw = atan2(ux_1, ux_0);
289 }
290
291 if constexpr (DEBUG) {
292 return coordinates;
293 }
294 }
295
296 } // namespace media
297 } // namespace android
298