KinectFusion是一种利用kinect相机的深度数据进行实时三维重建的技术。本文学习一篇比较早的论文（见参考文献），并对其进行详细地讲述。这篇文章构建了KinectFusion的雏形，被后来者争相follow。

图1.Kinect相机的输入和输出结果。最左侧为带有噪声和空洞的数据，可以作为Kinect的输入，后面组是输出结果，彩色图是Normal map，灰度图是Phong色调渲染的结果

0.工作流程

KinectFusion的过程可以分为四个模块：

• 表面测量（surface measurement）
  这是一个预处理的过程，根据原始深度数据计算出稠密的顶点图（vertex map）和法向图（normal map）金字塔。
• 表面重建更新（surface reconstruction update）
  全局场景融合过程，根据由跟踪（tracking）得到的相机位姿（pose），新测量的surface measurement被融合到由TSDF表示的全局场景模型中
• 表面预测（surface prediction）
  利用光线投影（raycasting）将模型投影到估计的frame获得预测的surface
• 传感器位置估计（sensor pose estimation）
  多尺度的ICP算法匹配measured surface和predicted surface。

图2.算法流程图

1.surface measurement

首先对原始深度数据做双边滤波处理，对处理后的深度图做反投影（back-project）操作，得到vertex map。
向量之间的叉乘可以得到法向量，具体运算可以见图3。

图3.normal map的计算方法。V代表每个像素对应的三维点，v是归一化操作

2.surface reconstruction

surface construction的核心是TSDF及其更新。
TSDF全程Truncated Signed Distance Function。它将空间场景模型化为一个立方体栅格，每个栅格中都存有两个值，一个是距离值F，另一个是权重值W。
如图4所示，TSDF模型中存储的距离值，surface处值为零，传感器一侧的值为正，距离表面越远值越大，另一侧值为负，距离表面越远值越小。而权重值则与表面测量的不确定度有关。

图4.TSDF模型

而在实际的测量中，接近零的位置的值才是有效的，因此需要设定一个阈值u，将与surface距离大于u置为无效，不予考虑，而小于u的值进行归一化截断。
图5给出了当前帧TSDF距离值的计算方法。

图5.TSDF距离值F的计算公式

全局的TSDF是由每一帧单个的TSDF加权平均得来的，如图6所示。

图6.全局TSDF

虽然理论上权重值和表面测量的不确定度有关，即与顶点光线方向和法线方向夹角的余弦值成正比，与相机中心和顶点的距离值成反比。但是在实际中，权重恒等于1可以得到很好的结果，并且权重的累积不是一直增大，而是设置了最大值阈值，如图7所示。

图7.设置最大阈值

3.Surface prediction

上文提到过，在TSDF模型中距离值F=0的位置就是surface的位置，因此利用ray casting，从相机中出发，逆着光线走，直到TSDF体元中的距离值由正变负或者由负变正，即确定为surface的位置。
但是有一个问题，在逆着光线方向走的时候，以多长距离为间隔呢？这里由于场景中很大一部分都是没有物体的无效位置，每个体元都去判断正负值显然很费时间。这里就根据TSDF每个体元中存储的距离值来确定步长，当走到TSDF体元存储着正距离值的时候，步长要设定为小于u值，而在此之前，就以u值作为步长，这样就可以快速找到正负变化的位置。
好像还是哪里有问题。对，虽然正负变化的体元位置找到了，但是哪里才是表面确定位置，毕竟表面是一个值而不是一个有厚度的区域。所以我们采用图8的式子近似找到一个合理的位置。

图8.更精确的表面位置.其中t表示时间，即光线上一点的坐标，F表示在光线上对对应位置进行三线性插值后得到的值

找到surface的坐标后，预测的法向量可以由这一点的梯度得到，如图9，并在不同尺度上计算以适应不同的体元精度。

图9.计算predicted normal map.png

4.pose estimation

相机的pose通过ICP算法求解。和一般的ICP不同，这里的ICP不是通过两帧之间的对应点进行求解，而是将当前帧的点与上一帧得到的predicted surface点进行匹配。在建立measured surface和predicted surface之间点的对应关系时，作者没用使用过ICP中的最近点，而是将measured surface上的点投影到上一帧相机的位置，得到的图像平面上的坐标对应的predicted surface上的点就是要找的对应点。如图10所示。

图10.建立点对应的示意图。黄线代表measured surface，绿线代表predicted surface

在度量measured surface和predicted surface之间的误差时，作者使用的是ICP中的点面误差度量，如图11。

图11.误差最小化求得相机位姿。该式其实一个点到平面的距离平方和公式

以上为算法全部内容。
持续更新，欢迎提出质疑或与作者就相关问题进行讨论。

*参考文献
[1]KinectFusion: Real-Time Dense Surface Mapping and Tracking
[2]A Volumetric Method for Building Complex Models from Range Images