Real-time Scalable Dense Surfel Mapping 论文学习与效果演示
Real-time Scalable Dense Surfel Mapping
实时可扩展稠密面元建图
Abstract
利用稀疏视觉SLAM估计相机位姿，并在全局一致模型中融合灰度图和深度图
从灰度图和深度图中提取超像素（Superpixel）来构建Surfel模型
https://www.zhihu.com/question/27623988?sort=created （知乎：Superpixel的大致原理）
* 把一些具有相似特性的像素“聚合”起来，形成一个更具有代表性的大“元素”
* 任何图像分割算法的过度分割(over-segmentation)，即可生成超像素根据SLAM系统的位姿图进一步组织Surfel以实现 O(1)的融合时间复杂度（不论重建模型的尺度）
利用优化的位姿图进行了一个快速地图变换，以实现实时的全局地图一致性
I. Introduction
移动机器人的建图系统需满足以下四点：
1. 稠密建图用于导航
2. 良好的扩展性和效率易应对不同环境和算力限制
3. 全局一致性
4. 能够从RGB-D、双目、单目相机融合不同质量的深度地图利用超像素代表Surfel
主要贡献：
1. 利用从灰度和深度图中提取的超像素构建Surfel模型superpixel能够融合低质量的深度图 / superpixel-based surfel提高运行和内存效率
2. 利用稀疏SLAM系统的位姿图进一步组织Surfels
3. 使该系统仅利用CPU计算（even in urban-scale）
II. Related Work
- 与基于体素的方法相比，基于面元的方法相对可扩展性更强，因为只有表面信息被储存在系统中
III. System Overview
系统功能分为6部分：定位系统和位姿图、快速地图变换、超像素提取、面元初始化、局部地图提取和面元融合
图2. 所提出方法的系统框架。传感器由机器人应用程序确定。我们的系统可以融合来自RGB-D、双目或单目摄像机的深度图。定位系统用于跟踪相机位姿，检测回环，并提供关键帧的优化位姿图。系统中的每个面元都匹配到一个关键帧，所有面元都存储在地图数据库中。
使用最先进的稀疏视觉SLAM系统（ORB-SLAM2或VINS-MONO）作为定位系统来跟踪摄像机的运动，检测闭环并优化位姿图
建图的关键：
1. 基于超像素的面元
2. 基于位姿图的面元融合
3. 快速地图变换
- 图3. 使用KITTI odometry 05作为示例，在相机重新访问街道时显示地图变形和以前地图的重复使用。为简单起见，面元可视化为点云，局部地图以彩色突出显示。在重新访问期间，在第1327帧上检测到回环。（a）：提取局部地图用来融合第1326帧。（b）：第1326帧的结果。如（a）和（b）中的红色圆圈所示，地图由于回环检测前的漂移而未对准。（c）：1327帧检测到一个回环，并相应地变形地图以消除漂移。由于更新的位姿图，可以找到更多局部一致的位姿和面元。（d）：提取局部地图用来融合1327帧。如图所示，先前的地图被重新用于融合当前帧。（e）是第1327帧融合后的结果。
IV. Surfel Mapping System
A. Notation
面元表示环境
对于每个面元 S=\left[S_{\mathbf{p}}, S_{\mathbf{n}}, S_{c}, S_{w}, S_{r}, S_{t}, S_{i}\right]^{T}有如下属性：
1. 位置$S_{\mathbf{p}}\in \mathbb{R}^{3}$
2. 法线$S_{\mathbf{n}}\in \mathbb{R}^{3}$
3. 灰度$S_{c}\in \mathbb{R}$
4. 权重$S_{w}\in\mathbb{R}^{+}$
5. 半径$S_{r}\in \mathbb{R}^{+}$
6. 更新时间$S_{t}\in \mathbb{N}$：用于检测瞬时异常值或动态目标
7. 匹配关键帧的索引$S_{i}\in \mathbb{N}$：用于表示观察到该面元的最后一个关键帧系统的输入是灰度图 、深度图 、相机自我运动 和位姿图 ：
1. 第 $i$ 幅灰度图 $I_{i}$: $\Omega \subset \mathbb{R}^{2} \mapsto \mathbb{R}$
2. 第 $i$ 幅深度图 $D_{i}$: $\Omega \subset \mathbb{R}^{2} \mapsto \mathbb{R}$
3. 利用相机投影函数 $\mathbf{u}=\pi(\mathbf{p})\in \Omega$ 将空间中的点 $\mathbf{p}=[x, y, z]^{\mathrm{T}}$ 投影到图像的像素 $\mathbf{u}:=[u, v]^{\mathrm{T}} \in \Omega$ 上
4. 像素可被反投影到空间中一点 $\mathbf{p}=\pi^{-1}(\mathbf{u}, d)$ ，其中 $d$ 是深度B. Localization System and Pose Graph
稀疏视觉SLAM作定位系统来跟踪相机运动，并且在回环时优化位姿图。
在相机坐标系下的第 i 幅灰度图 I_{i} 中的点 \mathbf{p}_{c} 和向量 \mathbf{n}_{c}，在世界坐标系下的表示如下：
\mathbf{p}_{w}=\mathbf{R}_{w, i} \mathbf{p}_{c}+\mathbf{t}_{w, i}\\ \mathbf{n}_{w}=\mathbf{R}_{w, i} \mathbf{n}_{c}
其中 \mathbf{R}_{w, i}\in\mathbb{SO}(3) 和 \mathbf{t}_{w, i}\in\mathbb{R}^{3} 分别是第 i 个相机位姿 \mathbf{T}_{w, i} \in \mathbb{S} \mathbb{E}(3) 的旋转矩阵和变换向量；同理，利用 \mathbf{T}_{w, i}^{-1} 可以转回相机坐标
本系统中的位姿图是一个无向图 （类似于ORB-SLAM2的隐含图）：
1. **顶点** ：关键帧（SLAM中维护）
2. **边缘** ：表示关键帧共享共同特征局部一致性 ：关键帧彼此之间边缘的最小数量小于 G_{\delta}
C. Fast Map Deformation
- 若更新位姿图则进行面元变形，以在面元初始化和融合之前保持全局一致
- 只对面元变形，以保证面元间的相对位姿和其关联的关键帧保持不变
- 对于关键帧 F 上的面元 S，其位置和法线利用 \mathbf{T}_{w, \hat{F}} \mathbf{T}_{w, F}^{-1} 进行变换（ \mathbf{T}_{w, F} 和 \mathbf{T}_{w, \hat{F}} 分别是 F 优化前后的位姿）
D. Superpixel Extraction
基于超像素模型的优点：
1. 减轻系统内存负担
2. 减少低质量深度图带来的的外点和噪声改进SLIC的k-means提取方法：
1. 将RGB功能扩展到能够分割灰度和深度图像
2. 首先初始化聚类中心，然后在**分配**(assignment)和**更新**(update)步骤间交替
3. 即使部分像素没有有效的深度信息也能进行图像分割聚类中心 C_{i}=\left[x_{i}, y_{i}, d_{i}, c_{i}, r_{i}\right]^{T} 在图像的规则网格初始化：
1. 是聚类像素的平均位置：$\left[x_{i}, y_{i}\right]^{T}$（初始化为中心位置）
2. 平均深度：$d_{i}$（初始化为 $\left[x_{i}, y_{i}\right]^{T}$ 的深度，若无有效的深度信息则初始化为NaN `Not a Number`）
3. 平均灰度：$c_{i}$（初始化为 $\left[x_{i}, y_{i}\right]^{T}$ 的灰度）
4. 超像素半径：$r_{i}$ （定义为离 $\left[x_{i}, y_{i}\right]^{T}$ 的最大距离）Assignment Step：
每个通过SLIC得到的聚类会因每个像素的更新而替换（保持复杂度的同时解决深度无效问题）。在像素 u 和候选聚类中心 C_{i} 间定义两种距离：
\begin{array}{c} D=\frac{\left(x_{i}-\mathbf{u}_{x}\right)^{2}+\left(y_{i}-\mathbf{u}_{y}\right)^{2}}{N_{s}^{2}}+\frac{\left(c_{i}-\mathbf{u}_{i}\right)^{2}}{N_{c}^{2}} \\ D_{d}=D+\frac{\left(1 / d_{i}-1 / \mathbf{u}_{d}\right)^{2}}{N_{d}^{2}} \end{array}
其中 D_{d} 和 D 分别是包括和不包括深度信息的距离；\left[\mathbf{u}_{x}, \mathbf{u}_{y}\right], \mathbf{u}_{d} 和 \mathbf{u}_{i} 分别是像素 u 的位置、深度和灰度；在求和之前，\mathbf{N}_{s}^{2}, \mathbf{N}_{c}^{2} 和 \mathbf{N}_{d}^{2} 分别用于归一化 距离、颜色和深度接近度。
每个像素需要扫描4个相邻的聚类中心，比较 D_{d} 完成分配；若其中有无效深度，则比较 D
Update Step：
分配完像素后将更新聚类中心：
1. \mathbf{x}_{i}, \mathbf{y}_{i} 和 \mathbf{c}_{i} 根据所有分配像素的均值更新
2. d_{i} 通过最小化具有半径 \delta 的Huber损失来更新(通过最小二乘迭代求解)：
E_{d}=\sum_{\mathbf{u}} L_{\delta}\left(\mathbf{u}_{d}-d_{i}\right)
其中，u 是具有有效深度信息的分配像素，{u}_{d} 是其深度信息；d_{i} 通过高斯-牛顿法迭代估计
这种异常值鲁棒的深度不仅使系统能够处理低质量的深度地图，也能保持深度的不连续性
E. Surfel Initialization
一个有足够分配像素的超像素聚类中心 {C}_{i} ，利用异常值鲁棒的方法初始化一个面元 S=\left[S_{\mathbf{p}}, S_{\mathbf{n}}, S_{c}, S_{w}, S_{r}, S_{t}, S_{i}\right]^{T} ，初始值如下：
1. {S}_{c}：聚类中心灰度 {C}_{i}
2. {S}_{i}：参考关键帧 F_{r e f} 的索引
3. {S}_{t}：初始化为0（表示还未被其它帧融合）
4. {S}_{n}：所有像素的平均法线，然后通过最小化拟合误差 {E}_{S} 进行微调
E_{S}=\sum_{\mathbf{u}} L_{\delta}\left(S_{\mathbf{n}} \cdot\left(\mathbf{p}_{\mathbf{u}}-\overline{\mathbf{p}}\right)+b\right)
其中，\mathbf{p}_{\mathbf{u}}=\pi^{-1}\left(\mathbf{u}, \mathbf{u}_{d}\right) （具有深度信息的分配像素反投影到空间中的点），\overline{\mathbf{p}} 表示3D点 \mathbf{p}_{\mathbf{u}} 的均值，b 是偏置
5. {S}_{p}：被定义为在面元上被相机观察作为 \left[x_{i}, y_{i}\right]^{T}的点
\left\{\begin{array}{l}S_{\mathbf{n}} \cdot\left(S_{\mathbf{p}}-\overline{\mathbf{p}}\right)+b=0 \\ \pi\left(S_{\mathbf{p}}\right)=\left[x_{i}, y_{i}\right]^{T}\end{array}\right.
且能够得到闭式解（解析解）：
S_{\mathbf{p}}=\frac{S_{\mathbf{n}} \cdot \overline{\mathbf{p}}-b}{S_{\mathbf{n}} \cdot\left(K^{-1}\left[x_{i}, y_{i}, 1\right]^{T}\right)} K^{-1}\left[x_{i}, y_{i}, 1\right]^{T}
其中 K 是相机内参矩阵
6. {S}_{r}：初始化为面元半径使得其投影可以覆盖灰度图像中提取的超像素：
S_{r}=\frac{S_{\mathbf{p}}(z) \cdot r_{i} \cdot\left\|K^{-1} \cdot\left[x_{i}, y_{i}, 1\right]^{T}\right\|}{f \cdot S_{\mathbf{n}} \cdot\left(K^{-1} \cdot\left[x_{i}, y_{i}, 1\right]^{T}\right)}
其中 S_{\mathbf{p}}(z) 是面元深度，f 是相机焦距
7. {S}_{w}：初始化为估计面元深度的逆方差，假设视差估计的方差是 \sigma^{2} :
S_{w}=\frac{b^{2} f^{2}}{S_{\mathbf{p}}(z)^{4} \sigma^{2}}
其中 b 是双目传感器的基线；
深度估计方法：首先估计像素视差 {d}_{dis}，然后反转为深度值：d=bf / d_{dis}
F. Local Map Extraction
-
只有一个基于位姿图提取的面元子集 （subset）会与初始化的面元融合，原因如下：
- 无论重建规模，局部地图融合都要保证 O(1)的更新时间复杂度
- 累积跟踪误差会导致融合后产生较大漂移，无法实现全局一致性
-
提取局部一致的关键帧上的面元作为局部地图
-
局部一致的关键帧可通过在位姿图上的广度优先搜索找到
G. Surfel Fusion
提取的局部面元会与新初始化的面元进行融合，步骤如下：
给定当前相机位姿估计 T_{w,c}，局部面元的位置和法线通过 T_{w,c}^{-1} 转换到当相机坐标系
然后每个局部面元 {S}^{l} 被反投影到输入帧的对应的像素：u = \pi(S_{p}^l)
如果一个面元 {S}^{n} 是基于一个包含 u 的超像素初始化得到的，如果它们有相似的深度和法线，则认为它们匹配：
\left|S_{\mathbf{p}}^{n}(z)-S_{\mathbf{p}}^{l}(z)\right|
{S}^{l} 会与匹配的面元 {S}^{n} 融合，通过如下式子实现：
\begin{aligned}&S_{\mathbf{p}}^{l} \leftarrow \frac{S_{\mathbf{p}}^{l} S_{w}^{l}+S_{\mathbf{p}}^{n} S_{w}^{n}}{S_{w}^{l}+S_{w}^{n}}, S_{c}^{l} \leftarrow S_{c}^{n}\\ &S_{\mathbf{n}}^{l} \leftarrow \frac{S_{\mathbf{n}}^{l} S_{w}^{l}+S_{\mathbf{n}}^{n} S_{w}^{n}}{S_{w}^{l}+S_{w}^{n}}, S_{i}^{l} \leftarrow S_{i}^{n}\\ &\begin{array}{l}S_{t}^{l} \leftarrow S_{t}^{l}+1, \quad S_{w}^{l} \leftarrow S_{w}^{l}+S_{w}^{n} \\ S_{r}^{l} \leftarrow \min \left(S_{r}^{n}, S_{r}^{l}\right)\end{array}\end{aligned}
为处理外点，\left|S_{i}-F_{\text {ref }}\right|>10 但更新次数少于5的面元会被剔除
V. 实际效果
SLAM定位系统 ：VINS-RGBD-on-Atlas-200DK （Author：刘健恒，周才杰，刘浩文）
https://gitee.com/caide_long/atlas-object-detection-for-UAV/tree/atlas_slam/
传感器 ：Intel® Realsense D455
处理器 ：Intel® Core™ i5-7300HQ CPU @ 2.50GHz
实验室环境下效果图：
演示视频：
【Dense Surfel Mapping + VINS + Intel Realsense D455-哔哩哔哩】https://www.bilibili.com/video/BV15V411E71H/
Dense Surfel Mapping + VINS + Intel Realsense D455