『OPEN3D』1.8 点云的配准理论

点云的配准是将不同的3D点云对齐成一个完成的点云模型；配准的目标是找到两帧点云之间的相对旋转（rotation）与平移（translation），使得两份点云中有重叠的区域能够完好拼接。

点云配准示例图（来自PCL） 

上图为初始的5份点云数据，需要将着几份点云数据拼接成如下图中完整的模型

点云动态配准示例

常见的点云配准方式主要包含ICP配准以及NDT配准以及他们的配准，当然还包括神经网络的点云配准方式。

1、点云配准理论

点云配准需要找到两份点云数据之间点的对应估计，也就是找到两份点云中重叠的部分才可以进行配准，这类配准，对于有确定对应点的配准拥有闭式解（closed form solution）不需要迭代求解即可获取最优值。

       对应点的寻找方式：

1 若为深度相机，则可以借助图像信息来寻找对应的匹配点并投影回3d空间

2 若为两帧点云信息，则可以借助点云特征点来进行确立

不过上诉方法都会出现误匹配，一般会借助RANSAC（随机采样一致性）来得到更为robust的估计。

1 ICP主要为point2point ICP和point2plane两种ICP方式

ICP配准点云时需要有较为准确的初始位姿后再使用该方法进行精配准；ICP的点云配准大体步骤分为如下几步：

1 点的匹配（可以从图像中来或者点云中来）

        1.1 在两帧点云中找到对应的关键点，点云中的关键点一般为特定的几何结构，比如书本的边角，与图像类似；点云中的关键点包括NARF;SIFT;FAST；当然也可以不适用关键点；使用每一个点或者点云中的一部分点来进行配准；不过那样的话会导致计算量过大。

        1.2 特征的描述子；与图像类似，再找到点云的关键点后，需要提取该关键点的描述子信息；通过组合该结构的信息来生成对应的向量用于比对；点云的中描述子包括NARF;FPFH;BRIEF;SIFT等。

2 匹配点估计；给定两帧点云的特征向量集合；通过关键点与描述子来找到重叠区域中相互对应的关键点：对于关键点或特征的匹配，可以使用暴力搜索；kd-tree（FLANN）等方式来进行匹配

3 匹配点对估计；类似与图像中关键点匹配；点云中的关键点匹配也存在许多误匹配；误匹配会影响最终的配准结果；因此此处可以使用ransac或者根据匹配点对的百分位进行截取等方式来进行优化；如果一帧中的某个关键点与另一帧中的多个关键点匹配；则会取最小距离来确定匹配点

4  变换矩阵计算；再得到准确的相互匹配的点云后；则可以计算变换矩阵；

        其中point2point的评估误差为

        point2plane的评估误差为（其中np为点P的法线信息）

        

估计两个点集的R和t可以使用使用SVD或者非线性优化求解； 

1 SVD方法

        给定两对匹配点的点集和，对其中第i对匹配点的误差项为：

因此可以对所有点构建最小二乘问题，求得使误差平方和达到极小的R和t：

分别定义两组点的质心为(此处使用不带下标的项来代表质心)：

在误差函数中进行展开

展开后最后一项元素求和为0(所有元素相加减去n个的质心为0),因此优化的结果仅与前两项相关，即

上式中，只有左边和旋转矩阵R相关，右侧同时R与t。因此只要求得R后令右式为0即可求出t。

ICP的SVD方法流程如下

        1、计算两组匹配点的质心与,  然后将每个点减去质心得到去质心的坐标：

   

        2、根据上述阐述的优化，计算旋转矩阵：

        3、根据计算得到的旋转矩阵计算平移向量t

                        

因此，按照上面的步骤，要先求取两组点集之间的旋转矩阵，所以下面说重点一下R的计算：

其中第一项与R无关；第二项中因为R为正交矩阵因此，也与R无关；只有最后一项的与R有关,因此有如下推导：

        

               注： 其中为trace(迹)，有

        然后可以使用SVD分解得出最优的旋转矩阵R。

        可知W是一个n*n的矩阵，n为点集的维度，若是点云数据则n=3则W是3*3的矩阵，

若此时W是可逆矩阵，则可以直接通过公式获得最优的旋转矩阵

但考虑到所有情况，直接对W进行SVD分解也是可以的：

                

其中为奇异值组成的对角矩阵，对角线元素从大到小排列，UV则为正交矩阵；当W满秩时，R为

        

若R的行列式为负数，则去-R为最优数值。

对这块刚兴趣的同学可以参考Kabsch算法的实现

https://en.wikipedia.org/wiki/Kabsch_algorithmhttps://en.wikipedia.org/wiki/Kabsch_algorithm

2 非线性优化

2 open3d中使用ICP

更新中