欢迎访问我的博客首页。
SLAM 的基本问题是推断传感器坐标系的相对位姿。根据传感器坐标系的相对位姿，可以把其它传感器坐标系中的坐标点变换到某一个传感器坐标系中，这一个传感器坐标系通常称为世界坐标系。这样就可以建图或定位。
空间坐标系分为左手坐标系与右手坐标系。使大拇指、食指和中指两两垂直，它们三个的指向分别定为 x, y, z 轴正方向，则左手确定的坐标系为左手坐标系，右手确定的坐标系为右手坐标系。
在 SLAM 中，为了与像素坐标系保持一致，通常使用右手坐标系。因为这时，x, y 轴平行于 u, v 轴且 z 轴指向相机前方。
图 2.1 表示的是，黑色坐标系
O
a
O_a
Oa 绕 z 轴逆时针旋转 90° 得到绿色坐标系
O
b
O_b
Ob，绿色坐标系按向量
t
=
(
0
,
1
,
0
)
T
{\bf t} = (0, 1, 0)^T
t=(0,1,0)T 平移得到红色坐标系
O
c
O_c
Oc。可以直接看出，点
P
P
P 在黑色坐标系中的坐标是
P
a
=
(
−
4
,
2
,
1
)
P_a = (-4, 2, 1)
Pa=(−4,2,1)，在绿色坐标系中的坐标是
P
b
=
(
2
,
4
,
1
)
P_b = (2, 4, 1)
Pb=(2,4,1)，在红色坐标系中的坐标是
P
c
=
(
2
,
3
,
1
)
P_c = (2, 3, 1)
Pc=(2,3,1)。
需要注意的是，旋转和平移的主体是坐标系，而不是空间点，空间点是不变的。下面我们分析这三个坐标系的关系。
如果以 O a O_a Oa 为世界坐标系，则 O a O_a Oa 的单位正交基为 e x a = ( 1 , 0 , 0 ) T {\bf e}_x^a=(1,0,0)^T exa=(1,0,0)T, e y a = ( 0 , 1 , 0 ) T {\bf e}_y^a=(0,1,0)^T eya=(0,1,0)T, e z a = ( 0 , 0 , 1 ) T {\bf e}_z^a=(0,0,1)^T eza=(0,0,1)T， O b O_b Ob 的单位正交基为 e x b = ( 0 , 1 , 0 ) T {\bf e}_x^b=(0,1,0)^T exb=(0,1,0)T, e y b = ( − 1 , 0 , 0 ) T {\bf e}_y^b=(-1,0,0)^T eyb=(−1,0,0)T, e z b = ( 0 , 0 , 1 ) T {\bf e}_z^b=(0,0,1)^T ezb=(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 \;. [exaeyaeza]⋅Pa=[exbeybezb]⋅Pb.
于是
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 \;. Pb=[exbeybezb]−1⋅[exaeyaeza]⋅Pa=Rba⋅Pa.
其中 R b a {\bf R}_{ba} Rba 表示从坐标系 O a O_a Oa 到坐标系 O b O_b Ob 的变换，它是一个三阶正交矩阵，
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}. Rba=[exbeybezb]−1⋅[exaeyaeza].
因为一组单位正交基组成的矩阵也是正交矩阵，所以
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}. Rba=[exbeybezb]T⋅[exaeyaeza]= 0−10100001 .
旋转矩阵 R {\bf R} R 可以表示旋转，它是一个行列式为 1 的正交矩阵。反之，任意行列式为 1 的正交矩阵都表示一个旋转。正交矩阵的逆等于其转置，所以旋转矩阵的转置表示一个相反的变换。
平移的推导很简单。假如坐标系 O b O_b Ob 按向量 t {\bf t} t 平移得到坐标系 O c O_c Oc，则点 P P P 在这两个坐标系中的坐标 P b P_b Pb 和 P c P_c Pc 的关系是
P c = P b − t . P_c = P_b - {\bf t}. Pc=Pb−t.
由于不涉及旋转，这里 t {\bf t} t 的坐标表示，无论使用它在坐标系 O b O_b Ob 中的坐标，还是使用它在坐标系 O c O_c Oc 中的坐标，都是一样的。
综上所述，坐标系 O a O_a Oa 到坐标系 O c O_c Oc 的变换可以表示为：
P c = R b a ⋅ P a − t . P_c = {\bf R}_{ba} \cdot P_a - {\bf t}. Pc=Rba⋅Pa−t.
利用该公式，可以在已知一个点 P P P 在坐标系 O a O_a Oa 中的坐标 P a P_a Pa 时，求出它在坐标系 O c O_c Oc 中的坐标 P c P_c Pc。
为了把上式写成两个矩阵相乘的形式，我们使用齐次坐标且把减号改成加号。使用齐次坐标且令 t c b = − t {\bf t}_{cb} = - {\bf t} tcb=−t，则坐标系 O a O_a Oa 到坐标系 O c O_c Oc 的变换可以表示为：
[ 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}. [Pc1]=[Rba0Ttcb1]⋅[Pa1].
其中
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} Tca=[Rba0Ttcb1]
是一个四阶矩阵，称为变换矩阵。
上面的变换比较简单，所以我们能根据坐标轴的几何关系推导出旋转和平移。这个推导过程仅用于理解旋转和平移的几何意义，实际应用中求解旋转和平移，会使用分解单应矩阵/本质矩阵、PnP、ICP、Sim3 等算法。
上面我们把旋转和平移看成两次变换，从坐标系 O a O_a Oa 经坐标系 O b O_b Ob 到坐标系 O c O_c Oc 的两次变换可以表示为
P c = R b a ⋅ P a + t c b . P_c = {\bf R}_{ba} \cdot P_a + {\bf t}_{cb}. Pc=Rba⋅Pa+tcb.
实际应用中，我们通常把它们看成从坐标系 O a O_a Oa 到坐标系 O c O_c Oc 的一次变换
{ 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. ⎩ ⎨ ⎧PcTca=Rca⋅Pa+tca,=[Rca0Ttca1].
其中 R c a = R b a {\bf R}_{ca} = {\bf R}_{ba} Rca=Rba， t c a = t c b {\bf t}_{ca} = {\bf t}_{cb} tca=tcb。需要注意的是，此时 t c a {\bf t}_{ca} tca 是坐标系 O c O_c Oc 中的向量，而不是坐标系 O a O_a Oa 中的向量。
所有旋转都可以分解为绕坐标轴的旋转。当坐标系发生了旋转后，我们可能需要知道一个静止的点在旋转前后的坐标系中的坐标关系；当向量发生了旋转后，我们可能需要知道该向量旋转前后在同一个坐标系中的坐标关系。这些坐标关系就是旋转，本节借助 Eigen 库求这样的旋转。
图 2.2 的左图表示的是，从 Z 轴负方向看向正方向时，绿色坐标系 O 1 O_1 O1 绕 Z 轴逆时针旋转 90° 变成红色坐标系 O 2 O_2 O2 的变换。这个变换可以用 R 21 R_{21} R21 表示。借助 Eigen 库表示这样的旋转 R 21 R_{21} R21：
Eigen::AngleAxisd R21(M_PI / 2, Eigen::Vector3d::UnitZ());
已知点 P P P 在坐标系 O 1 O_1 O1 中的坐标 P 1 P_1 P1，可以求得它在坐标系 O 2 O_2 O2 中的坐标 P 2 P_2 P2： P 2 = R 21 ⋅ P 1 P_2 = R_{21} \cdot P_1 P2=R21⋅P1。
图 2.2 的中图，同样是 P 1 P_1 P1 经 R 21 R_{21} R21 到 P 2 P_2 P2 的旋转。不同的是，此时 R 21 R_{21} R21 表示的是，从 Z 轴负方向看向正方向时，向量 O P 1 → \overrightarrow{OP_1} OP1 绕 Z 轴顺时针旋转 90° 变成向量 O P 2 → \overrightarrow{OP_2} OP2 。
综上所述，代码中的 R 21 R_{21} R21 既可以表示坐标系逆时针旋转，也可以表示向量顺时针旋转。表示坐标系逆时针旋转时， R 21 R_{21} R21 用于求静止点在旋转所得新坐标系中的坐标；表示向量顺时针旋转时， R 21 R_{21} R21 用于求向量顺时针旋转之后的新坐标。
我们把从坐标系 O a O_a Oa 到坐标系 O b O_b Ob 的变换表示成 T b a {\bf T}_{ba} Tba 而不是 T a b {\bf T}_{ab} Tab，这是 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}. Tda=Tdc⋅Tcb⋅Tba.
从上式的右侧向左侧看下标，很容易知道这是从坐标系 O a O_a Oa 变换到坐标系 O b O_b Ob、从坐标系 O b O_b Ob 变换到坐标系 O c O_c Oc、从坐标系 O c O_c Oc 变换到坐标系 O d O_d Od 的链式变换。
Eigen 是一个开源的线性代数库，我们可以借助它实现矩阵运算。
P a P_a Pa 和 P b P_b Pb 分别表示点 P P P 在坐标系 O a O_a Oa 和 O b O_b Ob 中的坐标。 t a t_a ta 和 t b t_b tb 分别表示向量 O a O b → \overrightarrow{O_a O_b} OaOb 在坐标系 O a O_a Oa 中的坐标和向量 O b O a → \overrightarrow{O_b O_a} ObOa 在坐标系 O b O_b Ob 中的坐标；或者说， t a t_a ta 和 t b t_b tb 分别表示原点 O b O_b Ob 在坐标系 O a O_a Oa 中的坐标和原点 O a O_a Oa 在坐标系 O b O_b Ob 中的坐标。 R b a R_{ba} Rba 是坐标系 O a O_a Oa 到坐标系 O b O_b Ob 的旋转。因为
P b = R b a ⋅ P a + t b , P_b = {\bf R}_{ba} \cdot P_a + {\bf t}_b, Pb=Rba⋅Pa+tb,
所以
P a = R b a T ⋅ ( P b − t b ) . P_a = {\bf R}_{ba}^T \cdot (P_b - {\bf t}_b). Pa=RbaT⋅(Pb−tb).
由此我们可以得到逆变换中的旋转和平移
{ 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. ⎩ ⎨ ⎧Rabta=RbaT,=−RbaT⋅tb.
下面使用 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;
}
上面说的变换都是默认先旋转再平移，当然变换也可以是先平移再旋转。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+tR⋅(P+t)usepretranslate,usetranslate.
位姿插值用于环形扫描的激光雷达等场景中。如果坐标系在 t 0 t_0 t0 到 t 1 t_1 t1 时刻做匀速直线运动或匀速圆周运动，且这两个时刻的位姿已知，则可以通过位姿插值求出它在 t 0 t_0 t0 到 t 1 t_1 t1 中间任意时刻的位姿。
#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 之间插值。
机体坐标系：机体坐标系是右手坐标系，x 轴指向机头，y 轴指向右机翼，z 轴指向下。欧拉角用于描述刚体绕机体坐标系坐标轴的旋转：滚转角 roll 表示绕 x 轴旋转的角度，俯仰角 pitch 表示绕 y 轴旋转的角度，偏航角 yaw 表示绕 z 轴旋转的角度。
当从旋转轴负方向看向正方向时，顺时针旋转的角度为正，逆时针旋转的角度为负。即，右倾时滚转角 roll 为正，爬升时俯仰角 pitch 为正，右偏时偏航角 yaw 为正。
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= r00r10r20r01r11r21r02r12r22 ,
那么，欧拉角的弧度值是
{ 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.. ⎩ ⎨ ⎧xrollypatchzyaw=arctanr22r21=−arctanr212+r222 r20=−arcsinr20=arctanr00r10.
使用 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;
}