欢迎访问我的博客
首页
。
三维刚体变换
SLAM 的基本问题是推断传感器坐标系的相对位姿。根据传感器坐标系的相对位姿，可以把其它传感器坐标系中的坐标点变换到某一个传感器坐标系中，这一个传感器坐标系通常称为世界坐标系。这样就可以建图或定位。
1. 坐标系
1.1 空间坐标系
空间坐标系分为左手坐标系与右手坐标系。使大拇指、食指和中指两两垂直，它们三个的指向分别定为 x, y, z 轴正方向，则左手确定的坐标系为左手坐标系，右手确定的坐标系为右手坐标系。
图 1.1 左手坐标系与右手坐标系
1.2 右手坐标系与像素坐标系
在 SLAM 中，为了与像素坐标系保持一致，通常使用右手坐标系。因为这时，x, y 轴平行于 u, v 轴且 z 轴指向相机前方。
图 1.2 像素坐标系与右手坐标系
2. 旋转与平移
图 2.1 表示的是，黑色坐标系
O
a
O_a
O
a
绕 z 轴逆时针旋转 90° 得到绿色坐标系
O
b
O_b
O
b
，绿色坐标系按向量
t
=
(
0
,
1
,
0
)
T
{\bf t} = (0, 1, 0)^T
t
=
(
0
,
1
,
0
)
T
平移得到红色坐标系
O
c
O_c
O
c
。可以直接看出，点
P
P
P
在黑色坐标系中的坐标是
P
a
=
(
−
4
,
2
,
1
)
P_a = (-4, 2, 1)
P
a
=
(
−
4
,
2
,
1
)
，在绿色坐标系中的坐标是
P
b
=
(
2
,
4
,
1
)
P_b = (2, 4, 1)
P
b
=
(
2
,
4
,
1
)
，在红色坐标系中的坐标是
P
c
=
(
2
,
3
,
1
)
P_c = (2, 3, 1)
P
c
=
(
2
,
3
,
1
)
。
图 2.1 右手坐标系的旋转与平移
需要注意的是，旋转和平移的主体是坐标系，而不是空间点，空间点是不变的。下面我们分析这三个坐标系的关系。
2.1 推导旋转
如果以
O
a
O_a
O
a
为世界坐标系，则
O
a
O_a
O
a
的单位正交基为
e
x
a
=
(
1
,
0
,
0
)
T
{\bf e}_x^a=(1,0,0)^T
e
x
a
=
(
1
,
0
,
0
)
T
,
e
y
a
=
(
0
,
1
,
0
)
T
{\bf e}_y^a=(0,1,0)^T
e
y
a
=
(
0
,
1
,
0
)
T
,
e
z
a
=
(
0
,
0
,
1
)
T
{\bf e}_z^a=(0,0,1)^T
e
z
a
=
(
0
,
0
,
1
)
T
，
O
b
O_b
O
b
的单位正交基为
e
x
b
=
(
0
,
1
,
0
)
T
{\bf e}_x^b=(0,1,0)^T
e
x
b
=
(
0
,
1
,
0
)
T
,
e
y
b
=
(
−
1
,
0
,
0
)
T
{\bf e}_y^b=(-1,0,0)^T
e
y
b
=
(
−
1
,
0
,
0
)
T
,
e
z
b
=
(
0
,
0
,
1
)
T
{\bf e}_z^b=(0,0,1)^T
e
z
b
=
(
0
,
0
,
1
)
T
。由空间向量基本定理知
[
e
x
a
e
y
a
e
z
a
]
⋅
P
a
=
[
e
x
b
e
y
b
e
z
b
]
⋅
P
b
.
\begin{bmatrix} {\bf e}_x^a & {\bf e}_y^a & {\bf e}_z^a \end{bmatrix} \cdot P_a = \begin{bmatrix} {\bf e}_x^b & {\bf e}_y^b & {\bf e}_z^b \end{bmatrix} \cdot P_b \;.
[
e
x
a
e
y
a
e
z
a
]
⋅
P
a
=
[
e
x
b
e
y
b
e
z
b
]
⋅
P
b
.
于是
P
b
=
[
e
x
b
e
y
b
e
z
b
]
−
1
⋅
[
e
x
a
e
y
a
e
z
a
]
⋅
P
a
=
R
b
a
⋅
P
a
.
P_b = \begin{bmatrix} {\bf e}_x^b & {\bf e}_y^b & {\bf e}_z^b \end{bmatrix}^{-1} \cdot \begin{bmatrix} {\bf e}_x^a & {\bf e}_y^a & {\bf e}_z^a \end{bmatrix} \cdot P_a = {\bf R}_{ba} \cdot P_a \;.
P
b
=
[
e
x
b
e
y
b
e
z
b
]
−
1
⋅
[
e
x
a
e
y
a
e
z
a
]
⋅
P
a
=
R
ba
⋅
P
a
.
其中
R
b
a
{\bf R}_{ba}
R
ba
表示从坐标系
O
a
O_a
O
a
到坐标系
O
b
O_b
O
b
的变换，它是一个三阶正交矩阵，
R
b
a
=
[
e
x
b
e
y
b
e
z
b
]
−
1
⋅
[
e
x
a
e
y
a
e
z
a
]
.
{\bf R}_{ba} = \begin{bmatrix} {\bf e}_x^b & {\bf e}_y^b & {\bf e}_z^b \end{bmatrix}^{-1} \cdot \begin{bmatrix} {\bf e}_x^a & {\bf e}_y^a & {\bf e}_z^a \end{bmatrix}.
R
ba
=
[
e
x
b
e
y
b
e
z
b
]
−
1
⋅
[
e
x
a
e
y
a
e
z
a
]
.
因为一组单位正交基组成的矩阵也是正交矩阵，所以
R
b
a
=
[
e
x
b
e
y
b
e
z
b
]
T
⋅
[
e
x
a
e
y
a
e
z
a
]
=
[
0
1
0
−
1
0
0
0
0
1
]
.
{\bf R}_{ba} = \begin{bmatrix} {\bf e}_x^b & {\bf e}_y^b & {\bf e}_z^b \end{bmatrix}^T \cdot \begin{bmatrix} {\bf e}_x^a & {\bf e}_y^a & {\bf e}_z^a \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix}.
R
ba
=
[
e
x
b
e
y
b
e
z
b
]
T
⋅
[
e
x
a
e
y
a
e
z
a
]
=
0
−
1
0
1
0
0
0
0
1
.
旋转矩阵
R
{\bf R}
R
可以表示旋转，它是一个行列式为 1 的正交矩阵。反之，任意行列式为 1 的正交矩阵都表示一个旋转。正交矩阵的逆等于其转置，所以旋转矩阵的转置表示一个相反的变换。
2.2 推导平移
平移的推导很简单。假如坐标系
O
b
O_b
O
b
按向量
t
{\bf t}
t
平移得到坐标系
O
c
O_c
O
c
，则点
P
P
P
在这两个坐标系中的坐标
P
b
P_b
P
b
和
P
c
P_c
P
c
的关系是
P
c
=
P
b
−
t
.
P_c = P_b – {\bf t}.
P
c
=
P
b
−
t
.
由于不涉及旋转，这里
t
{\bf t}
t
的坐标表示，无论使用它在坐标系
O
b
O_b
O
b
中的坐标，还是使用它在坐标系
O
c
O_c
O
c
中的坐标，都是一样的。
2.3 推导变换
综上所述，坐标系
O
a
O_a
O
a
到坐标系
O
c
O_c
O
c
的变换可以表示为：
P
c
=
R
b
a
⋅
P
a
−
t
.
P_c = {\bf R}_{ba} \cdot P_a – {\bf t}.
P
c
=
R
ba
⋅
P
a
−
t
.
利用该公式，可以在已知一个点
P
P
P
在坐标系
O
a
O_a
O
a
中的坐标
P
a
P_a
P
a
时，求出它在坐标系
O
c
O_c
O
c
中的坐标
P
c
P_c
P
c
。
为了把上式写成两个矩阵相乘的形式，我们使用齐次坐标且把减号改成加号。使用齐次坐标且令
t
c
b
=
−
t
{\bf t}_{cb} = – {\bf t}
t
c
b
=
−
t
，则坐标系
O
a
O_a
O
a
到坐标系
O
c
O_c
O
c
的变换可以表示为：
[
P
c
1
]
=
[
R
b
a
t
c
b
0
T
1
]
⋅
[
P
a
1
]
.
\begin{bmatrix} P_c \\ 1 \end{bmatrix} = \begin{bmatrix} {\bf R}_{ba} & {\bf t}_{cb} \\ {\bf 0}^T & 1 \end{bmatrix} \cdot \begin{bmatrix} P_a \\ 1 \end{bmatrix}.
[
P
c
1
]
=
[
R
ba
0
T
t
c
b
1
]
⋅
[
P
a
1
]
.
其中
T
c
a
=
[
R
b
a
t
c
b
0
T
1
]
{\bf T}_{ca} = \begin{bmatrix} {\bf R}_{ba} & {\bf t}_{cb} \\ {\bf 0}^T & 1 \end{bmatrix}
T
c
a
=
[
R
ba
0
T
t
c
b
1
]
是一个四阶矩阵，称为变换矩阵。
上面的变换比较简单，所以我们能根据坐标轴的几何关系推导出旋转和平移。这个推导过程仅用于理解旋转和平移的几何意义，实际应用中求解旋转和平移，会使用分解单应矩阵/本质矩阵、PnP、ICP、Sim3 等算法。
2.4 刚体变换
上面我们把旋转和平移看成两次变换，从坐标系
O
a
O_a
O
a
经坐标系
O
b
O_b
O
b
到坐标系
O
c
O_c
O
c
的两次变换可以表示为
P
c
=
R
b
a
⋅
P
a
+
t
c
b
.
P_c = {\bf R}_{ba} \cdot P_a + {\bf t}_{cb}.
P
c
=
R
ba
⋅
P
a
+
t
c
b
.
实际应用中，我们通常把它们看成从坐标系
O
a
O_a
O
a
到坐标系
O
c
O_c
O
c
的一次变换
{
P
c
=
R
c
a
⋅
P
a
+
t
c
a
,
T
c
a
=
[
R
c
a
t
c
a
0
T
1
]
.
\left\{\begin{aligned} P_c &= {\bf R}_{ca} \cdot P_a + {\bf t}_{ca}, \\\\ {\bf T}_{ca} &= \begin{bmatrix} {\bf R}_{ca} & {\bf t}_{ca} \\ {\bf 0}^T & 1 \end{bmatrix}. \end{aligned}\right.
⎩
⎨
⎧
P
c
T
c
a
=
R
c
a
⋅
P
a
+
t
c
a
,
=
[
R
c
a
0
T
t
c
a
1
]
.
其中
R
c
a
=
R
b
a
{\bf R}_{ca} = {\bf R}_{ba}
R
c
a
=
R
ba
，
t
c
a
=
t
c
b
{\bf t}_{ca} = {\bf t}_{cb}
t
c
a
=
t
c
b
。需要注意的是，此时
t
c
a
{\bf t}_{ca}
t
c
a
是坐标系
O
c
O_c
O
c
中的向量，而不是坐标系
O
a
O_a
O
a
中的向量。
2.5 坐标系旋转与向量旋转
所有旋转都可以分解为绕坐标轴的旋转。当坐标系发生了旋转后，我们可能需要知道一个静止的点在旋转前后的坐标系中的坐标关系；当向量发生了旋转后，我们可能需要知道该向量旋转前后在同一个坐标系中的坐标关系。这些坐标关系就是旋转，本节借助 Eigen 库求这样的旋转。
图 2.2 坐标系旋转(左)、向量旋转(中)和坐标变换中的平移(右)
图 2.2 的左图表示的是，从 Z 轴负方向看向正方向时，绿色坐标系
O
1
O_1
O
1
绕 Z 轴
逆
时针旋转 90° 变成红色坐标系
O
2
O_2
O
2
的变换。这个变换可以用
R
21
R_{21}
R
21
表示。借助 Eigen 库表示这样的旋转
R
21
R_{21}
R
21
：
Eigen::AngleAxisd R21(M_PI / 2, Eigen::Vector3d::UnitZ());
已知点
P
P
P
在坐标系
O
1
O_1
O
1
中的坐标
P
1
P_1
P
1
，可以求得它在坐标系
O
2
O_2
O
2
中的坐标
P
2
P_2
P
2
：
P
2
=
R
21
⋅
P
1
P_2 = R_{21} \cdot P_1
P
2
=
R
21
⋅
P
1
。
图 2.2 的中图，同样是
P
1
P_1
P
1
经
R
21
R_{21}
R
21
到
P
2
P_2
P
2
的旋转。不同的是，此时
R
21
R_{21}
R
21
表示的是，从 Z 轴负方向看向正方向时，向量
O
P
1
→
\overrightarrow{OP_1}
O
P
1
绕 Z 轴
顺
时针旋转 90° 变成向量
O
P
2
→
\overrightarrow{OP_2}
O
P
2
。
综上所述，代码中的
R
21
R_{21}
R
21
既可以表示坐标系逆时针旋转，也可以表示向量顺时针旋转。表示坐标系逆时针旋转时，
R
21
R_{21}
R
21
用于求静止点在旋转所得新坐标系中的坐标；表示向量顺时针旋转时，
R
21
R_{21}
R
21
用于求向量顺时针旋转之后的新坐标。
3. 链式变换
我们把从坐标系
O
a
O_a
O
a
到坐标系
O
b
O_b
O
b
的变换表示成
T
b
a
{\bf T}_{ba}
T
ba
而不是
T
a
b
{\bf T}_{ab}
T
ab
，这是 SLAM 中的常用做法。由于变换矩阵用于左乘，所以这样的表示在坐标系链式变换中的优势很明显：
T
d
a
=
T
d
c
⋅
T
c
b
⋅
T
b
a
.
{\bf T}_{da} = {\bf T}_{dc} \cdot {\bf T}_{cb} \cdot {\bf T}_{ba}.
T
d
a
=
T
d
c
⋅
T
c
b
⋅
T
ba
.
从上式的右侧向左侧看下标，很容易知道这是从坐标系
O
a
O_a
O
a
变换到坐标系
O
b
O_b
O
b
、从坐标系
O
b
O_b
O
b
变换到坐标系
O
c
O_c
O
c
、从坐标系
O
c
O_c
O
c
变换到坐标系
O
d
O_d
O
d
的链式变换。
4. Eigen 库
Eigen 是一个开源的线性代数库，我们可以借助它实现矩阵运算。
4.1 变换与逆变换
P
a
P_a
P
a
和
P
b
P_b
P
b
分别表示点
P
P
P
在坐标系
O
a
O_a
O
a
和
O
b
O_b
O
b
中的坐标。
t
a
t_a
t
a
和
t
b
t_b
t
b
分别表示向量
O
a
O
b
→
\overrightarrow{O_a O_b}
O
a
O
b
在坐标系
O
a
O_a
O
a
中的坐标和向量
O
b
O
a
→
\overrightarrow{O_b O_a}
O
b
O
a
在坐标系
O
b
O_b
O
b
中的坐标；或者说，
t
a
t_a
t
a
和
t
b
t_b
t
b
分别表示原点
O
b
O_b
O
b
在坐标系
O
a
O_a
O
a
中的坐标和原点
O
a
O_a
O
a
在坐标系
O
b
O_b
O
b
中的坐标。
R
b
a
R_{ba}
R
ba
是坐标系
O
a
O_a
O
a
到坐标系
O
b
O_b
O
b
的旋转。因为
P
b
=
R
b
a
⋅
P
a
+
t
b
,
P_b = {\bf R}_{ba} \cdot P_a + {\bf t}_b,
P
b
=
R
ba
⋅
P
a
+
t
b
,
所以
P
a
=
R
b
a
T
⋅
(
P
b
−
t
b
)
.
P_a = {\bf R}_{ba}^T \cdot (P_b – {\bf t}_b).
P
a
=
R
ba
T
⋅
(
P
b
−
t
b
)
.
由此我们可以得到逆变换中的旋转和平移
{
R
a
b
=
R
b
a
T
,
t
a
=
−
R
b
a
T
⋅
t
b
.
\left\{\begin{aligned} {\bf R}_{ab} &= {\bf R}_{ba}^T, \\\\ {\bf t}_{a} &= – {\bf R}_{ba}^T \cdot {\bf t}_b. \end{aligned}\right.
⎩
⎨
⎧
R
ab
t
a
=
R
ba
T
,
=
−
R
ba
T
⋅
t
b
.
下面使用 Eigen 库实现坐标的变换与逆变换。
#include <iostream>
#include <iomanip>
#include <Eigen/Eigen>
using namespace std;
int main() {
// 1.小数点对齐，保留两位小数。
cout << setiosflags(ios::fixed) << setiosflags(ios::right) << setprecision(2);
// 2.变换前后的坐标 point_a 和 point_b。
Eigen::Vector3d point_a(-4, 2, 1), point_b;
// 3.1 坐标系 O1 绕自己的 z 轴逆时针旋转 90 度得到坐标系 O2，坐标系 O2 在自己坐标系中平移 -(0, -1, 0)。
// Eigen::AngleAxisd rotation(-M_PI / 2, Eigen::Vector3d(0, 0, 1));
Eigen::AngleAxisd rotation(-M_PI / 2, Eigen::Vector3d::UnitZ());
Eigen::Vector3d translation = Eigen::Vector3d(0, -1, 0);
Eigen::Isometry3d transformation = Eigen::Isometry3d::Identity(); // 这是一个四阶矩阵。
transformation.rotate(rotation);
transformation.pretranslate(translation);
cout << "-- transform matrix:" << endl;
cout << transformation.matrix() << endl;
// 3.2 使用变换矩阵把 point_a 变换到 point_b。
point_b = transformation * point_a;
cout << "-- transform (-4, 2, 1) to:" << endl;
cout << point_b.matrix().transpose() << endl;
// 3.3 使用变换矩阵把 point_b 变换回 point_a。
point_a = transformation.inverse() * point_b;
cout << "-- transform back to (-4, 2, 1):" << endl;
cout << point_a.matrix().transpose() << endl;
// 3.4 使用四元数和平移向量把 point_a 变换到 point_b。
Eigen::Quaterniond q = Eigen::Quaterniond(rotation);
point_b = q * point_a + translation;
cout << "-- use quaternion to transform (-4, 2, 1) to:" << endl;
cout << point_b.matrix().transpose() << endl;
// 3.5 使用四元数和平移向量把 point_b 变换回 point_a。
point_a = q.inverse() * (point_b - translation);
cout << "-- use quaternion to transform back to (-4, 2, 1):" << endl;
cout << point_a.matrix().transpose() << endl;
}
4.2 何时平移
上面说的变换都是默认先旋转再平移，当然变换也可以是先平移再旋转。Eigen 使用 pretranslate 函数和 translate 函数来区别这两种变换：
{
R
⋅
P
+
t
u
s
e
p
r
e
t
r
a
n
s
l
a
t
e
,
R
⋅
(
P
+
t
)
u
s
e
t
r
a
n
s
l
a
t
e
.
\left\{\begin{aligned} {\bf R} \cdot P + {\bf t} \;\;\; & \rm{use \; pretranslate}, \\ {\bf R} \cdot (P + {\bf t}) \;\;\; & \rm{use \; translate}. \end{aligned}\right.
{
R
⋅
P
+
t
R
⋅
(
P
+
t
)
use
pretranslate
,
use
translate
.
4.3 位姿插值
位姿插值用于环形扫描的激光雷达等场景中。如果坐标系在
t
0
t_0
t
0
到
t
1
t_1
t
1
时刻做匀速直线运动或匀速圆周运动，且这两个时刻的位姿已知，则可以通过位姿插值求出它在
t
0
t_0
t
0
到
t
1
t_1
t
1
中间任意时刻的位姿。
#include <iostream>
#include <cmath>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
using namespace std;
int main() {
// 1.小数点对齐，保留两位小数。
cout << setiosflags(ios::fixed) << setiosflags(ios::right) << setprecision(2);
// 2.绕 z 轴旋转 0 度的旋转矩阵和四元数。
Eigen::AngleAxisd rotation_vector_z00(0, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond q_z00 = Eigen::Quaterniond(rotation_vector_z00);
cout << q_z00.coeffs().transpose() << endl; // 0.00 0.00 0.00 1.00
// 3.绕 z 轴旋转 30 度的旋转矩阵和四元数。
Eigen::AngleAxisd rotation_vector_z30(M_PI / 6, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond q_z30 = Eigen::Quaterniond(rotation_vector_z30);
cout << q_z30.coeffs().transpose() << endl; // 0.00 0.00 0.26 0.97
// 4.绕 z 轴旋转 45 度的旋转矩阵和四元数。
Eigen::AngleAxisd rotation_vector_z45(M_PI / 4, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond q_z45 = Eigen::Quaterniond(rotation_vector_z45);
cout << q_z45.coeffs().transpose() << endl; // 0.00 0.00 0.38 0.92
// 5.绕 z 轴旋转 60 度的旋转矩阵和四元数。
Eigen::AngleAxisd rotation_vector_z60(M_PI / 3, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond q_z60 = Eigen::Quaterniond(rotation_vector_z60);
cout << q_z60.coeffs().transpose() << endl; // 0.00 0.00 0.50 0.87
// 6.绕 z 轴旋转 90 度的旋转矩阵和四元数。
Eigen::AngleAxisd rotation_vector_z90(M_PI / 2, Eigen::Vector3d(0, 0, 1));
Eigen::Quaterniond q_z90 = Eigen::Quaterniond(rotation_vector_z90);
cout << q_z90.coeffs().transpose() << endl; // 0.00 0.00 0.71 0.71
cout << endl;
// 7.球面插值。
Eigen::Quaterniond p00 = Eigen::Quaterniond::Identity().slerp(0 / 6.0, q_z90);
Eigen::Quaterniond p30 = Eigen::Quaterniond::Identity().slerp(2 / 6.0, q_z90);
Eigen::Quaterniond p45 = Eigen::Quaterniond::Identity().slerp(3 / 6.0, q_z90);
Eigen::Quaterniond p60 = Eigen::Quaterniond::Identity().slerp(4 / 6.0, q_z90);
Eigen::Quaterniond p90 = Eigen::Quaterniond::Identity().slerp(6 / 6.0, q_z90);
cout << p00.coeffs().transpose() << endl; // 0.00 0.00 0.00 1.00
cout << p30.coeffs().transpose() << endl; // 0.00 0.00 0.26 0.97
cout << p45.coeffs().transpose() << endl; // 0.00 0.00 0.38 0.92
cout << p60.coeffs().transpose() << endl; // 0.00 0.00 0.50 0.87
cout << p90.coeffs().transpose() << endl; // 0.00 0.00 0.71 0.71
}
第 14 行的四元数 q_z00 代表绕 z 轴旋转 0°，第 18 行的四元数 q_z30 代表绕 z 轴旋转 30°，第 22 行的四元数 q_z45 代表绕 z 轴旋转 45°，第 26 行的四元数 q_z60 代表绕 z 轴旋转 60°，第 30 行的四元数 q_z90 代表绕 z 轴旋转 90°。第 34 至 38 行表示在 q_z00 和 q_z90 之间插值。
5. 欧拉角
5.1 欧拉角与机体坐标系
机体坐标系：机体坐标系是右手坐标系，x 轴指向机头，y 轴指向右机翼，z 轴指向下。欧拉角用于描述刚体绕机体坐标系坐标轴的旋转：滚转角 roll 表示绕 x 轴旋转的角度，俯仰角 pitch 表示绕 y 轴旋转的角度，偏航角 yaw 表示绕 z 轴旋转的角度。
当从旋转轴负方向看向正方向时，顺时针旋转的角度为正，逆时针旋转的角度为负。即，右倾时滚转角 roll 为正，爬升时俯仰角 pitch 为正，右偏时偏航角 yaw 为正。
图 5.1 欧拉角
5.2 欧拉角与三维旋转矩阵
SLAM 中需要把三维位姿变换到二维位姿，因此需要利用三维旋转矩阵求欧拉角。假设三维旋转矩阵是
R
=
[
r
00
r
01
r
02
r
10
r
11
r
12
r
20
r
21
r
22
]
,
R = \begin{bmatrix} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \end{bmatrix},
R
=
r
00
r
10
r
20
r
01
r
11
r
21
r
02
r
12
r
22
,
那么，欧拉角的弧度值是
{
x
r
o
l
l
=
a
r
c
t
a
n
r
21
r
22
y
p
a
t
c
h
=
−
a
r
c
t
a
n
r
20
r
21
2
+
r
22
2
=
−
a
r
c
s
i
n
r
20
z
y
a
w
=
a
r
c
t
a
n
r
10
r
00
.
\left\{\begin{aligned} x_{roll} &= {\rm arctan}\frac{r_{21}}{r_{22}} \\\\ y_{patch} &= -{\rm arctan}\frac{r_{20}}{\sqrt{r_{21}^2+r_{22}^2}} = -{\rm arcsin}r_{20} \\\\ z_{yaw} &= {\rm arctan}\frac{r_{10}}{r_{00}} \end{aligned}\right..
⎩
⎨
⎧
x
ro
ll
y
p
a
t
c
h
z
y
a
w
=
arctan
r
22
r
21
=
−
arctan
r
21
2
+
r
22
2
r
20
=
−
arcsin
r
20
=
arctan
r
00
r
10
.
使用 Eigen 验证：
void f6() {
Eigen::AngleAxisd rotation1(0 / 20, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd rotation2(M_PI / 30, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rotation3(0 / 60, Eigen::Vector3d::UnitZ());
Eigen::Quaterniond R(rotation1 * rotation2 * rotation3);
Eigen::Matrix3d M = R.toRotationMatrix();
double x = atan2(M(2, 1), M(2, 2)) * 180 / M_PI;
double y1 = -asin(M(2, 0)) * 180 / M_PI;
double y2 = atan2(-M(2, 0), sqrt(pow(M(2, 1), 2) + pow(M(2, 2), 2))) * 180 / 3.1415926;
double z = atan2(M(1, 0), M(0, 0)) * 180 / M_PI;
cout << x << " " << y1 << " " << y2 << " " << z << endl;
}
6. 参考
-
左手坐标系与右手坐标系
，CSDN，2018。 -
四元数、变换矩阵、欧拉角、轴角的转换关系详解
，CSDN，2021。 -
滚动，俯仰，偏航计算
，领悟书生。