real-time scalable dense surfel mapping：基于surfel的全局一致稠密建图系统

基于surfel的
使用稀疏方法追踪
全局一致的模型
动态处理策略：
通过建立不稳定的surfel，如果一定时间内被观测/融合次数不达标则作为离群值删除。

系统总览

我们的系统融合深度和强度（灰度）图像，到一个由surfels组成的全局一致的地图。其中使用Localization System（ORB-SLAM2）做定位、闭环优化、生成位姿图（pose graph）。系统的关键是1基于超像素的surfel，2基于位姿图的surfel融合，3在定位系统完成闭环优化后，对地图进行快速形变以满足全局一致。

记号

每一个surfel $S={\left[S_{\mathbf{p}},S_{\mathbf{n}},S_c,S_w,S_r,S_t,S_i\right]}^T$包含属性：
$S_{\mathbf{p}}\in {\mathbb{R}}^3$ 位置
$S_c\in \mathbb{R}$强度（颜色）
$S_w\in {\mathbb{R}}^{+}$ 权
$S_r\in {\mathbb{R}}^{+}$半径
$S_t\in \mathbb{N}$更新次数。
$S_i\in \mathbb{N}$关联的关键帧索引
系统的输入包括
强度图像（灰度图）
深度图像
相机的运动估计和位姿图（pose graph） （来自与ORBSLAM2）

定位系统和位姿图

使用了ORBSLAM用于相机跟踪和闭环优化。
对于每个输入帧（当前帧），SLAM系统会输出相机位姿估计${\mathbf{T}}_{w,i}\in \mathbb{S}\mathbb{E}(3)$，并给出当前帧的参考关键帧$F_{ref}$（沿用了ORBSLAM2中的概念，是与当前帧共视程度最高的关键帧）。
位姿图，即ORBSLAM2中的共视图，顶点是关键帧，边是关键帧共享的特征点数量。

快速地图变形

如果SLAM系统中的位姿图进行了更新，那么我们也会变形所有的surfel（位姿和法向）以保证全局一致，在进行当前帧的surfel初始化和融合之前。
我们以surfel的参考帧作为依据进行变形。对每一个surfel $S$，其参考帧为$F$，我们使用变换${\mathbf{T}}_{w,\hat{F}}{\mathbf{T}}_{w,F}^{-1}$来对$S$的法向和位姿进行处理，其中，${\mathbf{T}}_{w,\hat{F}}$和${\mathbf{T}}_{w,F}^{-1}$分别为关键帧$F$优化前后的位姿。

超像素提取

首先通过初始化聚类中心，然后在分配步骤和更新步骤之间交替，根据像素的强度，深度和空间位置对其进行聚类。
这种超像素分割适用于存在空缺深度的图像。
通过在图像上建立网格，初始化簇中心$C_i={\left[x_i,y_i,d_i,c_i,r_i\right]}^T$。
$x_i,y_i$是该簇像素的平均位置，初始化为位置中心（网格中心）。
$d_i$是平均深度，初始化为[xi,yi]像素的深度，或者为NaN。
$c_i$是平均强度值，初始化为[xi,yi]像素的强度。
$r_i$超像素半径（定义为簇内点的最大距离）。

分配

我们定义像素 u = { u x , u y , u d , u i } u=\{u_x, u_y,u_d,u_i\} u={ux​,uy​,ud​,ui​}和一个候选簇中心 C i = { x i , y i , d i , c i } C_i=\{x_i,y_i,d_i,c_i\} Ci​={xi​,yi​,di​,ci​}之间的距离
$$\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$分别是在当前像素无有效深度和有有效深度值时的距离值。
$N_s^2,N_c^2,N_d^2$是为在求和之前标准化距离、强度和深度。
遍历图像的每个像素，每个像素寻访周围四个候选聚类中心，如果目标像素和候选簇中心具有有效的深度值，则基于$D_d$进行比较并分配；否则采用$D$。

更新

一旦所有像素得到分配，更新簇中心。$x_i,y_i,c_i$由属于该簇的所有像素均值获得。而深度$d_i$可以通过最小化一个Huber误差获得：
$$E_d=\sum _{\mathbf{u}}{L}_{\delta }\left({\mathbf{u}}_d-d_i\right)$$
$u$是属于该簇的像素，且拥有有效深度，$u_d$是其深度。
可以使用高斯牛顿迭代求解簇深度$d_i$。

Surfel 初始化

对于一个拥有足够多像素的簇 C i = { x i , y i , d i , c i } C_i=\{x_i,y_i,d_i,c_i\} Ci​={xi​,yi​,di​,ci​}，我们初始化surfel $S={\left[S_{\mathbf{p}},S_{\mathbf{n}},S_c,S_w,S_r,S_t,S_i\right]}^T$
$S_c$强度初始化为簇的平均强度$c_i$
$S_i$是当前帧（当前图像）在ORB-SLAM2中跟踪线程获得的参考关键帧的索引。
$S_t$更新次数为0（代码里面为1）
$S_n$法向首先初始化为所有像素的平均的法向，然后再最小化下式获得最终法向：
$$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}}$是像素$u$在三维空间的投影，$\overline{\mathbf{p}}$是3D点的平均位置。
$S_p={\pi }^{-1}[x_i,y_i]$位置，满足以下方程：
$$S_{\mathbf{n}}\cdot \left(S_{\mathbf{p}}-\overline{\mathbf{p}}\right)+b=0$$
可以由下式解得：
$$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$是相机内参。
$S_r$半径，需要能够覆盖超像素：
$$S_r=\frac{S_{\mathbf{p}}(z)\cdot r_i\cdot \Vert K^{-1}\cdot {\left[x_i,y_i,1\right]}^T\Vert }{f\cdot S_{\mathbf{n}}\cdot \left(K^{-1}\cdot {\left[x_i,y_i,1\right]}^T\right)}$$
$S_p(z)$是surfel的深度，$f$是相机的focal length 焦距。
$S_w$权与surfel的深度有关：
$$S_w=\frac{b^2{f}^2}{S_{\mathbf{p}}(z{)}^4{\sigma }^2}$$
其中${\sigma }^2$是variane of disparity estimation ，视差估计的方差。

局部地图提取

重建一个大规模的环境地图需要百万量级的surfel。但是，每次进行surfel融合时，只需要根据位姿图提取一小部分的surfels即可。
我们基于假设：the keyframes with the number of minimum edges to the current keyframe $F_{ref}$ below $G_{\delta }$ are locally consistent.我们提取这些keyframe关联的surfel作为lcoal map。局部一致的keyframes可以在位姿图上通过广度优先搜索获得。

Surfel融合

这一步，融合局部地图local map和通过当前图像新建的surfels。
我们将世界坐标系下的localmap中的surfel变换到当前相机坐标系下，并投影到像素平面上，获得一个像素位置$\mathbf{u}=\pi \left(S_{\mathbf{p}}^l\right)$。
因为任一一个新建surfel都由图像中的一个超像素创建，如果某超像素创建了surfel $S^n$，且包含像素$u$，那么将建立$S^n$和$S^l$的对应关系。
通过$\left|S_{\mathbf{p}}^n(z)-S_{\mathbf{p}}^l(z)\right|<S_{\mathbf{p}}^l(z{)}^2/(bf)\cdot 2\sigma$和$S_{\mathbf{n}}^n\cdot S_{\mathbf{n}}^l>0.8$验证匹配的位置和法向关系。如果不满足则放弃该匹配关系。
如果满足匹配关系，那么更新surfel $S^l$：
$$\begin{array}{rl}& \begin{array}{l}{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\end{array}\\ & S_t^l\leftarrow S_t^l+1,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}$$
那些没有被融合的新建surfel也填入地图中。为了处理离群值，我们剔除$\left|S_i-F_{ref}\right|>10$（即当前时间距离surfel被创建时间，已经经过了10张关键帧），但是surfel的融合次数少于5此，删去该surfel。