深度剖析点云配准中的数学基础

作者：智林 

引言

点云配准(点云配准初识)的目的是计算不同视角下若干组具有共视关系点云的空间变换关系(位置和姿态)，从而将这些点云统一到一个坐标系下，进而得到被观测物体的整体信息。那么，我们如何计算这种变换关系呢？或者说其对应的数学问题是怎么样的呢？

其实，点云配准本质就是一个优化问题，从几何关系来说就是使具有共视关系点云的空间距离足够小，接近于零；从空间分布关系来说，就是最大化共视关系的点落在所在栅格的概率分布。因此，对于一个点云配准这个技术问题，我们需要定义优化函数，运用优化方法去求解所定义的优化问题。所需要用到的数学知识包括，空间变换的表示，矩阵分析，优化方法(常见的高斯-牛顿法，LM算法)，导数求解等等。

接下来的内容就是分享其中的一部分数学基础知识，其中优化方法和导数求解建议大家先去看看高博的《视觉slam十四讲》，这里可能就不展开了，因为一旦展开就太杂了，迟迟无法进入点云配准算法的讲解(要讲的东西实在太多太多)，所以大家自己先去学习一下吧！后面如果有机会，我在对这部分内容详细讲解。其实，这两部分是自动驾驶算法中的关键基础知识，大家必须都要会，不然无论是看论文还是看代码都是一脸茫然，快去自己看书学习，不懂的可以加入交流群一起讨论交流。下面，我们就着重讲解空间变换关系的表示及其参数化以及刚性配准的基本数学问题表达与求解，和对应的c++代码。

空间变换及其参数化

我们已经知道点云配准就是计算空间变换关系，也就是位置和姿态。那么，用什么方法表示位置和姿态呢？每个表示方法又有什么优点和缺点呢？实际运用中我们如何选择呢？带着这些有趣的问题，开始接下来的学习。

向量在三维空间中的表示方法

我们以后再没有特别说明的情况下，讨论的都是三维空间，所讨论的点云配准也是三维点云配准。那么，我们首先要知道如何表示三维空间中的点，以及两点连线构成的向量。可能，这里大家会有疑问，我知道怎么表示点不就行了吗，为什么还要表示向量呢？原因是，点云配准需要求解的空间变换关系除了位置之外，还有姿态。点的信息只能刻画位置，两点连接所形成的向量就可以表示姿态，并且向量本身也可以表示位置，所以我们的核心实则是研究向量，其既包括位置信息又包括姿态信息。

在三维空间中，无论是研究点还是由点构成的向量，我们都必须要指定坐标系，只有在某个指定的坐标系下才能去研究点的表示以及向量的表示。

线性代数的知识告诉我们，只需要找到三维空间的一组基，就可以利用这组基表示三维空间下的任意向量。简单回顾一下基的定义：

在线性代数中，一个向量空间 的基（basis）是指满足以下两个性质的向量集合:

生成性质 (Spanning Property)：基中的向量能够通过线性组合生成向量空间 中的任意向量。换句话说，对于任意 中的向量 ，存在基中的向量 和标量 使得

线性无关性质（Linear Independence Property）：基中的向量之间不存在非零的线性组合使得结果为零向量。换句话说，如果 ，其中 是标量，且不全为零，则基中的向量线性相关。

假设我们已经找到了三维空间的一组基 , 那么, 任意一个向量 在这组基下可以表示为:

这里 称为 在此基下的坐标 。由此可见，坐标的具体取值，一是和向量本身有关,，二是和坐标系 (基) 的选取有关。通常情况下，坐标系由 3 个正交的坐标轴组成，也就是我们常说的笛卡尔坐标系的x，y，z轴，所选择的基就是

在这里，为了方便后续对向量的讨论，这里引出一个向量外积的概念，这个概念很简单，但是在slam领域非常重要和实用。先简单回顾一下高中学到的向量内积是啥。对于任意的，内积的定义为：

其中， 指向量 的夹角。虽然内积的概念从高中就已经知道了，但是那时候毕竟是为了应付高考，现在我们可以想一想这个有啥实际的工程或者学术价值。这里，我就抛砖引玉一下，内积在实际工程中，可以计算其数值与零比较大小，判断两个向量的夹角是否超过90度，进而判断移动的智能体是否发生空间位置的变化(大家好好琢磨，其中的一个向量是点的法向量，另外一个向量是当前位置与点连线构成的向量)。话不多时，赶紧来聊聊新的知识，向量的外积，向量外积的定义为：

外积的一个重要性质是：外积的结果是一个向量，并且垂直于这两个向量，其大小为。这里，有一个重要的符号引入，即，其作用就是将一个向量映射为一个矩阵，并且这个矩阵是一一映射（单射），这个矩阵是反对称的，称为反对称矩阵。其定义为：对于任意向量，有：

同理，对于任意一个反对称矩阵，我们有：。

坐标系间的欧式变换与旋转矩阵

根据上面的分析，我们知道我们研究的最底层东西是向量，这个向量只有在定义了坐标系之后才有意义。那现在我们在回过头想一想点云配准的核心是什么？点云配准是计算两个具有共视关系点云的变换关系，这里的具有共视关系区域的每个点大家可以理解成向量，这些点(向量)是在不同时刻不同坐标系下定义的，因此我们所要求得坐标变换关系也就是坐标系的变换关系。

通过上面的分析，我想大家应该可以比较彻底的弄明白为啥书上讲到点云配准，或者slam，一定要首先讲解坐标系的欧式变换，只有理解了，才会觉得书上的内容不是那么突兀和跳跃，而是非常合理的安排，哈哈。那下面我们就开始介绍坐标系的欧式变换，进而直接引出旋转矩阵。

两个坐标系之间的运动由一个旋转加上一个平移组成,这种运动称为刚体运动。由于刚体运动保持了向量的长度和夹角，相当于把一个刚体原封不动地进行移动或旋转，不改变它自身的形状，因此它是一种欧式变换(Euclidean Transform)。这里大家可能有这样的疑问，夹角为啥没有变化，我坐标系都发生旋转了，向量的夹角怎么还没有变化呢？没有这样的疑问最好，有这样的疑问也没事，因为只是绝大多数书上没有怎么解释这个夹角是什么意思。这里的夹角通常指的是两个向量之间的角度。在不同坐标系中，两个向量之间的夹角是不变的。这意味着，如果你有两个向量 和，它们的夹角在刚体运动时会保持不变。无论刚体怎么运动，这两个向量之间的夹角都不会发生变化。

这里借用高博书上的一个例子，刚好我自己最近也弄到了一台mate60pro昆仑玻璃加持，摔不坏，但是我没有尝试过，哈哈哈。想象你把手机抛到空中、在它落地掉碎之前，只可能有空间位置和姿态的不同，而它自己的长度、各个面的角度等性质不会有任何变化。手机并不会像橡皮那样一会儿被挤扁，一会儿被拉长。此时，我们说手机坐标系到世界坐标之间，相差了一个欧氏变换。

有了这些基础之上，我们终于可以引出旋转矩阵了。假设某个单位正交基经过了一次旋转变成(,那么对于向量 而言，它在两个坐标系下 (同一个点参考系不同) 的坐标分别为和。由于 本身没有改变，因此：

对上面的式子做一个小小的变形，两边同时左乘，左边直接变成了单位矩阵，因此有：

我们将中间的矩阵记为，其描述了同一个向量在旋转前后(姿态发生变化前后)的坐标坐标变化关系，这里并没有表达出位置的关系，因此称为旋转矩阵(Rotation Matrix)。

旋转矩阵有一个非常重要的性质：。

这里的证明大家可以看书，比较简单，但是这个性质非常非常重要，后面我们慢慢回味。旋转矩阵的概念可以推广到多维，将维旋转矩阵的集合定义为：

式中 SO(n)表示特殊正交群，旋转矩阵即描述了两个坐标的姿态变换关系：

如果考虑平移，则可以更新表示为。因此两个坐标系的刚体运动可以由和进行描述，但是该变换不是一个线性变换。假设我们进行了两次坐标变换：，这里简单说明一下， 是指“把坐标系 2 的向量变换到坐标系 1” 中，从右往左读，这里借鉴了高博的十四讲的做法，当然你也可以按照自己的习惯反过来都一样。

那么，从 到 的变换为

这样的形式在变换多次之后会显得很不简约。因此，引人齐次坐标和变换矩阵，重写上式：

将原始的三维向最末尾增加1个维度，取值为1，该四维向量被称为齐次坐标，而矩阵写作，称为变换矩阵(Transform Matrix)，这种类型的矩阵又被称为特殊欧式群：

式中，为 3×3 的矩阵， 的逆表示进行反向的变换：

有了变换矩阵这个工具之后，我们就可以对上面连续进行两次变换的叠加有一个简单直观的表示，用 表示 的齐次坐标。那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：

旋转向量/角轴/轴角

旋转矩阵描述了物体运动的3自由度旋转，在此基础上引入了变换矩阵，描述了6自由度的三维刚体运动，看似已经足够了，我们已经找到了点云配准所需计算的变换运动的表达形式。是的，的确是可以了，在计算的时候一般也是输出和，然后将点云投影到统一的坐标系下，最终得到点云。

但是，通过矩阵表示方式存在两个明显的不足，分别是：

旋转矩阵有9个量，但是一次选准只有3个自由度；变换矩阵有16个量，但是一次欧式变换只有6个自由度。因此，这种表示方式存在冗余，占用的内存多。由此，是不是可以找到一种紧凑的表达形式呢？

上述分析中提到旋转矩阵有一个重要的性质：旋转矩阵是行列式为1的正交矩阵。因此在后续估计或者优化一个旋转矩阵时，我们必要要保持这一性质，这样会让求解变得麻烦困难。

首先，可以肯定的说，存在一种紧凑的方式来描述平移和旋转，用一个三维向量表达旋转，用一个六维的向量表达变换。事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种问量称为**旋转向量(或轴角/角轴，Axis-Angle)**，只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。具体表达形式为：

式中， 与旋转轴方向一致的单位向量， 为角度。

乍一看，这种表示方式好极了，表示6自由度的变换恰到好处非常紧凑。但是，旋转向量也存在缺点：

使用轴角表示方式无法表达两次连续的旋转，由于两次旋转的旋转轴会不一样，因此旋转角度不能直接相加。

当旋转角度为 0°或者 180°时，罗德里格斯公式失效，此时的旋转矩阵为 0。可以理解为这种情况下旋转角具有无数多种情况。由于旋转矩阵到轴角表示法的映射关系是一对多的关系，因此存在多种轴角组合对应一种旋转矩阵的情况。

这里简单解释一下，因为后面我们单独会讲各种变换关系之间的转化，其中罗德里格斯公式也会详细介绍，这里直接给出：

罗德里格斯公式描述的是旋转向量到旋转矩阵的转换关系，当或者时，。

欧拉角

通过上述分析知道，旋转矩阵和旋转向量都可以表示旋转，但各自又都有缺点，不是很完美。除了已经谈到自身固有的缺点之后，它们两还有一个非常大的bug，那就是不直观。给你一个旋转矩阵或者旋转向量在你面前，比较难去想像这个旋转究竟是什么样的，或者比较简单的旋转向量还可以，但是一旦旋转轴复杂一些，就懵了。那下面就介绍一个非常直观的表示旋转的一种方式------欧拉角。

欧拉角使用了3个分离的转角，把一个旋转分解成 3 次绕不同轴的旋转。对于我们来说，很容易理解绕单个轴旋转的过程。但是，这里也带来一定的问题，由于分解方式有许多种，所以欧拉角也存在着众多不同的、易于混淆的定义方法。例如，先绕 轴，再绕 轴，最后绕 轴旋转，就得到了一个 轴的旋转，同理也可以得到，，甚至可以定义，等等。

这种定义方式上的不确定性带来了很多实际当中的困难，但是在某一个特定的领域，欧拉角通常有统一的定义方式。在航天航空领域，欧拉角便有一种约定俗成的定义方式，便是用“偏航- 俯仰-滚转"(yaw-pitch-roll)3个角度来描述一个旋转。它等价于 轴的旋转，因此就以 为例。假设一个刚体的前方( 朝向我们的方向)为 轴，右侧为轴，上方为轴，如下图所示。那么， 转角相当于把任意旋转分解成以下 3 个轴上的转角：

绕物体的 轴旋转，得到偏航角 yaw。

绕旋转之后的轴旋转，得到俯仰角 pitcho

绕旋转之后的 轴旋转，得到滚转角 roll。

此时，可以使用 这样一个三维的向量描述任意旋转，并且这个非常直观。所以在跨行业或者跨模块协作的时候，一定要问清楚对方是哪一种欧拉角。

欧拉角一般可以定义为静态和动态两种形式。在介绍静态和动态两种形式之前，首先定义绕各个旋转轴转相应角度时所应对的选准矩阵。

当绕轴旋转 角度时对应的旋转矩阵为：

当绕轴旋转 角度时对应的旋转矩阵为：

当绕轴旋转 角度时对应的旋转矩阵为：

这三个矩阵怎么得到的，大家可以去看我b站视频，因为这里画图太麻烦了，实则非常简单，大家直接百度也可以搜到。那下面就来介绍静态和动态欧拉角。

所谓静态的欧拉角指绕世界坐标系三个固定轴的旋转，旋转过程中世界坐标系的三个轴不发生变化，也称为外旋旋转方式。如图 2-2 所示，按照外旋方式，X-Y- 旋转顺序 (指先绕固定轴X 旋转 α, 再绕固定轴 Y 旋转 , 最后绕固定轴 旋转 ), 这种形式的欧拉角也称为 RPY 角，α、β和γ对应航空领域的 Roll、Pitch 和 Yaw。最终可得旋转矩阵

而动态欧拉角是指绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着旋转变换运动，也称为内旋旋转方式。如图 2-3 所示，此时若按照 的顺序依次旋转 、 和 α 的角度，这种形式的欧拉角也称为 欧拉角，则最终可得旋转矩阵

这里要注意：内旋时绕自身坐标系旋转，右乘坐标向量，坐标系(基底)在变换， 变换；外旋时绕固定坐标系旋转，左乘坐标向量，坐标(向量) 在变换，行变换。这种情况下如果使用 RPY 角进行描述，即绕物体的 轴旋转 , 得到偏航角 Yaw, 绕旋转之后的 Y轴旋转,得到俯仰角 Pitch, 绕旋转之后的 轴旋转 , 得到滚转角 Roll。因此 Yaw、Pitch 和Roll 分别对应 、 和

通过上述分析，可以看出来欧拉角非常直观，便于立即。但是这个方法同样也存在缺点，即著名的万向锁(或万向节死锁)(Gimbal Lock)问题：在俯仰角为 时， 第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由3次旋转变成了2次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要想用3 个实数来表达三维旋转，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中。我们也很少在 SLAM 程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转(因为它具有奇异性)。不过，若你想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。

四元数

四元数最早于 1843 年由哈密顿（William Rowan Hamilton ) 发明，作为复数 ( Complex Numbers) 的扩展。直到1985 年才由 Shoemake 把四元数引入到计算机图形学中。四元数在一 些方面优于欧拉角、轴角和旋转矩阵，比如解决万向锁问题。由于仅需存储4 个浮点数，相比矩阵更加轻量，使得四元数解决求逆、串联（多个变换的叠加变换）等操作，相比矩阵更加高效。总结一下，四元数具有一下两个良好的性质：一是其表示旋转是紧凑的；二是其不存在奇异性。

这里应用高博十四讲中的做法引出单位四元数可以表示三维旋转。例如，当我们想要将复平面的向量旋转 角时，可以给这个复向量乘以 e。这是极坐标表示的复数，它也可以写成普通的形式，只要使用欧拉公式即可：

这正是一个单位长度的复数。所以，在二维情况下，旋转可以由单位复数来描述。类似地，我们会看到，三维旋转可以由单位四元数来描述。

一个四元数由一个实部和三个虚部构成，表达形式如下：

其中，,,为四元数的三个虚部。这三个虚部满足以下关系式：

为了方便表示四元数，通常用一个标量和一个向量来表达四元数：

这里， 称为四元数的实部，而 称为它的虚部。如果一个四元数的虚部为 0, 则称为实四元数；反之，若它的实部为0, 则称为虚四元数。

四元数也可以进行各种运算，例如：加法。乘法、共轭，求逆，数乘等等，这些也都是我们工程或者学习中经常使用。这里直接给出其结论：

对四元数有了初步的了解之后，下面我们来考虑四元数如何表示旋转？

可以用四元数表达对一个点的旋转。假设有一个空间三维点 。以及一个由单位四元数 指定的旋转。三维点 经过旋转之后变为 。如果使用矩阵描述，那么有。而如果用四元数描述旋转，它们的关系又如何表达呢？

首先，把三维空间点用一个虚四元数来描述：

相当于把四元数的 3 个虚部与空间中的 3 个轴相对应。那么，旋转后的点 可表示为这样的乘积：

这里的乘法均为四元数乘法，结果也是四元数。最后把的虚部取出，即得旋转之后点的坐标。这是为什么呢？因为上述公式的计算结果的实部为 0, 故为纯虚四元数。

关于四元数更多的知识，以及上述我所提到的知识的深入讲解，大家可以参考论文《Quaternion kinematics for the error-state Kalman filter》。

总结

这一小节我们主要介绍了如何表示三自由度的旋转，讲解了四种表示方式，分别为：旋转矩阵，旋转向量，欧拉角和四元数，它们各有其优势和不足，大家可以自己思考一下，回顾一下自己是否掌握了。现在我们已经解决了一个非常棘手的问题，就是用什么工具来表示点云配准中的变换关系。但是，我们有面临一个苦恼，这么多方法我怎么选择呀？或者说我计算出其中一个能不能得到另外三个？答案是肯定的，它们之间可以相互转换，那一节我们将探究空间变换的不同表示方法之间的互相转换。

参考资料

[1] 高翔等，视觉slam十四讲[M].

[2] 郭浩，点云配准从入门到精通[M].

[3] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.

编辑：黄飞