[置顶] Kintinuous 笔记

摘要

扩展版的 KinectFusion，加入了回环检测，位姿优化和 Map 优化，适合做大场景的三维重建。

Extended Scale Volumetric Fusion

Volume Representation

同 KinectFusion 也使用 voxel 模型表示，voxel 模型参见 KinectFusion：
http://blog.csdn.net/fuxingyin/article/details/51417822

每个网格模型中存储的值：
signed distance value ( S(s)τ truncated float16)
unsigned weight value unsigned ( S(s)W unsigned int8)
A byte for each color component R, G and B ( S(s)R , S(s)G , S(s)B )
每个网格占 6字节
用 vs=512 维度在GPU中存储，消耗的显存为768M
每个维度的物理长度为 （ vd metres）表示

相机在 TSDF 坐标系下的位姿表示为 Pτi ，由旋转矩阵 Rτi 和 tτi 组成，开始时初始化为 Rτi=I , tτi=(0,0,0)τ ，
TSDF volume 在全局坐标系下坐标初始化 g0=(0,0,0)τ

Volume Shifting

循环利用显存的方式解决大场景重建 voxel 模型显存消耗的问题。

定义 ms 为 volume shift 的阈值（以 voxels 为单位），当相机在 volume 中心 gu 移动的单元格数大于 ms 时，触发一次 shift，触发是在 x , y , z 三个维度上单独计算，相机移动的单元个数为：

u=⌊vstτi+1vd⌋

⌊⌋ 表示取下整，在 shift 之后更新相机相对于 volume 的位姿：

tτ′i+1=tτi+1−vduvs

因为 volume 的中心位置变了，相机相当于 volume 的坐标和 volume 相对于 global voxel 坐标变动的方式相反。volume 在 global voxel 位置变动如下：
gi+1=gi+u

如上图虚线表示的是 global voxel 坐标，volume 相对于虚线的坐标为 gi （voxel 为单位），相机相当于 volume 的坐标为 ti （物理值）。

(1) 相机移动的距离超过了设定的阈值 ms
(2) 离开 volume 的红色区域在三个坐标轴方向上提取点，并且红色区域在显存中释放掉
(3) 提取的表面点用 GPT 算法计算 mesh
(4) 蓝色区域表示移出之后重复使用的空闲区域

Implementation

三维的 voxel 点在显存中的索引关系为：
a=(x+yvs+zv2s)

volume 移动后，TSDF integration 和 raycasting 在显存中索引的方式为：

x′=(x+gix)  mode vs
y′=(y+giy)  mode vs
z′=(x+giz)  mode vs
a′=(x′+y′vs+z′v2s)

Surface extraction

上面计算的 u 和 gi 可以用来在 TSDF 中索引 extract point 的区域，在和坐标轴相同的三个方向上提取在 volume 中提取从正到负的穿越点，既提取表面点。

每次 extract 的 point 形成一个cloud slice ，通过 incremental 的方式根据提取的 cloud slice 构建 mesh，每个 cloud slice 和提取的时候相机的位姿相对应，相对应的相机的位姿是在全局坐标物理坐标系下的位姿，位姿计算公式如下：

Ri=Rτi
ti=tτi+vdgivs

变换前 Ri 和 ti 是相机相对于 volume 的位姿，这里将相机相对于volume的位姿转换为相机相对于全局物理坐标系的位姿。

Ri=Rτi 是不是弄错了， Ri 是相机相对于 volume 的，volume 相对于全局物理的是单位矩阵。

Dynamic cube positioning

前面说到把相机放在 volume 的中心，但是如果把相机放到 volume 的中心，相机的视场和 volume 交叠的很少，如下图：

定义 βi 为相机相对于 volume 在 y 轴上的旋转，重新计算 TSDF 的中心相对于相机的位姿为：

rτ=(vs2⋅cos(βi+π2),0,vd2⋅sin(βi−π2))
（公式没看懂，请大神指教）

Color estimation

和深度融合类似，颜色值融合也是按照加权的方式融合，不一样的地方是通过光线投影算法得到的颜色图像不会参与相机的位姿估计。

由于 RGB 相机的深度图像校准问题和 light diffraction 的影响，closed object 边缘估计的颜色值通常不准，在深度通道处这些边缘通常有 stark discontimuities，作者拒绝在深度图像处 strong boundaries 这些测量到的颜色值，边缘通过 7x7 邻域的深度图像计算得到，上图红色的部分显示的是拒绝的地方，在颜色值融合的时候，按照surface normal 和相机光心的夹角进行加权融合，目标的表面和相平面越是平行，加权的时候权值越大。

Camera pose estimation

相机位姿估计结合了 ICP 算法和直接法。
ICP 算法：
http://blog.csdn.net/fuxingyin/article/details/51505854
http://blog.csdn.net/fuxingyin/article/details/51425721

Loop Closure

系统总的流程图如上所示，整个系统分为前端和后端，其中前端包括 camera tracking、surface extraction 和 extended scale volumetric fusion。后端包括visual place recognition、pose optimization 和 map optimization。

作者使用 iSAM 优化通过回环检测建立的约束优化 pose graph，再用优化后的相机的轨迹和回环检测中匹配的特征点来优化 deformation graph，然后使用优化好的 graph 更新 map。

Pose graph

所有加入到 pose graph 相机的位姿都是在全局坐标系下表示的，系统处理的每一帧都会估计他的位姿 Pi ，在 pose graph 中的相机位姿有些和 closed slice 相连接，在位姿相连接的时候，用 Hessian 矩阵来表示位姿相连接的不确定度：
∑=(JτJ)−1

如上二维的 TSDF shifting 和 pose graph 的示意图，绿色 Pγ 触发了shifting， Pγ 和提取的 cloud slice 相连接，如红线所示， deformation graph 将相邻的 4 个节点建立连接。

Place Recognition

作者用 DBoW SURF 描述子进行回环检测，DBoW 介绍：
http://blog.csdn.net/fuxingyin/article/details/51489160

当相机旋转或者移动的距离超过一定阈值的时候，将当前帧加入做关键帧并且进行回环检测，综合角度和平移移动计算公式为：

mab=||r(R−1aRb)||2+||ta−tb||2r(R):SO(3)→R3

当前帧为 [rgbi,di] ，相机移动的距离超过阈值时，首先计算关键点和对应的描述子 Ui∈Ω×R64 ，这些描述子存储在内存中，深度图像 di 在压缩后也存储在内存中。

回环检测首先通过 DBoW 寻找匹配的关键帧，如果存在匹配的图像，将在内存中存储的匹配图像的 SURF 描述子 Um 和深度图像 dm 重新索引出来。

SURF correspondence threshold

给定两帧图像的 SURF 描述子 Ui 和 Um ，用 FLANN 来建立 SURF 的匹配关系，如果能够匹配上的 SURF 点数量不超过 35 个，则认为这不是一个有效的匹配，通过匹配建立 SURF 的匹配关系： G∈Ω×Ω 。

RANSAC Transformation Estimation

通过上一步建立的 SURF 间的匹配，用 RANSAC 算法估计两帧之间的位姿，得到位姿后再用 LM 算法优化重投影误差优化相机位姿。

Point Cloud ICP

用 ICP 再优化上述算法计算得到的位姿，如果匹配点之间的误差小于设定阈值，则认为这是一个有效的回环。

Incremental Graph Construction

构建 graph 时先按照空间关系抽样，如下所示，当节点和节点之间距离大于阈值时将当前帧加入 graph，加入 graph 后节点和节点之间建立连接，连接的方式是每一个节点和前后两个节点之间建立连接。

对于每一个触发 shift 的 pose 都会对应一个 cloud slice，对于 cloud slice 中的 vertex 用 deformation graph 中优化好的参数做更新，更新的公式和 ElasticFusion 中相同。
每个 vertex 在 graph 中索引更新节点时，首先根据 vertex 对应的 cloud slice 找到对应的 pose node，在找到的 pose node 后，和 node 距离上最相近和 k 个 node 相连接，算法如下：

pose graph 中的 node 本来就是按照时间关系存储的，所以可以用二分法进行查找时间上最相近的 node，找到时间上最相近的node之后，再向后查找空间上相近的 node。

每个 vertex 和 graph node 之间的连接是按照 incrementally online 的方式进行的，frontend volume shifting 提取处新的 cloud slice，使用 incremental 的方式构建 vertex 的 weight 是提高 map optimization 效率的关键，但是 cloud slice 和 deformation graph 如何建立连接的，deformation graph 的结果是如何作用于cloud slice，代码里没看到。

Optimization

当检测到存在回环时，优化分为两步，第一步用 iSAM 算法优化回环检测建立的位姿约束优化相机的 pose graph，第二步用优化好的 pose graph 作为 user specified constrain 来优化 deformation graph。

Map Deformation

同 ElasticFusion 优化 deformation graph，ElasticFusion 介绍：
http://blog.csdn.net/fuxingyin/article/details/51433793

第一项保持 R 的单位正交性，第二项保持 deformation graph 参数的连续性，第三项用 iSAM 优化好的 pose graph 代替 user specified vertex position，表达形式如下：

Econp=∑i||ϕ(ti)−t′i||22

其中 t′i 是 iSAM 优化后相机的平移向量， ϕ(ti) 表示根据当前deformation graph 参数计算 deform 后相机的坐标。

作者说只使用上述约束 at some points the surface orientation may not be well optimized，为了克服它又加了优化前后投影出来的 SURF 三维点坐标的约束。

Esurf=∑q||ϕ((RiGq)+ti)−((R′iGq)+t′i)||22

Ri 和 R′i 是优化前后的旋转矩阵， ti 和 t′i 是优化前后的平移向量。

参考文献：

“Real-time large scale dense RGB-D SLAM with volumetric fusion”