Kinect-Fusion ICP算法构建带法向量点云金字塔

主要步骤：

1、构建带法向量的点云金字塔，ICP运行时先对金字塔高层的点云进行匹配，再往下层走，即从粗匹配到精匹配的过程，加快收敛速度；

2、找两点云的匹配点；

3、构造目标函数，并且最小化目标函数获得转换矩阵；

4、判断目标函数的大小是否小于所设定的阈值，或者迭代次数是否超过某个设定的最大值，如果前面的判断是F，则返回到第二步，直至前面的判断是T；

第一步：

金字塔第一层：

使用双边滤波器对原始的深度图进行滤波，再通过以下两个公式进行计算出每个像素对应的相机坐标系下的坐标点和法向量；

 u为像素点坐标，D为深度图，K为相机的内部参数可以通过张正友校正法得到；

v[]为归一化函数；

金字塔第n层：

先对n-1层的深度图进行均值滤波，然后再进行下采样获得第n层的深度图，也可以使用opencv中的pyrDown函数（这个是经过高斯滤波之后进行下采样）。