基于RGB-D相机的三维重建算法学习笔记（一）——KinectFusion方法解读

声明

本人书写本系列博客目的是为了记录我学习三维重建领域相关知识的过程和心得，不涉及任何商业意图，欢迎互相交流，批评指正。

概念介绍

1. 深度相机（RGB-D Camera）：可以直接获取被测对象的三维信息（深度图）的相机；
2. 深度图（Depth Texture）：图像中每一个像素值表示场景中某点与摄像机的距离；
3. 两种计算物体位置和深度信息的方法：结构光&ToF（1）结构光：用投影仪投射特定的光信息到物体表面由摄像头采集，根据物体造成的光信号的变化来计算深度，常用于解锁以及安全支付；（2）ToF（Time of Flight，飞行时间）：传感器发出经调制的近红外光遇物体后反射，通过计算光线发射和反射时间差或相位差来换算深度，常用于智能机后置摄影具体介绍；

KinectFusion

流程概述

1. 表面测量（Surface measurement）：预处理阶段，将从Kinect设备捕捉的原始深度图转化为密集点云图（dense vertex map）和法向量图金字塔（normal map pyramid）；
2. 表面重建更新（Surface reconstruction update）：全局场景融合阶段，给定姿态（pose）——通过追踪来自新传感器帧的深度数据确定，场景中物体表面的量化通过一种基于体素的，截断符号距离函数（a volumetric, truncated signed distance function ，TSDF）来表示；
3. 表面预测（Surface prediction）：将SDF Raycast到估计的frame上面，用来提供密集表面预测，根据该预测来align实时深度图；
4. 传感器位姿估计（Sensor pose estimation）：运用一种介于预测表面（predicted surface）和当前传感器测量（current sensor measurement）之间的多尺度ICP配准方法，来实现实时传感器追踪（Live sensor tracking）；

准备工作

1. $T_{g,k}=\left[\begin{array}{cc}{R}_{g,k}& t_{g,k}\\ 0& 1\end{array}\right]$：相机坐标系到global frame的变换矩阵，时间k的相机坐标系下的点$p_k$映射到global坐标系g下的变换为：$p_g=T_{g,k}{p}_k$；
2. $K$：恒定相机标定矩阵（相机内部参数），将传感器平面上的点转换到图像像素；
3. 用$q=\pi (p)$函数对$p\in (x,y,z{)}^T$进行透视投影（perspective projection），得到投影点$q=(x/z,y/z{)}^T$；
4. $\dot{u}:=(u^T|1{)}^T$：使用点表示法（dot notation）来表示齐次向量；
5. $\lambda =||K^{-1}\dot{u}|{|}_2$：后面会用到的参数；

Surface Measurement

在时间k时，获取到原始深度图像$R_k$，其对应的图像像素$u=(u,v{)}^T$对应的校准深度测量（calibrated depth measurement）为$R_k(u)$，故像素u对应的三维坐标为$p_k=R_k(u)K^{-1}\dot{u}$，并且采用双边滤波对原始深度图进行降噪，使得$R_k$降噪为$D_k$；
所以首先得到像素点到相机坐标系下的点云转换关系为：$V_k(u)=D_k(u)K^{-1}\dot{u}$；
接下来利用临近点云向量之间叉乘得到对应的单位法向量表示：$$N_k(u)=v\left[(V_k(u+1,v)-V_k(u,v))\times (V_k(u,v+1)-V_k(u,v))\right]$$
紧接着生成了一个以点云和法向量图金字塔的形式表示的3层多尺度表面测量（an L = 3 level multi-scale representation of the surface measurement in the form of a vertex and normal map pyramid），具体原理就是：底层为原始双边滤波深度图，下一层经过分块平均从上一层计算，然后下采样到分辨率的一半，并通过限定采样范围来保证平滑不会发生在深度边界上；
最后对每层深度图进行像素到三维映射，然后将相机坐标系映射到global坐标系，得到密集点云图和法向量图金字塔：$$V_k^g(u)=T_{g,k}{\dot{V}}_k(u)$$$$N_k^g(u)=R_{g,k}{N}_k(u)$$

Mapping as Surface Reconstruction

截断符号距离函数（TSDF，truncated signed distance function）：基于体素的，SDF描述的是点到面的距离，在面上为0，在面的一边为正，另一边为负。TSDF(Truncated SDF)是只考虑面的邻域内的SDF值，邻域的最大值是max truncation的话，则实际距离会除以max truncation这个值，达到归一化的目的，所以TSDF的值在-1到+1之间；每个点的TSDF表示$S_k(p)$有两个分量：$F_k(p)$——该体素到最近表面（zero crossing）的距离，$W_k(p)$——权重；
上一步骤进行的表面测量为物体表面的重建提供了两个重要的约束条件：

1. 首先，假设相机捕捉的原始深度信息存在$\pm \sigma$的不确定度，那么对于沿每个深度图射线从相机中心的距离r满足$r<(\lambda R_k(u)-\sigma )$；
2. 其次，假设沿摄像机光线在$r>(\lambda R_k(u)+\sigma )$处的体素中没有获得表面信息；

所以在相机光线方向上满足$|r-\lambda R_k(u)|\le \sigma$的r都是可能的表面测量，而在表面前$\sigma$外的体素一律截断为$\sigma$，在表面后$\sigma$外的体素一律记为NULL；
所以，对于一个原始深度像素信息$R_k$，并给定了变换矩阵$T_{g,k}$，那么其global坐标系下的TSDF表示为：$$\begin{gathered} F_{R_k}(p)=\Psi ({\lambda }^{-1}||t_{g,k}-p|{|}_2-R_k(x)), \\ \lambda =||K^{-1}\dot{x}|{|}_2, \\ x=\lfloor \pi (K{T}_{g,k}^{-1}p)\rfloor , \\ \Psi (\eta )=\{\begin{array}{ll}min(1,\frac{\eta }{\mu }sgn(\eta ))& \text{iff }\eta \ge -\mu \\ null& \text{otherwise}\end{array} \end{gathered}$$
而对于时间k时的TSDF，将其与global的TSDF进行融合：$$\begin{gathered} F_k(p)=\frac{W_{k-1}(p)F_{k-1}(p)+W_{R_k}(p)F_{R_k}(p)}{W_{k-1}(p)+W_{R_k}(p)} \\ {W}_k(p)=W_{k-1}(p)+W_{R_k}(p) \end{gathered}$$
可以结合此图进行理解：

Surface Prediction from Ray Casting the TSDF

由于上一步已经计算了表面的TSDF表示，假设一个虚拟的相机与上一k帧拥有相同的位姿pose，就能够运用Raycast（光线投影）的方法进行表面预测（surface prediction）；该表面的预测结果被存储为第k帧中的顶点和法线图$V_k$和$N_k$，并用于后续的相机位姿估计步骤，即计算下一帧的$T_{g,k}$。
Ray Casting方法：在global坐标系下，每个像素u对应的射线$T_{g,k}{K}^{-1}\dot{u}$，从该像素的最小深度开始行进，并在发现表面的zero crossing时停止行进，则被认为是找到了表面，将该顶点加入$V_k$；并且在表面附近，利用梯度计算代表法向量——$R_{g,k}{\hat{N}}_k(u)={\hat{N}}_k^g(u)=v[\nabla F(p)]$，并加入$N_k$中；
几点细节：

1. 如果遇到背面（TSDF从负变正），或者最终退出working frame时，会被认为未找到表面；
2. 利用小于$\sigma$的步长行进代替逐个像素点行进，加快速度；
3. 在casting的过程中采用（trilinearly interpolated）差值方法提高表面预测的准确度；

对每一个像素进行Ray casting处理，完成表面预测任务：

Sensor Pose Estimation

至此，我们拥有从表面测量（surface measurement）中得到的当前帧的$V_k$和$N_k$，以及利用上一帧表面预测（surface prediction）所得到的$V_{k-1}$和$N_{k-1}$，使用ICP算法利用二者来实现帧到帧之间的跟踪，即计算当前的相机位姿估计$T_{g,k}$；
$$E(T_{g,k})=\sum _{{\Omega }_k(u)\ne null}||(T_{g,k}\dot{V_k(u)}-{\hat{V}}_{k-1}^g(\hat{u}){)}^T{\hat{N}}_{k-1}^g(\hat{u})|{|}_2$$
上式则是定义的用来计算$T_{g,k}$的误差函数，利用非线性优化方法Gauss-Newton法，就可以得到$T_{g,k}$的最优解；
其中我们需要寻找两帧之间点的相关性，这就用到的Projective point-plane data association方法；

算法的大致思想是通过$T_{i-1}^{-1}{v}_{i-1}^g$得到相机坐标系下的上一帧顶点坐标$v_{i-1}$，然后利用相机参数投影到vertex map上在uv space下找到对应点。之后通过当前帧的变化矩阵来得到在global空间下的顶点和法向量，这样就和上一帧计算出的表面点在同一空间，即global空间内了。然后根据设定的距离阈值和法向量阈值来判断是否是相关点；

参考文献和资料

[1]: Kinectfusion: Real-time dense surface mapping and tracking
[2]: KinectFusion: Real-time 3D Reconstruction and Interaction
Using a Moving Depth Camera
[3]: A Volumetric Method for Building Complex Models fromRange Images
[4]: KinectFusion论文阅读
[5]: Kinect Fusion 算法浅析：精巧中带坑
[6]: 啥是KinectFusion