KinectFusion公式推导、理解

  “KinectFusion Real-Time Dense Surface Mapping and Tracking”一文于2012年发表，该文章首次实现了实时稠密重建（Real time dense restruction），我认为微软的Kinect深度相机是其成功的根本，该文章也是首次成功应用了深度相机，是做实时稠密重建（Real time dense restruction）方向的必读论文。

  系统的主体结构如图所示：（摘自原文）

1. 深度相机采集数据，对数据做预处理。（Surface measurement）
2. 使用Prj ICP算法估计相机位姿。（Sensor pose estimation）
3. 融合当前帧点云到模型。（Surface reconstruction update）
4. 使用光线投射法（投影算法）将点云模型投影到下一帧相机位置的图像上供下一帧计算。（Surface prediction）

深度图像预处理（Surface measurement）

  记从传感器获得的第 $k$ 帧深度图像为 $R_{k}$ ，记 $\textbf{u} = \begin{bmatrix} u & v \end{bmatrix}^T$为深度图某个像素的像素坐标。那么 $R_{k}(\textbf{u})$ 为第 $k$ 帧深度图的第 $(u,v)$ 像素的值，即深度。对 $R_{k}$ 进行双边滤波得到 $D_{k}$ 。

  记 $K$ 为相机的内参矩阵（Intrinsic matrix）， $\dot{\textbf{u}} = \begin{bmatrix} \textbf{u}^T & 1 \end{bmatrix}^T$ 为 $\textbf{u}$ 的齐次化向量。通过反投影法由 $D_{k}$ 计算得到该帧的三维点云集合 $V_{k}$ （其中每个点 $V_{k}(\textbf{u}) \in R^3$ ）：

$$V_{k}(\textbf{u}) = R_{k}K^{-1}\dot{\textbf{u}}$$

*注1：双边滤波器可以参考双边滤波器的原理及实现一文。其作用是保留边界梯度的情况下，滤除高频噪声。

  然后通过 $V_{k}(\textbf{u})$ 计算对应像素点 $\textbf{u}$ 的法向量 $N_{k}(\textbf{u})$ :

$$N_{k}(\textbf{u}) = \nu [(V_{k}(u,v+1) - V_{k}(u,v)) \times (V_{k}(u+1,v) - V_{k}(u,v))]$$   其中 $\nu[x]$ 为向量单位化函数：
$$\nu(\textbf{x}) = \frac{\textbf{x}}{||\textbf{x}||_2}$$

  由于深度图像并不是每个像素点都是有值的，如下图，深度图像中纯黑色的部分就是传感器无法采集深度的区域，原因可能是多种的：探测区域超出了传感器的探测范围、过于光滑的材质、能够吸收红外光的材质等。

  因此，需要一个掩码矩阵$M_{k}$来表示该帧深度图像对应的位置是否有值。若对应像素 $\textbf{u}$ 有值，则 $M_{k}(\textbf{u}) = 1$ ，否则 $M_{k}(\textbf{u}) = 0$。

  至此，得到了 $R_{k}$ 与 $N_{k}$ 以供后面的投影法 ICP使用。

相机位姿估计（Sensor pose estimation）

  定义相机坐标系，取相机光心为原点，取相机光轴为 $Z$ 轴，像素坐标轴 $u$ 与相机坐标轴 $X$ 同向。坐标系符合右手系法则。

  TSDF(truncated signed distance function)，一般译作“截断符号距离描述函数”，可以把这个词拆开看：

• 一张深度图像反投影变换到三维空间中的每个三维点都对应着原深度图像中的每一个像素点，当点云比较稠密时，我们可以将该点云看成一个三维曲面，也就是function，记作$F(x,y,z)=0$。
• 所谓signed distance就是带正负符号的距离，以相机的光轴为Z轴，对于一个三维点$(x_{0},y_{0},z_{0})$，记$d=F(x_{0},y_{0},z_{0})$，如果它在平面前面，则$d$的值为正，否则为负，（这是《高等数学》空间几何代数部分的基础内容）。
• 而“truncated”就是截断，当$|d| > t$时，则$|d| = 0$，其中$t$是截断距离。

  ICP算法（Iterate Closest Points）可以用于计算两组点云之间的刚体变换，对于 ICP算法有多种变种，如点到点 ICP（Point-to-Point）、点到面 ICP（Point-to-Plane）、面到面 ICP（Plane-to-Plane）等。

  传统的 ICP（Point-to-Point）是需要知道两组点云之间，一组点云的各个点在另一组点云中的最近点，才能进行计算。显然，这需要遍历另一组点云以寻找最近点，最差会需要 $O(N^{2})$ 的时间复杂度，即使使用一些数据结构来降低复杂度，（如KD-Tree、Octave-Tree等）也至少需要 $O(Nlog(N))$的复杂度来搜索最近点，这直接导致我们的算法无法实时运行。

  为了抛掉“各个点的对应关系”这一搜索步骤，变种的 ICP都假设两组点之间的旋转、平移量很小，所以一个点附近的点就是它的当前最近点。
  左图中是点到面 ICP，它并不需要去寻找最近的点是哪一个，这个算法所需要的只有两组点云的坐标和当前帧的法向量，也就是 $R_{k}$、$N_{k}$，通过 $R_{k}$ 可以计算得到当前帧的点云 $V_{k}$，已知在此之前融合好的模型点云 $\hat{V}_{k}^{g}$ 。

（图片摘自“KinectFusion 解析”）

  GPU的三维数组中的每一个元素代表一个体素（Voxel），其存储在体素空间内的顶点的位置。
  例如：用 $1000 \times1000 \times1000$大小的三维数组表示 $1m^{3}$空间内的三维结构。每个元素就是一个体素（Voxel），这个元素对应的数组下标就表示体素在这$1m^{3}$空间内的对应位置的$1cm^{3}$空间，这个元素所存储的值，就是处于这$1cm^{3}$空间中的顶点在这$1m^{3}$内的精确位置。

  通过使用TSDF的描述方法，以上图形式将 $\hat{V}_{k}^{g}$ 作为距离描述函数曲面直接放在GPU的三维数组中，即GPU的各个线程对这个三维数组赋值，把对应位置的元素赋值为“0”，离相机近的一侧为正，另一侧为负。然后每个线程负责读取一个$V_{k}(\textbf{u})$在这个三维数组中对应位置的值，就得到了当前 Vk(u)<