Kintinuous 算法精讲

理解本文需要有KinectFusion的基础，建议先阅读我的文章“KinectFusion 论文精析”对KinectFusion的讲解。

---

Kintinuous利用消费级RGB-D相机，实现了实时SLAM系统，该SLAM系统可以运用在几百米大的场景中，并且得到高质量的全局一致性表面重建。

---

Kintinuous的工作主要有以下三个亮点：
* 1.利用基于GPU的3D循环缓存技巧将深度图（depth map）的fusion扩展到无界空间区域（unbounded spatial region）；
* 2.同时利用稠密的几何（geometric）相机位姿约束和光度（photometric）相机位姿约束，克服了大场景下相机位姿估计不准确的限制；
* 3.根据场景识别（place recognition）和随后的闭环限制（loop closure constraint），使用尽可能刚性（但其实是非刚性）的空间变形（space deformation）来更新建好的地图。

下面，我们通过扩展尺度的场景融合、相机位姿估计、闭环检测三个部分来介绍Kintinuous算法：

---

一、扩展尺度的场景融合

1.场景表示

Kintinuous算法依然沿用TSDF建模空间场景，这在我之前介绍KinectFusion的文章[2]中详细说明过。相比于之前KinectFusion，这里的TSDF存储的内容稍有一些区别，每个体元中不光存储了两个字节的有符号距离值和一个字节的无符号权重，还包括了各一个字节的RGB三个通道的分量值，也就是说一个体元中存储了6个字节的数据。

2.体积移位（volume shifting）

KinectFusion只支持初始TSDF提供的固定大小的场景，Kintinuous将TSDF结构视为一个循环缓存，通过它的移位来表示相机在空间场景中的移动，从而扩展了重建区域。如图1所示。

TSDF移位的同时，相机的位姿也要更新，更新方式如图2公式，其中式1表示相机平移换算成的体元数，由于体元数是整数，所以该式做了取整处理；式2表示TSDF移位后相机的平移，由于式1做了取整处理，所以此时的相机平移并不等于零而接近于零；式3表示新的TSDF所处的位置。

图3用二维形式演示了相机移动和volume shifting的过程。第0帧到第1帧相机的移动小于阈值m，TSDF没有发生移动，第1帧到第2帧相机的移动大于阈值m，TSDF移位到g2位置。

2.1算法实现方法

在算法的执行流程中，有两个过程需要访问TSDF中的体元，一个是体融合（volume integration）过程，也就是图1中将蓝色部分加入的到TSDF中，另一个是体光线投影（volume raycast）过程，也就是图2中红色部分生成surface的过程。
TSDF是三维的，它以一维的 形式存储在GPU的内存中，通过内存访问TSDF体元的方式如图4公式所示：

TSDF体移位反应在内存地址上的改变就如图5所示。

2.2surface提取

移出TSDF的红色部分需要生成surface，这里的surface并不需要以哪一帧相机的位姿为参考，而是直接以TSDF正交的三个轴向做光线穿过点云，用Raycasting方法生成surface，为了抑制噪声的影响，这里只提取有最小权重值的点。但是这样生成的surface可能会造成TSDF同一个体元生成两个或三个surface点，我们需要个一个网格滤波，即在提取出来的surface点上覆盖一个与TSDF体元大小相同的网格，每个网格生成一个点，用这个点代表落在这个体元中所有点的中心。
每次提取出来的surface都称为一个cloud slice，使用GPT算法增量式地把cloud slice生成mesh。每个cloud slice被提取时刻的相机位姿被称为与之相联系的相机位姿，如图6所示。

无论是否与cloud slice相关联，所有的相机位姿增量式的联系起来就构成了位姿图（pose graph），图7表示的完整的TSDF、cloud slice和pose graph（只画出了与cloud slice相关联的pose）。

3.dynamic cube positioning

按照前文所述，将相机置于TSDF中心，那么相机只能覆盖TSDF的一小部分体积，大部分区域都浪费了。根据相机的朝向动态的改变相机在TSDF中的位置则可以改善这一点。如图8所示。

TSDF中心位置相对于相机的计算方式可以图9公式就算：

4.颜色估计

在估计surface的同时要估计surface的颜色，但仅仅是为了显示。颜色值融合到TSDF中和深度值相似，也是利用值截断和加权，但这里得到的颜色值不会参与相机位姿的估计。
通常在物体的边缘点，颜色估计是不准确的。边缘点在深度值上是不连续的，作者使用一个7x7的窗口，如果邻域中的深度与其当前像素深度值的差大于一定阈值就认为这一点位于边缘点。
作者使用surface法向与相机像平面之间的夹角为颜色值加权，夹角越大，即surface与像平面越平行，权重值也就越大。如图10所示。

颜色值估计的算法流程如图11所示。

二、相机位姿估计

单纯的ICP算法对于类似走廊或者三维特征的场景会失效，因此Kintinuous算法使用ICP算法和直接法相结合的方法进行相机位姿估计。

1.ICP算法估计相机位姿

ICP算法估计相机位姿可以参考我以前的文章“Point-to-Plane ICP算法的线性近似求解”对相机位姿求解的方法，Kintinuous使用的ICP和这个原理上是相同的，这里就不在赘述了。

2.直接法估计相机位姿

直接法通过最小化前后两帧光度误差的方法来估计相机位姿。
直接法估计估计相机位姿的详细介绍可以参考我的另一篇文章“直接法估计相机位姿”。

3.直接法和间接法相结合

三、闭环

后续再更新

---

作者正在努力完成中……

---

持续更新，欢迎提出质疑或与作者就相关问题进行讨论。

---

*参考文献
[1]Real-time large scale dense RGB-D SLAM with volumetric fusion
[2]KinectFusion 论文精析