视觉SLAM中的数学基础 第一篇 3D空间的位置表示
前言
转眼间一个学期又将过去，距离我上次写《一起做RGBD SLAM》已经半年之久。《一起做》系列反响很不错，主要由于它为读者提供了一个可以一步步编码、运行的SLAM程序，为读者理解SLAM实现的细节作了详细的介绍。但是我也有很多对它不满意的地方。作为面向实现的介绍，它的代码不够稳定可靠，例如，甚至没有对匹配丢失的情况进行处理，因而只能用于教学。另一方面，对SLAM研究者来说，我只是介绍了编码方面如何调用一些常见的库函数，而没有对这些函数进行深入的，原理上的讲解。这就导致了读者只了解了函数的接口，而没法根据数学原理进行创新。归根到底，研究机器人相关问题，一是要有扎实的数学基础，二是要有强大的动手编程能力，这对大多数刚入门的研究者来说，极具挑战性。我也希望，通过阅读我的博客，你能走进SLAM研究的门槛，有朝一日自己也写出优秀的程序和论文。
有鉴于此，我准备写一写SLAM相关的数学知识，包括代数、几何、概率、运筹等等。对于重要的算法例如ICP，EKF，细致讨论它的原理，并给出它的实现（原生的代码或在某个库的实现）。由于它们的原理较复杂，我会从最基本的东西开始讲起。但是我毕竟不是在写数学书，我不会像数学书那样写成``定义——定理——推论”的结构。我们不会纠缠于一些定理的严格证明，相反的，我们只在必要的情况下加以说明，告诉读者这些数学公式在SLAM中有何应用，如何应用。
由于博客编辑器的限制，我们以斜体字$x$表示变量，以粗正体$mathbf{A}$表示矢量和字母，以黑板粗体$mathbb{R}$表示空间。希腊字母没有粗体所以保持原样。向量默认为列向量。其余和普通的数学书一致。
小萝卜：师兄，这么严肃不是你的风格啊！
师兄：啊，数学嘛……
刚体运动
本篇我们讨论一个很基础的问题：如何描述机器人的位姿。这也是研究SLAM的第一个问题。注意这里“位姿”的用语包含了位置和姿态。描述位置是很简单的。如果机器人在平面内运动，那么用两个坐标来描述它的位置：$$ mathbf{x} = left[ x, y ight]^T.$$ 相应的，如果它在三维空间中，我们就用三个空间坐标来表示：$$ mathbf{x} = left[ x, y, z ight].$$
姿态的表达比点稍为复杂。2D的姿态可以只用一个旋转角$ heta$表达。3D姿态的表达方式则有多种。常见的如欧拉角、四元数、旋转矩阵等。我们将在后文详细介绍。有了位置和姿态，我们就可以描述一个坐标系。进一步，还能描述坐标系间的变换关系。常见的问题如：机器人视野中某个点，对世界坐标系的（或地图的）哪个点？这时，就需要先得到该点针对机器人坐标系坐标值，再根据机器人位姿转换到世界坐标系中。
齐次坐标系
在位姿转换中，通常采用射影空间的齐次坐标表示。齐次坐标是什么呢？记$n$维射影空间为$mathbb{P}^n$，其中一个空间点的坐标为普通的3D坐标加一个齐次分量：$$ mathbf{x}=left[ x_1, ldots, x_n, w ight]^T. $$ 例如，在2维和3维射影空间中的点，分别表示为：
[egin{equation}
mathbf{x}_{2D} = left[x, y, w
ight]^T quad mathbf{x}_{3D} = left[ x, y, z, w
ight]^T
end{equation}]
小萝卜：既然一个空间点只有3个坐标，为啥非要用四个数表示呢？
师兄：嗯，四个数表示点，说明点和坐标肯定不是一一对应的。没错，在齐次坐标中，某个点$mathbf{x}$的每个分量同乘一个非零常数$k$后，仍然表示的是同一个点。因此，一个点的具体坐标值不是唯一的。如$left[1,1,1,1 ight]^T$和$left[2,2,2,2 ight]^T$是同一个点。但在$w eq 0$时，我们可以对每个坐标除以最后一项$w$，强制最后一项为1，从而得到一个点唯一的坐标表示：
[egin{equation}
mathbf{x}_{3D} = left[ x/w, y/, z/w, 1
ight]^T
end{equation}]
这时，忽略掉最后一项，这个点的坐标和欧氏空间就是一样的。那么，为要用齐次坐标呢？原因有以下几条。
- 齐次坐标下点和直线（高维空间里为超平面）能够使用同样的表达。
例如，3D空间$mathbb{R}^3$中，一个平面$pi$可由一个方程定义：
[egin{equation}
pi : ax + by + cz + d = 0
end{equation}]
则该平面$pi$可以用$mathbb{P}^3$中的坐标$mathbf{pi} = left[ a,b,c,d
ight]^T$来描述。这样，点位于平面上（2D对应点位于直线上）的事情可以简洁地表示为：
[egin{equation}
label{eq:pointOnPlane}
mathbf{pi}^T mathbf{x} = 0.
end{equation}]
把点和超平面采用同样的表示，这种做法一个非常直接的好处，是射影几何里的“对偶原理”。该原理是说，任何有关“点”与“平面”的命题，都可以交换“点”与“平面”的概念，得到一个对偶的命题。对偶命题和原命题是一样的。通过“对偶原理”，射影几何的数学家就可以偷懒，只需要证一半定理，因为对偶命题和原命题有同样的涵义。例如，我们证明了$mathbb{P}^2$中某条件下三点共线，那么替换概念后的三线共点则自然成立。
小萝卜：数学家真是好懒啊！
- 齐次坐标能囊括无穷远点与无穷远超平面。
最后一项坐标为零的点称为无穷远点，它们在$mathbb{P}^n$中真实存在，且能够很方便地参与正常的代数运算。根据式
ef{eq:pointOnPlane}，易见所有无穷远点都在一个平面$mathbf{pi}_infty = left[ 0, 0, 0, 1
ight]^T$上，该平面记作无穷远平面（2D对应无穷远直线）。
$mathbb{P}^2$中的无穷远直线较容易理解。它就像是地平线，与所有直线相交于位于它之上的无穷远点。而且，在射影变换中（例如照相），很容易在照片中看到地平线并算出它的方程。这说明2D射影变换会把无穷远线变成通常的直线。
- 齐次坐标可以方便地将平移与旋转放在一个矩阵中。
师兄：这应该是最明显的好处啦！大家都爱用齐次坐标，包括我。有关坐标系怎么用齐次坐标进行变换，后文会详细解释。现在我们能表达点了，还剩下一个姿态。由于2D与3D差别较大，我们分而述之。
2D姿态的描述
2D空间中，物体的位姿可用两个平移量$t_x, t_y$加一个旋转角$ heta$表示，如下图所示。此时，设机器人坐标系$O'-X'-Y'$下某点的坐标为$left[ x_r, y_r ight]^T$，对应在世界坐标系$O-X-Y$下为$left[x_w, y_w ight]^T$，那么由直观推得：
[egin{equation}
left{ egin{array}{l}
{x_w} = {x_r}cos heta - {y_r}sin heta + {t_x}\
{y_w} = {x_r}sin heta + {y_r}cos heta + {t_y}
end{array}
ight.
end{equation}]
读者可以自行尝试推导一下，在虚线处建立一个中间坐标系即可。若将该式写成矩阵形式，则有：
[egin{equation}
label{eq:2dTransRT}
mathbf{x}_w = mathbf{R} mathbf{x}_r + mathbf{t}
end{equation}]
其中
$$mathbf{R} = left[ {egin{array}{*{20}{c}}
{cos heta }& - sin heta \
{sin heta }&{cos heta }
end{array}}
ight], quad mathbf{t} = left[ t_x, t_y
ight]^T$$
$mathbf{R}$称为旋转矩阵，$mathbf{t}$为平移矢量。注意到$mathbf{R}$是一个正交矩阵，且只有一个自由度。加上平移矢量后，一共有三个自由度。
此定义下的旋转阵必是正交阵。而正交阵并非全是旋转矩阵。事实上，行列式为$+1$的正交阵才是旋转矩阵，行列式为$-1$的正交阵是镜像后的旋转矩阵。
式
ef{eq:2dTransRT}中，$mathbf{x}_w$和$mathbf{x}_r$还不是线性关系。下面我们用齐次坐标表示它们，即：
$$ mathbf{x}_w = left[ x_w, y_w, 1
ight]^T, mathbf{x}_r = left[ x_r, y_r, 1
ight]^T $$
则有：
[egin{equation}
{mathbf{x}_w} = left[ {egin{array}{*{20}{c}}
mathbf{R}_{2 imes 2} & mathbf{t}_{2 imes 1 }\
mathbf{0}^T_{1 imes 2} & I_{1 imes 1}
end{array}}
ight]{mathbf{x}_r}
end{equation}]
为便于理解，我们在矩阵下方标出了它的维数。可以看使用齐次坐标标满足了线性关系，记作：
[egin{equation}
mathbf{x}_w = mathbf{T}_{w,r} mathbf{x}_r
end{equation}]
其中$mathbf{T}_{w,r}$表示从世界坐标系到机器人坐标系的变换矩阵。我们也可以轻松地写出反向的变换矩阵：
[egin{equation}
mathbf{x}_r = mathbf{T}_{r,w} mathbf{x}_w = mathbf{T}^{-1}_{w,r} mathbf{x}_w = left[ {egin{array}{*{20}{cc}}
{{mathbf{R}^{ - 1}}}&{{-mathbf{R}^{ - 1}} mathbf{t}}\
{{0^T}} & 1
end{array}}
ight] mathbf{x}_w
end{equation}]
既然如此，我们就可用$mathbf{T}$表示机器人的位姿，那么机器人在时刻$t$的位姿就可以记作$mathbf{T}_t$。当然，从存储上来讲，存储$mathbf{T}$是不经济的。在2D运动中，它有九个变量，但实际自由度只有三个。所以我们可以只存储位移矢量$mathbf{t}$与旋转角$ heta$，而在需要计算的时候再构建出$mathbf{T}$。称2D欧几里得变换，它对矩阵乘法构成群(群是一个集合加一种运算，且运算在该集合上满足封闭性、结合律、有单位元和逆元。)，该群记作$SE(2)$。相应的，二维旋转构成二维旋转群(或称特殊正交群)$SO(2)$。有关它们进一步的性质，我们会在以后的李群、李代数中提到。
3D变换
3D的旋转可以由旋转矩阵、欧拉角、四元数等若干种方式描述，它们也统称为三维旋转群$SO(3)$。而3D的变换即旋转加上位移，是为$SE(3)$。为了和2D变换统一起见，我们首先介绍旋转矩阵表示法。
旋转矩阵描述
旋转矩阵是一种$3 imes 3$的正交矩阵，它对变换的描述十分类似于2D情形。参照上一节的数学符号，我们有：
[egin{equation}
{mathbf{x}_w} = left[
{egin{array}{*{20}{c}}
mathbf{R}_{3 imes 3} & mathbf{t}_{3 imes 1 }\
mathbf{0}^T_{1 imes 3} & 1_{1 imes 1}
end{array}}
ight]{mathbf{x}_r}
end{equation}]
这里$mathbf{R}$为3D的旋转矩阵，同样的，$mathbf{t}$为3D的平移矢量。
由于3D旋转都可以归结成按照某个单位向量$mathbf{n}$进行大小为$ heta$的旋转。所以，已知某个旋转时，可以推导出对应的旋转矩阵。该过程由罗德里格斯公式表明，由于过程比较复杂，我们在此不作赘述，只给出转换的结果：
[egin{equation}
mathbf{R}(mathbf{n}, heta) = left[ {egin{array}{*{20}{c}}
{n_x^2left( {1 - c heta }
ight) + c heta }&{{n_x}{n_y}left( {1 - c heta }
ight) + {n_z}s heta }&{{n_x}{n_z}left( {1 - c heta }
ight) - {n_y}s heta }\
{{n_x}{n_y}left( {1 - c heta }
ight) - {n_z}s heta }&{n_y^2left( {1 - c heta }
ight) + c heta }&{{n_y}{n_z}left( {1 - c heta }
ight) + {n_x}s heta }\
{{n_x}{n_z}left( {1 - c heta }
ight) + {n_y}s heta }&{{n_y}{n_z}left( {1 - c heta }
ight) - {n_x}s heta }&{n_z^2left( {1 - c heta }
ight) + c heta }
end{array}}
ight]
end{equation}]
这里$mathbf{n} = left[ n_x, n_y, n_z
ight]^T, c heta=cos heta, s heta=sin heta$。公式虽然较为复杂，但实际写成程序后，只需知道旋转方向和角度后即可完成计算。另一件有趣的事是，如果用$$mathbf{n}^{wedge} = left[ {egin{array}{*{20}{c}}
0&{ - {n_z}}&{{n_y}}\
{{n_z}}&0&{ - {n_x}}\
{ - {n_y}}&{{n_x}}&0
end{array}}
ight] $$表示与$mathbf{n}$对应的一个反对称矩阵，那么有：
[egin{equation}
mathbf{R} (mathbf{n}, heta ) = cos heta mathbf{I} + (1-cos heta ) mathbf{n} mathbf{n}^T + sin heta mathbf{n}^{wedge} = exp ( heta mathbf{n}^{wedge} )
end{equation}]
最后那个指数，读者若不理解，可以暂时不管它，这将在之后的李代数中会讲到。根据此式，我们也可以从任意给定的旋转矩阵，求出对应的转轴与转角。关于转角$ heta$，我们对上式两边求矩阵的迹，可得：
[egin{equation}
egin{array}{lll}
trleft( mathbf{R}
ight) &=& cos heta trleft( mathbf{I}
ight) + left( {1 - cos heta }
ight)trleft( { mathbf{n} {mathbf{n}^T}}
ight) + sin heta tr({mathbf{n}^ wedge })\
&=& 3cos heta + (1 - cos heta )\
&=& 1 + 2cos heta
end{array}
end{equation}]
因此：
[egin{equation}
heta = arccos ( frac{1}{2} [ tr(mathbf{R}) - 1 ] )
end{equation}]
关于转轴$mathbf{n}$，由于旋转轴上的向量在旋转后不发生改变，说明$$mathbf{R} mathbf{n} = mathbf{n}. $$
因此，只要求此方程的解向量即可。这也说明$mathbf{n}$是$mathbf{R}$特征值为$1$的一个特征向量。
总之，读者应当明白在3D时，旋转和平移仍可用转移矩阵$mathbf{T}$来描述，其结构也与2D类似。而$mathbf{T}_{4 imes 4}$构成了三维欧氏变换群$SE(3)$。注意到$mathbf{T}$虽然有16个变量，但真正的自由度只有6个，其中3个旋转，3个位移。
旋转矩阵描述是一种比较适合数学推导及计算机实现的方式，但它不符合人类的思维方式。当你看到一个$3 imes 3$的矩阵时，很难想象物体实际上发生了怎样的旋转。反之，给定一个旋转方式，人也很难直接写出矩阵的数值。所以，为了便于人类理解，人们还使用了其他方法来表示三维旋转。
欧拉角
欧拉角是一种广为使用的姿态描述方式，以直观见长。在最常用的欧拉角表达方式中，我们把旋转分解成沿三个轴转动的量：滚转角－俯仰角－偏航角(roll-pitch-yaw)。它的好处是十分的直观，且只有三个参数描述。缺点是会碰到著名的万向锁问题：在俯仰为$pm 90 ^circ $时，表达某个姿态的形式不唯一。此外，它也不易于插值和迭代。
我们并不会详细介绍欧拉角，因为它在SLAM中用处也很有限。我们既不会在数学推导中使用它，也不会在程序中用欧拉角表示机器人的姿态。不过，在您想验证自己算法输出的姿态是否有错时，转换成欧拉角能让你快速辨认结果是否有错。
四元数
四元数原理和各种运算将在下一篇博客中提到。
本篇小结
本篇博客介绍了2D和3D空间中刚体运动的表示方法，以旋转矩阵为主。下一篇我们将介绍四元数表示法，然后演示如何用Eigen3对这些矩阵进行操作。敬请期待。
如果你觉得我的博客有帮助，可以进行几块钱的小额赞助，帮助我把博客写得更好。
