一.点云配准介绍

1.点云配准的概念

图像配准是图像处理研究领域中的一个典型问题和技术难点，其目的在于比较或融合针对同一对象在不同条件下获取的图像，例如图像会来自不同的采集设备，取自不同的时间，不同的拍摄视角等等，有时也需要用到针对不同对象的图像配准问题。具体地说，对于一组图像数据集中的两幅图像，通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。一个经典的应用是场景的重建，比如说一张茶几上摆了很多杯具，用深度摄像机进行场景的扫描，通常不可能通过一次采集就将场景中的物体全部扫描完成，只能是获取场景不同角度的点云，然后将这些点云融合在一起，获得一个完整的场景。

简单点说，点云配准（Point Cloud Registration）指的是输入两幅点云 $P_s$ (source) 和 $P_t$ (target) ，输出一个变换 $T$ 使得 $T(P_s)$ 和 $P_t$ 的重合程度尽可能高。或者说，对于两个不同视角下的坐标系，比如世界坐标系和相机坐标系，我们需要求出一个变换 $T$ 使得两个坐标系变换到统一视角下。我们这里只考虑刚性变换，即变换只包括旋转、平移。

2.点云配准的过程

目前，传统的主流点云配准技术主要包括粗配准和精配准两个阶段。粗配准阶段的目的是，对于任意初始状态的两片点云，使得两片点云大致对齐，给旋转矩阵R和平移向量T提供初值。而精配准是在粗配准的基础上，进行更精确、更细化的配准。总而言之，点云配准的过程就是矩阵变换的过程。

二.点云粗配准算法

点云粗配准又被称为点云初始配准，旨在对任意初始位置的两片点云进行粗略的配准，使其大致对齐，从而为点云的精配准提供良好的初始位置。点云粗配准算法主要分为两大类，分别是基于全局搜索思想的配准方法和基于几何特征描述的配准方法。

1.基于全局搜索思想的配准方法

基于全局搜索思想的配准方法通常从源数据中随机地选择几个点（通常是三个），并根据对目标数据的穷举搜索从目标数据中找到对应的点，计算所有可能的变换矩阵，通过投票的方式或者选取误差函数最小的方式确定最优变换。这种通过考虑所有可能的对应关系，可以得到较好的配准效果，但往往会产生很大的计算负荷。其中最常用的算法框架是RANSAC（随机采样一致性）算法。

1.1 RANSAC点云配准算法

RANSAC 算法最早是在数学/统计学领域提出，它是一种利用随机采集的样本来准确拟合出整体数学模型参数的方法。它的主要思想是从给定的样本集中随机选取一些样本并估计一个数学模型，将样本中的其余样本带入该数学模型中验证，如果有足够多的样本误差在给定范围内，则该数学模型最优，否则继续循环该步骤。
后来，RANSAC算法被引入三维点云配准领域，产生了基于RANSAC思想的RANSAC点云配准算法，其算法过程主要如下：

其本质就是不断的对源点云进行随机样本采样并求出对应的变换模型，接着对每一次随机变换模型进行测试，并不断循环该过程直到选出最优的变换模型作为最终结果。根据大数定律，可知道在进行大量重复采样实验的情况之下，随机模拟可以近似地将正确结果求解出来。当然，RANSAC配准算法也存在着有限次随机性带来的不稳定配准和计算量大等弊端。

1.2 4PCS算法及其改进

4PCS算法全称为 4-Points Congruent Sets 即 4点全等集配准算法。该算法也是基于RANSAC算法框架，对两片点云的初始姿态不做约束，针对搜索对应点的策略进行了优化，将基本的三组对应点扩展到了四组具有一定约束性的对应点集，大大增加了算法的鲁棒性，提高了算法的搜索效率，算法的时间复杂度约为 $O(n^2+k)$ 。该算法的核心思想就是利用刚体变换中的几何不变性（向量/线段比例、点间欧几里得距离），根据刚性变换后交点所占线段比例不变以及点之间的欧几里得距离不变的特性，在目标点云中尽可能寻找４个近似共面点（近似全等四点集）与之对应，从而利用最小二乘法计算得到变换矩阵，基于RANSAC算法框架迭代选取多组基，根据最大公共点集（LCP）的评价准则进行比较得到最优变换。

（1）全等四点集

在4PCS算法中，我们将局部配准点云由三个点扩展为四个点，并且这四个点具有一定的附加约束，如果能够在目标点云中也找到相应的近似满足约束的四点集，我们就将这两个对应四点集称为全等四点集，用于求解点云变换。相比传统的RANSAC配准算法中完全随机采样的方式，通过全等四点集的应用，一方面算法减少了计算量，提高了效率，使得全局搜索更有目标性；另一方面算法使用带有约束的局部四点配准，准确性和鲁棒性更高。四点集的选择和约束标准如下：

首先在源点云中随机选择三个点，要求这三点所构成的三角形面积尽量的大（三点确定一个平面，向量叉积可以计算面积），但是三点间的距离不能超过一定的阈值上限，该上限由两片点云的给定重叠率 f 确定。因为三点之间距离越大，配准的鲁棒性越高；但距离过大，三点均在两点云的重叠区域之外了，配准效果又不好。因此需要在满足上限的基础之上，尽可能保证大的三级形面积。若没有给定点云重叠率f，则可以进行f=1.0,0.75,0.5…重叠率递减测试，选择最优变换。
确定三点后，源点云四点集中第四点的选择方式为：遍历源点云中所有的点，对每一个点进行计算验证选择最优的第四点。第四点需要与其他三点组成的平面尽可能的共面（即不强制要求四点共面，但第四点到其他三点平面的距离尽可能小），并且第四点与其他三点的距离也要满足距离阈值范围（不能太近不能太远）。
源点云中的四点集选择完成后，就可以计算其四点构成的两线段交点的变换不变比，根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集用于配准计算，这就是（近似）全等四点集。

（2）4PCS算法流程

在了解了全等四点集这一核心概念之后，我们来介绍一下4PCS的算法步骤如下：

STEP1：在源点云P中，使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4}，其中(b1,b2)确定线段1，(b3,b4)确定线段2。接着去计算不变量d1=|b1-b2|，d2=|b3-b4|，不变比r1=|b1-e|/|b1-b2|，r2=|b3-e|/|b3-b4|。注意因为四点不一定共面，两条线段也不一定相交，所以可以使用连接两个线段的最近点的中心点作为“交点”。、
STEP2：在目标点云Q中，遍历所有的点对，筛选满足第一个约束（线段长度在d1或d2误差范围内）的点对集合R1，R2。其表示为：

STEP3：遍历点对集合R1中的所有点对元素 $r1_i = {(q_i,q_j)}$ ，计算其线段上满足不变比r1的目标交点 $e1_i$ ，并将所有计算结果e存入搜索树ANN Tree，并构建相应的映射 $Map(e1_i) = r1_i = (q_i,q_j)$
STEP4：遍历点对集合R2中的所有点对元素 $r2_i = (q_i,q_j)$ ，计算其线段上满足不变比r2的目标交点 $e2_i$ ，并构建相应的映射 $Map(e2_i) = r2_i$ 。然后遍历所有的 $e2_i$ 点，在之前构建的ANN Tree中搜索可接受误差范围内的重合点 $e1_i$ ，若可找到则说明能在Q中找到一个对应的近似全等四点集 $U_i = \{e1_i,e2_i\}$ ，所有的近似全等四点集 $U = \{U_0,U_1,...\}$
STEP5：遍历所有的近似全等四点集 $U_i \in U$ ，对每一个 $U_i$ ，通过最小二乘法计算其与B的变换矩阵 $T_i$ 。然后使用变换矩阵对源点云P进行变换得到P’，统计P’与Q中的最大公共点集（LCP），记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。
STEP6：不断迭代这个过程，记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。