多帧点云拼接的全局ICP算法【附Matlab代码链接】

用RGBD相机采集一组多视角深度点云，假设多帧点云之间有共视邻接关系，通常会先进行Pair-wise的帧间点云匹配，具体方法见另外一个帖子：
两帧点云刚性配准的ICP算法
连续的帧间ICP可以把点云变换到一个公共空间中（比如第一帧的三维坐标系），但是帧间配准一般只考虑当前帧和前一帧的对应关系，对于首尾两帧有共视即点云重叠的情况（在SLAM中称为回环），可能会由于累计误差导致这两帧点云并未按照预期重合在一起。

这种带回环的配准问题可以用全局ICP解决。假设K帧点云已经过两两初始配准放在一个公共的空间中，为了进一步提高配准精度，需要进行多视点云全局优化整体配准，此时。全局优化的目的是确定旋转平移参数Ri和ti(i=1…k)，使得整体配准误差E最小。E和两帧间的ICP一样，有’Point-point Error’和’Point-plane Error’两种。
优化过程的迭代步骤仍然分两步：
1).找对应点：每一帧点云的每个点都到具有邻接关系的相邻帧找对应点。因为没有其它线索，这个对应点只能是就近的点（Closest point）；
2).求解RTi：每帧有6个参数，那么k帧点云就有6*(k-1)的参数，作为方程的变量dX，雅可比矩阵J的构造如下：


线性化求解过程中的Jacobian矩阵和方程组的构造
以4个点云为例：

点云两两匹配邻接关系PC1-PC2-PC3-PC4-PC1，点云重叠区域才能产生匹配点(实际上PC1-PC3，或PC2-PC4之间也可能产生匹配点对）
有邻接关系近邻匹配点个数分别为N12,N23,N34,N14
利用“点面误差”求解， 共24个参数
一对点的观测对应方程的一行，左边Jacobian矩阵和右边残差项的定义，和点面误差ICP中一样，如下图：

如果固定其中一个点云，比如以PC1为基准，[R,t]=[I,0],求解参数变成18个，那么Jacobian矩阵中这6个参数对应的列要移到方程组的右边。

可见方程组的构造并不是很复杂，大家可以试着自己实现。也可以参考网上的一个Matlab实现，网址和截图如下：
https://www.geo.tuwien.ac.at/downloads/pg/pctools/pctools.html

备注：
点击下面链接，进入奥比中光开发者社区，了解更多3D视觉技术信息：https://developer.orbbec.com.cn/

或扫描下方二维码,进入奥比中光开发者社区：