Kinect-Fusion解读

原文链接：KinectFusion: Real-time dense surface mapping and tracking | IEEE Conference Publication | IEEE Xplore

本文若有错误请批评指正

1.OverView

先定性的了解一下大致框架，首先我们的输入是来自RGB-D图像含深度信息Rk（k表示第k帧），然后：

1.Measurement：计算Vk（像素点对应相机坐标系下的坐标）、Nk（像素点对应相机坐标系下的其表面点的法向量）-用这两个玩意来表示表面信息

2.Pose Estimation：利用上一帧的Vk-1、Nk-1、相机位姿Tg,k-1和当前帧进行过Measurement的输出Vk、Nk通过ICP（一种点到面的ICP：point-to-plate）计算本帧对应的相机位姿Tg,k

3.Update Reconstruction：利用本帧的深度信息Rk和本帧的位姿Tg,k更新我们的空间立体块模型Sk（利用一种TSDF的玩意表示-更新我们的空间立体模型就是计算这个立体块中的每个体素的TSDF值；我们最终需要输出展示的表面重构就是从这个立体快中提取的-见下一步Surface Prediction）

4.Suface Prediction：利用本帧的TSDF模型Sk（就是上一步的空间立体快模型）和本帧的相机位姿取提取立体快模型中的表面-Vk、Nk

注1：这里的Tg,k-1和Update Reconstruction的输入Tg,k是相同的，在Update Reconstruction这一步没有改变相机位姿

注2：那为什么这一步中输入的位姿是Tg,k-1不是Tg,k输出不是Vk、Nk而是Vk-1、Nk-1？我们要用迭代更新的思维去理解它，因为我们这一步的输出要做为前一帧的信息传到第二步Pose Estimation 做相邻两帧之间的ICP计算，所以我们有意这么写是为了Pose Estimation服务的

2.Measurement

2.1 对输入数据（原始深度图和彩色图）进行三层金字塔分级

最底层是原始深度/彩色图像，上层是通过下层重采样生成，分辨率降低一半，我们将在金字塔每一层面上进行求解Vertexs信息（Vk）、Normals信息（Nk）

2.2 双边滤波（Bilateral filtering）

何谓双边滤波，在这之前我们需先了解数字图像处理的各种滤波器例如高斯滤波器、平均值滤波器等等，这些滤波器在滤除噪声点的同时也会模糊一些图像中的边缘特征（例如像素值发生剧变的一些地方），而双边滤波则会在滤除噪声点的同时会保留图像中的边缘特征（如下图所示），我们会在金字塔的每层都进行双边滤波操作。

双边滤波公式给出：

 看不懂吧，我也看不懂，看懂的教教我。

2.3在金字塔每一层求解Vertexs、Normals

这步我们在Overview中说到过怎么计算，直接给公式，简单的一...Vk(u)表示Vertex信息，Nk(u)表示Normal信息

 注：Vk(u)的计算中，表示金字塔每一层中的每个像素（齐次坐标形式），K表示相机内参矩阵，Dk(u)表示该像素对应的RGB-D深度值（不懂建议看十四讲相机成像章节）。Nk(u)的计算中用到的是我们中学用到的原理-叉积，即在相机坐标系下的表面某一点对应的法向量等于该点的两个相邻像素点与该点连接的向量叉乘一下，根据右手原则得到以该点出发的垂直法向量（垂直于该点周围领域的表面），我们这里还做了单位化操作（变成单位法向量）

3.Pose Estimation

 3.1 投影数据关联-求匹配点

利用算法projective data association对前一帧和当前帧的（Vertex、Normal）进行匹配，算法如下：

1. 在当前帧 i 的深度图像上的每一个像素 u 并行计算：

2. 对于深度值大于0的像素：

3. 求该像素点对应的顶点在上一帧所处的相机坐标系下的位置：方法是用上一帧的相机位姿Tg,k-1的逆矩阵左乘上一帧中该点对应的像素点的全局坐标(该值如何求得可见下文的Update Reconstruction) （注：上一帧中该点对应的像素点是怎么得到的我也有疑问，我估摸着时进行两帧图像特征匹配得到的）

 4. 将 投影到像素平面得到p

5. 对于p属于当前帧范围内的（说明该顶点在上一帧中也在相机视口范围内）：

6. 用上一帧的位姿Tg,k-1左乘该点 Vi(p)(注：这是本帧中像素p对应的Vertex) 将其投影到全局坐标中得 v

7. 同上得全局坐标下法向量 n

8. 如果 v 和  的距离小于阈值，且 n 和  的夹角小于阈值，则

9.找到匹配点对

3.2 Point-to-Plane ICP-求本帧的相机位姿Tg,k

 Point-to-Plane ICP本质上就是最小化这个误差项，怎么理解Point-to-Plane ICP就是要怎么理解这个误差项，首先误差项中的是我们上述数据关联中求的匹配点对相连接构成的向量（例如上图中d1与s1连接所构成的向量，d2与s2连接所构成的向量）（注：这是在世界坐标系下的向量，因为将本帧中的Vertex乘以了相机位姿即变换到了世界坐标系下，是上一帧像素u对应的世界坐标），其次，括号外乘了一个，这也就是他为什么是Point-to-plane ICP而不是我们普通的ICP的原因，乘以它的目的就是将我们刚才的向量转化到点到面的距离（例如上图中的l1、l2、l3），这里不懂的可以回顾向量乘积。

 那怎么最小化这个误差值的到我们的最优Tg,k呢？我们用到了非线性优化里的LU方程求解，在这里不展开了，感兴趣的可自行学习。

 注：在kinect-fusion中这一步可以引申出来一个判断外点的小方法，在我们的kinect-fusion中通常外点就是场景中的非静态的移动物体对应的点，具体在算法中怎么体现呢？

我们上面计算过了每个匹配点对，然后对所以匹配点对求解Point-to-Plane距离，我们有一个设定的阈值，如果大于这个阈值，就被认定为外点（粗糙的理解）

 4.Update Reconstruction

 4.1 求解空间立体块中每个体素的TSDF值

 1.计算体素的世界坐标：设体素x中心在立体块中的坐标(vx,vy,vz)，那么在世界坐标系下的位置是：

2.将体素x的世界坐标转化到相机坐标系下：

 即用该帧的相机位姿的逆矩阵左乘世界坐标上一步Pose Estimation得到对应的像素坐标：

3.计算体素x的sdf值，用像素坐标的深度值 D(Ix) 减去体素中心和相机中心的在世界坐标系下的距离t(x)：

注：同样的，Ix表示体素x投影在相机成像平面下的像素坐标,乘以，是将沿着射线的距离转化到沿着光轴的距离，D(Ix)不乘这个系数是因为它本身就是像素点对应的沿着光轴的深度值

4.体素x的tsdf值为：

注：当− t ≤ sdf ( x ) ≤ t时，tsdf ( x ) = sdf ( x ) / t ∈ [ − 1 , 1 ]。当sdf ( x ) > t或者sdf ( x ) < − t时，tsdf ( x ) = 1或者− 1 。其实这个表达式是有物理意义的。t可以看作是体素x和截面对应点P深度差值的阈值。当体素x距离截面对应点P非常远的时候，它的tsdf值等于正负一。当体素x距离截面对应点P比较近的时候，它的tsdf值[ − 1 , 1 ]之间，是有意义的。用更为通俗的话说，当体素离表面非常近的时候，它的tsdf值接近于零；当体素离表面非常远的时候，它的tsdf值趋于正一或者负一

求解TSDF的伪码给出：

4.2 更新体素值        

我们上一步求出来的体素x的TSDF值不是直接赋值给对应体素就行了，而是要利用权值去修饰它，因为这里面涵盖了“批量”的思想（并不是基于马尔可夫性-本帧状态只与前一帧有关，而是与历史所有帧有关），具体如下：

一开始时W(0)赋值为全0张量，每个时刻更新体素x的tsdf值时都会用一个对应于该时刻的临时权值w(t)赋值为全1张量，然后将本帧的权值W(t)赋值为W(t-1)+w(t),体素x的tsdf值计算只用到了W(t-1)和w(t)，W(t)是为下一时刻服务的。

 那为什么W(0)赋值为全0张量，w(t)赋值为全1张量呢？我们对公式进行简单手动递推不难发现，W(t-2) -> W(t-1) -> W(t) -> ....是不断变大的，而w(t)永远是1，这是不是就和我们前面所说的“批量”的思想对应上了，它强调历史对现在的重要性，淡化本时刻的重要性，可以好好体会一下。

5.Surface Prediction

 这一步就是从我们立体块中提取我们的表面重构，将其传回Pose Estimation进行下一次的优化，但经历了n轮后，算法结束，就不再传回Pose Estimation，并对（Vertexs,Normals）进行着色转化成mesh输出我们的重建结果

5.1 对每个像素进行反投影（Raycasting算法）

1. 对于要输出的图像每个像素点 u 并行：

2. 该像素起始深度[u, 0]处转换到体积空间（世界坐标系）中记作 raystart

3. 下一个步长深度[u, 1]转换到体积空间中记作 raynext

4. 相减再normalize得到方向向量raydir，通常即是视线方向

5. 沿视线方向寻找视线与体素块的开始与结束交点

6. 沿着 raydir 方向，在开始与结束交点内，按照一定步长遍历体素 g

7.累计步长，存储前一个体素g为gprev

8. 如果 g 和 gprev 的 tsdf (由上一小节中求得)符号不同，即两体素之间存在物体表面点，则：

9. 三线性插值方法得该表面点在世界坐标系中的值 p

 上图是双线性插值方法，三线性插值就是从平面类推到空间（注：t是坐标，F是TSDF值）

10.Tg,k左乘p即得到该表面点的Vertex：Vk

11.tsdf(p) 的梯度，即为该表面点的法向量Normal：Nk

6.完结撒花