Apollo 10.0
自动驾驶开放平台
quaternion_math.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2023 The Apollo Authors. All Rights Reserved.
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 <cmath>
18
19namespace apollo {
20namespace cyber {
21
22inline void QuaternionToEuler(const double quaternion[4], double att[3]) {
23 double dcm21 =
24 2 * (quaternion[2] * quaternion[3] + quaternion[0] * quaternion[1]);
25 double dcm20 =
26 2 * (quaternion[1] * quaternion[3] - quaternion[0] * quaternion[2]);
27 double dcm22 = quaternion[0] * quaternion[0] - quaternion[1] * quaternion[1] -
28 quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3];
29 double dcm01 =
30 2 * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
31 double dcm11 = quaternion[0] * quaternion[0] - quaternion[1] * quaternion[1] +
32 quaternion[2] * quaternion[2] - quaternion[3] * quaternion[3];
33
34 att[0] = std::asin(dcm21); // the angle rotate respect to X
35 att[1] = std::atan2(-dcm20, dcm22); // the angle rotate respect to Y
36 att[2] = std::atan2(dcm01, dcm11); // the angle rotate respect to Z
37
38 return;
39}
40
41inline double QuaternionToHeading(double yaw) {
42 double a = std::fmod(yaw + M_PI_2 + M_PI, 2.0 * M_PI);
43 if (a < 0.0) {
44 a += (2.0 * M_PI);
45 }
46 return a - M_PI;
47}
48
49} // namespace cyber
50} // namespace apollo
double QuaternionToHeading(double yaw)
void QuaternionToEuler(const double quaternion[4], double att[3])
class register implement
Definition arena_queue.h:37