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

用RGBD采集一组多视角深度点云，先要进行Pair-wise的点云匹配，具体方法见另外一个帖子：
https://developer.orbbec.com.cn/forum_plate_module_details.html?id=1521
假设多帧点云之间有邻接关系，连续的帧间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的构造如下：


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

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

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