KinectFusion算法论文解读/代码解读随笔

——2021.6.8

上图是kinectfusion的流程图，是整个系统的工作流。我们对上面的流程进行分析以及相关代码的解读，使用的代码库是pcl的Kinectfusion复现。（ps：因为我只在ubuntu20.02上成功编译并且使用kinectv2跑通）

Surface measurement:这是对应的第一部阶段，在论文中是这样描述的：这是一个预处理的过程，将生成并输出稠密金字塔顶点图和法线金字塔图，输入的是原始的深度测量图从Kinect中获得。

但实际上面这句话有这么简单的吗？下面先上一个流程图，来自别人的分析图。

第一个阶段是双边滤波对图像进行降噪，一般是使用高斯滤波进行降噪，用这个的好处是可以进行边缘保留。上图中使用的公式是根据什么进行权重判断的呢？但是我搜索双边滤波相关的内容，法线它是根据空域核以及值域核进行相乘然后结果对原图的像素进行滤波，前者空域核是根据像素距离，越近像素的权值越大。后者值域核是根据像素的差异，依旧是周围像素和我相同的像素所占权重大。同时尽管离得很近但是如果像素颜色差异很大的时候，对应相同颜色的像素所占的比重大。如下图。首先Ν_σ(t)当t为0的时候N是最大的，其函数目的是为了根据t的大小返回权值，t越大权值越小。函数D_k(u)中第一个N是根据离中心像素的距离来返回权值，第二个N函数根据与中心像素的颜色的差值返回权值。两个权值相乘与一块区域的像素颜色相乘累加作中心u的像素颜色。

第二个阶段是根据相机的内参反投影出对应每个像素点在三维空间中具体为位置。这里不明白的可以看各种相机以及图片-SLAM14CP5 - xkyl - 博客园 (cnblogs.com)我之前写的博客。其中的相机模型就是将二维图片的点对应的空间中的三维点。因为是深度图所以我们有了每个像素的深度，这样在通过小孔成像能确定该像素可能存在的一条射线，结合深度就可以确定对应的xyz坐标值。上图公式中的D_k(k)是降噪的深度图，K是相机内参，因为是反投影所以需要右乘反矩阵(?不确定)，然后û是齐次坐标。

第三个阶段就是计算的到法线向量图，过程比较简单，通过相邻的左边得到向量然后通过叉乘得到法向量，就直接输出向量图。

第四阶段就是进行降采样，采用的均值降采样即深度图相邻的四个点深度值平均，并每隔一个取为中心即0，2，4.（ps:在数字图像处理课听了，理解了过程。）从而生成三层图像金字塔。即三张图大小依次减1/4。该步的目的是为了加速计算相机位姿。并且如上图，分别计算它们的顶点图和法线图为后面做准备（？）。

以上就是第一步的部分。代码先放一下。按照论文顺序先看一下Surface Reconstruction部分。（ps 原来想先看Pose Estimation的ICP算法，一下没看懂）

我们可以从上面的结构图出发，发现里面对应的是的输入是来自ICP算法所估计的的T_g,k相机的位姿估计以及传感器得到的R_k原始的深度图，输出通过TSDF符号表示的集成全局场景模型体素S_k，以及以上帧的相机位姿T_g,k-1。为什么是上一帧的位姿，本质上当完成将第k帧的TSDF融合之后可以说一次循环已经完成，至于下一步的surface predict本质上是为了下一帧做的预处理，所以得到的其实不是上一帧的相机位姿，而是第k帧的，然后要用于下一帧。（应该是这样理解的？）先介绍一个SDF这个函数，本质上是描述点到面的距离对应如果这个点在面上则为0，面的法线正方向为正数，返现反方向为负数。而TSDF是从这个概念中增加一个一个条件，在论文中描述正负分别从平面开始往看得见的空间方向为正，而平面往后面看不见的空间为负。我们可以先看一下下面图二的意思，对应的存在一个（-δ， δ）大小的线性范围来表示一个阈值范围，离表面的距离除以δ便是TSDF的值，从而达到归一化的目的，而TSDF，在SDF中多了一个字母T对应的意思是截断的意思，也就是当点离平面的距离的绝对值大于δ都截断为±1，所以TSDF的值域[-1,1]。（对应下面的截断符号距离，同时是全局TSDF的一部分）

TSDF有什么用呢？主要的思想是通过TSDF来对三维地图进行表示，TSDF主要思想是使用一个大的体积块（volume）来重建原来的三维图形大的体积块可以划分成很多小的体素（voxel）组成么一个体素对应一个空间点。也就是上面的图一所表示的内容。论文中所说的是体素块一一对应到连续的TSDF符号S_k(P)表示。TSDF符号分为两部分：一是对应到截断符号距离值F_k(p)；二是权值W_k(p)。公式如下：S_k(p)→ [F_k(p),W_k(p)].

稠密表面测量为重建提供了两个很重要的约束条件。第一，假设我们通过截断深度测量的不确定性，使真实值对应在±μ，然后r表示摄像机光心沿着射线到每个点的距离，当r<(λR_k(u)-μ)时为测量空闲空间的方法（λ=||K^-1û||₂）。第二，我们假设当r<(λR_k(u)+μ)时没有其他的表面信息。通过上面两个约束，我们能够确定SDF只需要表面测量的信息范围存在在|r-λR_k(u)|<μ之内。

我们要使用一个影射截断有符号函数F_{Rk（P），易于并行计算。下式中的p是一个需要重建的3D体积中全局帧的一个点（即在体积中的体素点）。}t_g,k是当前帧相机位姿。

首先我们先看第一个公式，||t_g,k-p||₂表示相机坐标点（相机位姿）与空间点之间的这个向量的欧几里得范数，即之间的距离。我们先看第三个公式，|_•_|这个是最近邻查找函数，通过这个公式而不是使用深度插值公式可以消除深度值不连续对测量值的影响。可以得到空间点p点在当前帧的最近邻点。然后是第二个公式以及在第一个公式使用λ^-1是将上面得到的距离转换成深度，与后面在初始深度图中的深度进行相减，从而得到体素距离平面的距离。最后Ψ函数即TSDF函数，将上面计算的体素到表面的距离映射到[-1,1]之间。注意：我们通过这个公式得到的是F_RK(p)与全局融合的TSDF的F_k(p)是不同的，前者是当前帧的计算值，而后面是全局的融合值，是一直更新存储在GPU中的。同时权值也是分成这两部分的。后面有对应当前帧是如何融合到全局的公式。权值的大小和cos(Θ)/R_k(p)成正相关，Θ是从摄像机到p点的射线方向与表面的法线的角度。体素越是正对着相机其权值就越大。

关于当前帧的权值W_Rk(p)通过实践简单的将其设为1对实际情况的效果是很好的。下面这两张图分别描述了公式S_k(p)→ [F_k(p),W_k(p)]，和公式5，6.

下式是如何将当前帧的截断符号距离以及权值融合到上一帧的TSDF中，同时更新为当前的TSDF。这里只对p不为null的点集进行处理。

权值的公式最后还是需要进行对应的截断防止超过W_η（不知道为什么）。

这部分就是对应的TSDF重建地图的部分Update Reconstruction。接下来是Surface Predict。首先对应输入的是当前帧输入的TSDF符号表示的集成全局场景模型体素S_k，以及以上一帧的相机位姿T_g,k-1。这里为什么是k-1呢，在上面有解释过本质上在完成上一个环节的时候第k帧的任务就完成了，就能对应显示效果了，因为已经有TSDF来对重建地图进行表示了，那么本质上这个环节是为了k+1帧做预处理的，得到的相机位姿实际上是第k帧的，但是在下一帧看来是k-1帧。然后为什么不直接使用上一帧从第一个环节surface measurement得到的法线以及顶点集，是因为：“frame-to-model tracking” intuition: TSDF model is more accurate/complete than the single measured depth map from frame k-1。

在理解这部分的比较混淆的地方之后这部分其实就很好解释整个算法的过程了。本质上的算法就是通过Ray Casting，在世界坐标系下，从像素u出发，光线沿着方向T_g,kK^-1u,直到遇到TSDF从正变成负数，认为找到了平面，把找到的顶点加入顶点集。有以下两种情况认为没有找到平面：1.TSDF从负数开始，即从back face开始。2.在沿着光线方向遍历完体素之后，还是找不到对应的平面。

这是原理上寻找intersection的方法，实践中还用到了interpolation以及增加march along the ray的步进距离（因为一次前进一个voxel的效率比较低，可以使用一个小于  的step加速寻找过程）。（引用，现在不太理解）

然后要计算法向量，通过以下公式：

由于Kinect的深度测量是有一定范围的（本文取0.4m～0.8m），所以在ray casting的时候还要加上这个限制得到的预测结果才是合理的。

通过上面这个过程我们就得到最后重要的相机位姿估计的所有所需的预处理，然后就开始pose estimate。对应的输入是第k帧从Surface measurement得到的顶点集V_k以及法线集N_k，还有从Surface Predict得到的第k-1帧的位姿估计T_g,k-1以及通过raycasting得到的顶点集V_k-1以及法线集N_k-1，最后输出当前第k帧的相机位姿T_g,k。

首先kinectfusion的匹配点采用的是frame-to-model的方式，不是frame-to-frame，是因为使用的上一帧的顶点集是通过raycasting得到的顶点集，不是原始深度图得到的顶点集，同时在论文中证明前者的效率更高。然后这个匹配筛选过程可以看下面的这个伪代码：

1).在当前帧的每一个图像u进行并行处理.2).当深度值大于0.3).对于当前帧的这个像素点对应上一帧的全局顶点v^g_i-1（通过特征点匹配）进行矩阵左乘T^-1_i-1变换到上一帧相机坐标系.4).通过透视投影到图片上得到像素p.5).当p像素在当前帧的图像范围内时。6).将当前帧的p像素这个位置的空间点v求出，通过左乘T_i-1将该像素在当前帧的坐标顶点变换到全局顶点v（？不太理解，为什么当前帧用上一帧的位姿可以得到全局顶点位置）7).得到对应的法向量n。8).当v和v^g_i-1的距离足够小，且n于n^g_i-1的角度足够小，则认为是匹配点。

定义误差函数（原文：global point-plane energy），使用  norm

 可以看作一个筛选函数，排除掉误差过于大的点，其评判依据可以参见论文中的公式(17)。接下来我们只需要对(9)式进行非线性优化（论文中使用的Gauss-Newton法），就可以得到最后的最佳pose estimation  了。非线性优化可以看非线性优化-SLAM14CP6 - xkyl - 博客园 (cnblogs.com)这篇文章。

到此分析完成kinectfusion系统，后续会通过源代码加深理解。