kinectfusion中使用的位姿估计方式是点到面的ICP。这个方法的选择主要是考虑到相机快速运动,相邻两帧之间位姿变化微小。该ICP方法的优势是可以快速收敛至最优解。
我们先来回顾一下ICP的通用流程。
1. 输入源点云与目标点云,源点云为需要进行变换的点云,目标点云为固定点云,也是源点云经变换后能与之重合的点云。
2. 确定源点云与目标点云之间的点云匹配关系。
3. 根据上一步骤确定的匹配点云,计算旋转平移变换矩阵。
4. 将上一步骤计算的变换矩阵,作用于源点云,得到新的源点云。
5. 计算新的源点云与目标点云之间的重合误差,若小于设定的阈值,那么就结束;否则重复2-4步骤。
不同的ICP方法,主要是在计算变换矩阵这一步骤有所不同。
点到面的ICP方法介绍
接下来,针对本文中使用的icp方法,涉及到的具体步骤详细展开介绍一下。
1.源点云为当前相机拍摄到的数据帧,记作,目标点云为上一帧数据,记作
。方法中为了提高准确率,目标点云不是直接用的上一帧数据,而是根据上一帧的位姿,生成的新的数据帧,生成方法,我们会在后面展开介绍,此处以上一帧数据代替,进行讲解。
2. 点云匹配关系的确定,本文采用的是投影法
假设是
中的任一点,
对应的位姿为
;
是
中的任一点,
对应的位姿为
,其中,
的初始值与
相同,后面会随着ICP迭代更新。
——————————————————————————————————————————
对于中的每一个点
作以下操作
如果是个有效的三维坐标
注:将源点云经变换矩阵,变换到世界坐标系
注:世界坐标下的源点云经逆变换矩阵,变换到
所在坐标系
注:k-1坐标系中的源点云坐标,经投影矩阵,到k-1像素坐标
如果是有效的
距离阈值
法向量夹角阈值
那么该匹配关系成立
——————————————————————————————————————————
3. 根据上一步骤得到的点匹配关系,最小化误差函数,计算变换矩阵。
基于点到平面的ICP的误差函数为
由于本文最初假设了相机运行速度快,并且旋转角度非常小,趋近于0,那么
,
,
,
假设绕x,y,z轴旋转的角度分别是,
,
,并且取值非常小,趋近于0,那么旋转矩阵可以近似为
可以重写误差函数
这样非线性的误差函数,近似为线性,每一步的最优解x为:
那么,
,
转为旋转矩阵与平移向量为
经过多次迭代后,求得最优解和
。