多帧点云拼接的全局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的构造如下:
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第1张图片
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第2张图片
线性化求解过程中的Jacobian矩阵和方程组的构造
以4个点云为例:
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第3张图片
点云两两匹配邻接关系PC1-PC2-PC3-PC4-PC1,点云重叠区域才能产生匹配点(实际上PC1-PC3,或PC2-PC4之间也可能产生匹配点对)
有邻接关系近邻匹配点个数分别为N12,N23,N34,N14
利用“点面误差”求解, 共24个参数
一对点的观测对应方程的一行,左边Jacobian矩阵和右边残差项的定义,和点面误差ICP中一样,如下图:
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第4张图片
如果固定其中一个点云,比如以PC1为基准,[R,t]=[I,0],求解参数变成18个,那么Jacobian矩阵中这6个参数对应的列要移到方程组的右边。

可见方程组的构造并不是很复杂,大家可以试着自己实现。也可以参考网上的一个Matlab实现,网址和截图如下:
https://www.geo.tuwien.ac.at/downloads/pg/pctools/pctools.html
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第5张图片
备注:
点击下面链接,进入奥比中光开发者社区,了解更多3D视觉技术信息:https://developer.orbbec.com.cn/

或扫描下方二维码,进入奥比中光开发者社区:
多帧点云拼接的全局ICP算法【附Matlab代码链接】_第6张图片

你可能感兴趣的:(3D视觉,开发者,结构光,matlab,算法,矩阵)