Apollo 10.0
自动驾驶开放平台
euler_angles_zxy.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 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
22#pragma once
23
24// TODO(all): should use Angle class internally.
25
26#include <cmath>
27
28#include "Eigen/Geometry"
29
31
36namespace apollo {
37namespace common {
38namespace math {
39
65template <typename T>
67 public:
71 EulerAnglesZXY() : roll_(0), pitch_(0), yaw_(0) {}
72
78 explicit EulerAnglesZXY(T yaw) : roll_(0), pitch_(0), yaw_(yaw) {}
79
88 : roll_(roll), pitch_(pitch), yaw_(yaw) {}
89
98 EulerAnglesZXY(T qw, T qx, T qy, T qz)
99 : roll_(std::atan2(static_cast<T>(2.0) * (qw * qy - qx * qz),
100 static_cast<T>(2.0) * (Square<T>(qw) + Square<T>(qz)) -
101 static_cast<T>(1.0))),
102 pitch_(std::asin(static_cast<T>(2.0) * (qw * qx + qy * qz))),
103 yaw_(std::atan2(static_cast<T>(2.0) * (qw * qz - qx * qy),
104 static_cast<T>(2.0) * (Square<T>(qw) + Square<T>(qy)) -
105 static_cast<T>(1.0))) {}
106
111 explicit EulerAnglesZXY(const Eigen::Quaternion<T> &q)
112 : EulerAnglesZXY(q.w(), q.x(), q.y(), q.z()) {}
113
118 T roll() const { return roll_; }
119
124 T pitch() const { return pitch_; }
125
130 T yaw() const { return yaw_; }
131
135 void Normalize() {
136 roll_ = NormalizeAngle(roll_);
137 pitch_ = NormalizeAngle(pitch_);
138 yaw_ = NormalizeAngle(yaw_);
139 }
140
145 bool IsValid() {
146 Normalize();
147 return pitch_ < M_PI_2 && pitch_ > -M_PI_2;
148 }
149
154 Eigen::Quaternion<T> ToQuaternion() const {
155 T coeff = static_cast<T>(0.5);
156 T r = roll_ * coeff;
157 T p = pitch_ * coeff;
158 T y = yaw_ * coeff;
159
160 T sr = std::sin(r);
161 T sp = std::sin(p);
162 T sy = std::sin(y);
163
164 T cr = std::cos(r);
165 T cp = std::cos(p);
166 T cy = std::cos(y);
167
168 T qw = cr * cp * cy - sr * sp * sy;
169 T qx = cr * sp * cy - sr * cp * sy;
170 T qy = cr * sp * sy + sr * cp * cy;
171 T qz = cr * cp * sy + sr * sp * cy;
172 if (qw < 0.0) {
173 return {-qw, -qx, -qy, -qz};
174 }
175 return {qw, qx, qy, qz};
176 }
177
178 private:
179 T roll_;
180 T pitch_;
181 T yaw_;
182};
183
186
187} // namespace math
188} // namespace common
189} // namespace apollo
Any orientation of a rigid body on a 3-D space can be achieved by composing three rotations about the...
EulerAnglesZXY(T yaw)
Constructs a rotation using only yaw (i.e., around the z-axis).
EulerAnglesZXY(T qw, T qx, T qy, T qz)
Constructs a rotation using components of a quaternion.
bool IsValid()
Verifies the validity of the specified rotation.
Eigen::Quaternion< T > ToQuaternion() const
Converts to a quaternion with a non-negative scalar part
EulerAnglesZXY(T roll, T pitch, T yaw)
Constructs a rotation using arbitrary roll, pitch, and yaw.
EulerAnglesZXY(const Eigen::Quaternion< T > &q)
Constructs a rotation from quaternion.
void Normalize()
Normalizes roll_, pitch_, and yaw_ to [-PI, PI).
EulerAnglesZXY()
Constructs an identity rotation.
Math-related util functions.
EulerAnglesZXY< double > EulerAnglesZXYd
EulerAnglesZXY< float > EulerAnglesZXYf
T Square(const T value)
Compute squared value.
Definition math_utils.h:141
double NormalizeAngle(const double angle)
Normalize angle to [-PI, PI).
Definition math_utils.cc:53
class register implement
Definition arena_queue.h:37
Definition future.h:29