Kinect-Fusion ICP算法寻找匹配点

Kinect-Fusion ICP算法 

特点及约束条件：

一、KinectFusion算法采用frame-to-model，相对于frame-to-frame更加精确；

二、通过第一步构建出带法向量点云的金字塔，从最高层开始匹配到最底层，加快收敛速度；

三、要求前后相邻两帧之间位姿变化小；

四、当环境主要由平行平面构成时，ICP算法定位失效；

计算匹配点：

第一步，通过光线投影算法，从模型中计算出前一位姿应该获得的点云；

第二步，将当前帧的点云转换到全局坐标系下，再转换到上一帧的相机坐标系下，将其投影到图像坐标系，计算得到对应前一帧的点；

第三步，将上述的匹配点进行距离和法向量的对比，看是否是匹配点；