KinectFusion算法论⽂解读代码解读随笔
——2021.6.8
上图是kinectfusion的流程图，是整个系统的⼯作流。我们对上⾯的流程进⾏分析以及相关代码的解读，使⽤的代码库是pcl的Kinectfusion复现。（ps：因为我只在ubuntu20.02上成功编译并且使⽤kinectv2跑通）
out ofSurface measurement：这是对应的第⼀部阶段，在论⽂中是这样描述的：这是⼀个预处理的过程，将⽣成并输出稠密⾦字塔顶点图和法线⾦字塔图，输⼊的是原始的深度测量图从Kinect中获得。
初中英语语法口诀
但实际上⾯这句话有这么简单的吗？下⾯先上⼀个流程图，来⾃别⼈的分析图。
弗兰奇 莫尼兹
第⼀个阶段是双边滤波对图像进⾏降噪，⼀般是使⽤⾼斯滤波进⾏降噪，⽤这个的好处是可以进⾏边缘保留。上图中使⽤的公式是根据什么进⾏权重判断的呢？但是我搜索双边滤波相关的内容，法线它是根据空域核以及值域核进⾏相乘然后结果对原图的像素进⾏滤波，前者空域核是根据像素距离，越近像素的权值越⼤。后者值域核是根据像素的差异，依旧是周围像素和我相同的像素所占权重⼤。同时尽管离得很近但是如果像素颜⾊差异很⼤的时候，对应相同颜⾊的像素所占的⽐重⼤。如下图。⾸先Νσ(t)当t为0的时候N是最⼤的，其函数⽬的是为了根据t的⼤⼩返回权值，t越⼤权值越⼩。函数D k(u)中第⼀个N是根据离中⼼像素的距离来返回权值，第⼆个N函数根据与中⼼像素的颜⾊的差值返回权值。两个权值相乘与⼀块区域的像素颜⾊相乘累加作中⼼u的像素颜⾊。
第⼆个阶段是根据相机的内参反投影出对应每个像素点在三维空间中具体为位置。这⾥不明⽩的可以
看我之前写的博客。其中的相机模型就是将⼆维图⽚的点对应的空间中的三维点。因为是深度图所以我们有了每个像素的深度，这样在通过⼩孔成像能确定该像素可能存在的⼀条射线，结合深度就可以确定对应的xyz坐标值。上图公式中的D k(k)是降噪的深度图，K是相机内参，因为是反投影所以需要右乘反矩阵(?不确定)，然后û是齐次坐标。
第三个阶段就是计算的到法线向量图，过程⽐较简单，通过相邻的左边得到向量然后通过叉乘得到法向量，就直接输出向量图。
英语作文 我的老师第四阶段就是进⾏降采样，采⽤的均值降采样即深度图相邻的四个点深度值平均，并每隔⼀个取为中⼼即0，2，4.（ps：在数字图像处理课听了，理解了过程。）从⽽⽣成三层图像⾦字塔。即三张图⼤⼩依次减1/4。该步的⽬的是为了加速计算相机位姿。并且如上图，分别计算它们的顶点图和法线图为后⾯做准备（？）。厦门学校
以上就是第⼀步的部分。代码先放⼀下。按照论⽂顺序先看⼀下Surface Reconstruction部分。（ps 原来想先看Po 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û||2）。第⼆，我们假设当r<(λR k(u)+µ)时没有其他的表⾯信息。通过上⾯两个约束，我们能够确定SDF只需要表⾯测量的信息范围存在在|r-λR k(u)|<µ之内。
我们要使⽤⼀个影射截断有符号函数F Rk（P），易于并⾏计算。下式中的p是⼀个需要重建的3D体积中全局帧的⼀个点（即在体积中的体素点）。t g,k是当前帧相机位姿。
⾸先我们先看第⼀个公式，||t g,k-p||2表⽰相机坐标点（相机位姿）与空间点之间的这个向量的欧⼏⾥得范数，即之间的距离。我们先看第三个公式，|_•_|这个是最近邻查找函数，通过这个公式⽽不是使⽤深度插值公式可以消除深度值不连续对测量值的影响。可以得到空间点p点在当前帧的最近邻点。
begin的现在分词然后是第⼆个公式以及在第⼀个公式使⽤λ-1是将上⾯得到的距离转换成深度，与后⾯在初始深度图中的深度进⾏相减，从⽽得到体素距离平⾯的距离。最后Ψ函数即TSDF函数，将上⾯计算的体素到表⾯的距离映射到[-1,1]之间。注意：我们通过这个公式得到的是F RK(p)与全局融合的TSDF的F k(p)是不同的，前者是当前帧的计算值，⽽后⾯是全局的融合值，是⼀直更新存储在GPU中的。同时
权值也是分成这两部分的。后⾯有对应当前帧是如何融合到全局的公式。权值的⼤⼩和cos(Θ)/R k(p)成正相关，Θ是从摄像机到p点的射线⽅向与表⾯的法线的⾓度。体素越是正对着相机其权值就越⼤。smart的意思
关于当前帧的权值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,k K-1u，直到遇到TSDF从正变成负数，认为找到了平⾯，把找到的顶点加⼊顶点集。有以下两种情况认为没有找到平⾯：1.TSDF从负数开始，即从back face开始。2.在沿着光线⽅向遍历完体素之后，还是找不到对应的平⾯。
在线英语发音器同声传译费用这是原理上寻找interction的⽅法，实践中还⽤到了interpolation以及增加march along the ray的步进距离（因为⼀次前进⼀个voxel的效率⽐较低，可以使⽤⼀个⼩于的step加速寻找过程）。（引⽤，现在不太理解）
然后要计算法向量，通过以下公式：
由于Kinect的深度测量是有⼀定范围的（本⽂取0.4m～0.8m），所以在ray casting的时候还要加上这个限制得到的预测结果才是合理的。通过上⾯这个过程我们就得到最后重要的相机位姿估计的所有所需的预处理，然后就开始po 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-1i-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法），就可以得到最后的最佳po estimation 了。⾮线性优化可以看这篇⽂章。
到此分析完成kinectfusion系统，后续会通过源代码加深理解。
zmc
