目录
内容抄自CSDN点云侠：【2024最新版】PCL点云处理算法汇总（C++长期更新版）。质量无忧，永久免费，可放心复制粘贴。
一、概述
将旋转矩阵转换为欧拉角是逆向理解三维对象姿态的一种方法。旋转矩阵是一个3x3的正交矩阵，它描述了在三维空间中的旋转，而欧拉角用一组角度来表示这种旋转。这种转换在计算机图形学、航空航天、机器人学等领域都非常重要。
1.1 原理
旋转矩阵到欧拉角的转换涉及决定每个旋转轴上的旋转顺序和旋转角度。常用的一种欧拉角顺序是“zyx”顺序，即首先绕z轴旋转（yaw），然后绕y轴旋转（pitch），最后绕x轴旋转（roll）。必须注意的是，不同的顺序会导致不同的结果。
假设旋转矩阵
R
(
α
)
R(\alpha)
R(α)为：
R
(
α
)
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R(\alpha) = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} &r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}
R(α)=
r11r21r31r12r22r32r13r23r33
绕Y轴pitch计算法公式为：
p
i
t
c
h
=
−
a
r
c
s
i
n
(
r
31
)
pitch=-arcsin(r_{31})
pitch=−arcsin(r31)
这里需要考虑
r
31
∈
[
−
1
,
1
]
r_{31}\in[-1,1]
r31∈[−1,1]
绕Z轴yaw计算法公式为：
如果
c
o
s
(
p
i
t
c
h
)
cos(pitch)
cos(pitch)不等于0则：
y
a
w
=
a
r
c
t
a
n
2
(
r
21
,
r
11
)
yaw=arctan2(r_{21},r_{11})
yaw=arctan2(r21,r11)
绕X轴Roll计算法公式为：
如果
c
o
s
(
p
i
t
c
h
)
cos(pitch)
cos(pitch)不等于0则：
r
o
l
l
=
a
r
c
t
a
n
2
(
r
32
,
r
33
)
roll=arctan2(r_{32},r_{33})
roll=arctan2(r32,r33)
如果
c
o
s
(
p
i
t
c
h
)
cos(pitch)
cos(pitch)等于0，即
∣
r
31
∣
=
1
|r_{31}|=1
∣r31∣=1发生万象节死锁，这种情况下，yaw和roll的值不能单独确定，一种有效的处理方式是：
如果
r
31
=
−
1
r_{31}=-1
r31=−1，即
p
i
t
c
h
=
−
π
/
2
pitch=-\pi/2
pitch=−π/2，则
y
a
w
=
0
,
r
o
l
l
=
a
r
c
t
a
n
2
(
−
r
12
,
−
r
13
)
yaw=0,roll=arctan2(-r_{12},-r_{13})
yaw=0,roll=arctan2(−r12,−r13)
如果
r
31
=
1
r_{31}=1
r31=1，即
p
i
t
c
h
=
π
/
2
pitch=\pi/2
pitch=π/2，则
y
a
w
=
0
,
r
o
l
l
=
a
r
c
t
a
n
2
(
r
12
,
r
13
)
yaw=0,roll=arctan2(r_{12},r_{13})
yaw=0,roll=arctan2(r12,r13)
1.2 实现步骤
旋转矩阵转欧拉角是一种将三维空间中的旋转表示从旋转矩阵形式转换为更常见的三个欧拉角（例如俯仰角、偏航角和滚动角）的过程。欧拉角是一组用于描述旋转顺序和旋转角度的方法，通常有几种不同的约定，如Z-Y-X、X-Z-Y和Y-X-Z等。
将旋转矩阵转换为欧拉角的具体步骤如下：
-
确定欧拉角的顺序（比如ZYX）。
-
将旋转矩阵分解为各个轴的旋转：先沿着z轴旋转θz，然后y轴旋转θy，最后x轴旋转θx。
-
使用公式计算每个轴的角度。对于ZYX顺序：
- θz = atan2(矩阵[1][2], matrix[1][1])
- θy = atan2(-matrix[0][2], sqrt(matrix[0][0]**2 + matrix[0][1]**2))
- θx = atan2(matrix[2][0], matrix[2][1])
-
这些角度可能存在舍入误差和 gimbal lock（两个轴完全锁定导致无法独立旋转）的问题，需要注意处理。
1.3 应用场景
旋转矩阵转欧拉角是一种将三维空间中的旋转数学表示从旋转矩阵形式转换为更直观的欧几里得坐标系下的三个角度（例如俯仰角、偏航角和滚动角）的过程。这种转换在许多领域都有应用：
- 机器人学：机器人运动控制中，需要将关节角度或姿态传感器的数据转换成适合导航或操作的空间方向。
- 游戏开发：游戏中角色或物体的定位、移动以及相机的跟随都需要处理旋转变换，以便于渲染和用户交互。
- 计算机视觉：在图像处理和机器识别中，对物体的姿态估计通常涉及将像素级别的旋转转换为可理解的角度描述。
- 航天工程：飞行器的姿态控制系统中，也需要用到欧拉角来描述其方向和位置。
- 图形学：三维图形渲染中，模型的旋转就需要通过欧拉角来调整。
1.4 注意事项
在将旋转矩阵转换为欧拉角（通常指yaw、pitch、roll三轴旋转）时，需要注意以下几点：
- 顺序问题：欧拉角有多种表示顺序，如Z-Y-X、X-Z-Y、Y-X-Z等。选择正确的顺序至关重要，因为不同的顺序对应不同的旋转路径。
- ** gimbal lock**：某些特定的角度组合下，比如Z轴旋转角度接近90度或-90度时，可能会导致“gimbal lock”，即两个轴的旋转不再独立，会影响结果的精确性。为了避免这个问题，需要采取特殊的处理策略，如四元数或其他避免锁死的方法。
- 精度问题：由于浮点数计算的精度限制，转换过程中可能出现近似误差。特别当旋转矩阵非常接近于某个旋转轴对齐的情况时，转换后的角度可能不那么准确。
- 连续性和周期性：欧拉角不是全局唯一的，它们在旋转空间中是循环的。例如，绕x轴旋转一周，然后紧接着绕y轴旋转一周，最终可能回到原始位置，但在欧拉角表示下看起来可能是两倍的角度。
- 保持一致性：如果矩阵是由一系列旋转步骤累积得到的，转换成欧拉角时应考虑是否按照同样的顺序进行，以保证转换的一致性。
二、关键函数
2.1 头文件
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
2.2 主要函数
Eigen::Vector3f rotationMatrixToEulerAngles(const Eigen::Matrix3f& R)
{
float sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
bool singular = sy < 1e-6; // If sy is close to zero, the rotation is singular (Gimbal Lock)
float roll = 0.0, pitch = 0.0, yaw = 0.0;
if (!singular)
{
roll = atan2(R(2, 1), R(2, 2));
pitch = atan2(-R(2, 0), sy);
yaw = atan2(R(1, 0), R(0, 0));
}
else
{
roll = atan2(-R(1, 2), R(1, 1));
pitch = atan2(-R(2, 0), sy);
yaw = 0;
}
return Eigen::Vector3f(roll, pitch, yaw);
}
三、完整代码
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
Eigen::Vector3f rotationMatrixToEulerAngles(const Eigen::Matrix3f& R)
{
float sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
bool singular = sy < 1e-6; // If sy is close to zero, the rotation is singular (Gimbal Lock)
float roll = 0.0, pitch = 0.0, yaw = 0.0;
if (!singular)
{
roll = atan2(R(2, 1), R(2, 2));
pitch = atan2(-R(2, 0), sy);
yaw = atan2(R(1, 0), R(0, 0));
}
else
{
roll = atan2(-R(1, 2), R(1, 1));
pitch = atan2(-R(2, 0), sy);
yaw = 0;
}
return Eigen::Vector3f(roll, pitch, yaw);
}
int main()
{
Eigen::Matrix3f R;
R << 0.36, -0.48, 0.8,
0.8, 0.6, 0,
-0.48, 0.64, 0.6;
Eigen::Vector3f eulerAngles = rotationMatrixToEulerAngles(R);
std::cout << "Euler angles (roll, pitch, yaw):\n" << eulerAngles * 180 / 3.1415926 << std::endl;
return 0;
}
三、 结果展示
