论文《Probabilistic Surfel Fusion for Dense LiDAR Mapping》的阅读记录

使用LIDAR数据建立surfels的两个问题

在构建密集surfel地图时，相对于使用RGB-D数据建立surfel，使用LIDAR数据建立surfels存在两个主要问题。

surfel退化

在RGB-D数据中，只需要通过像素平面的邻点，即可找到目标点的相邻点。但是在LIDAR数据中，没有投影平面，只能在3D空间内通过距离寻找相邻点。

如图，LIDAR点云中法向的退化。其中绿色的线表示法线，a)展示了理想情况，即点分布均匀时，求取的法向；b)退化情况，此时，点距离传感器较远，点云在物体表面呈线状分布，从这样的点集中估计得到的法线是不可靠的。
为了解决这样的情况，我们对surfel的位置和法向的不确定性进行建模。

surfel匹配更为复杂

在RGB-D数据中，可以采用投影数据关联。但是因为没有投影平面，该方法不适合LIDAR数据。最简单的方式是采用最近距离匹配。一般而言，考虑到传感器误差，如果搜索半径小于传感器误差，匹配准确性会降低，同时，如果搜索半径大于传感器误差，则会降低地图分辨率。但在LIDAR数据中，激光点沿其光束方向的深度不确定性较高，因此半径搜索方法会严重降低地图分辨率。可能会有人考虑在搜索时考虑该不确定性，如下图右侧，而这种方法也常常应用于基于滤波的SLAM算法。但是，在这种情况下，很难控制地图分辨率而不分散（discretizing）环境 。 为了解决这个问题，我们提出了一种新的用于surfel匹配的算法，该算法保证了的地图分辨率。

如图，传统的寻找点匹配的方法。红线表示雷达射线方向，黑点表示激光点。左侧是半径搜索方法，右侧是沿射线搜索（考虑不确定度）。

地图表示

系统建立两种全局surfel地图，ellipsoid surfel map（ESM）$S_g$和disk surfel map (DSM) $M_g$。前者主要用于快速且鲁棒地定位，而后者用于精确地进行三维环境重建。
两个surfel地图分别独立地使用他们的局部地图$S_l$、$M_l$进行更新，而局部地图则来源于传感器获得的激光扫描。
ESM是由3D椭球组成，使用多分辨率体素哈希从激光点中提取。每一个椭圆：$\left\{\mathbf{c}\in {\mathbb{R}}^3,{\Sigma }_{\mathbf{c}}\in {\mathbb{R}}^{3\times 3}\right\}$，前者是质心，后者是协方差矩阵，表达了点在体素内的分布。
DSM由2D disk surfel组成，$\phi =\left\{p\in {\mathbb{R}}^3,\hat{\mathbf{n}}\in {\mathbb{R}}^3\right\}\in \mathbb{M}$，前者是位置，从激光点中均匀取样获得；后者是是法向，由激光点及其邻域获得。与传统的surfel不同，我们还将使用${\Sigma }_{\mathbf{p}},{\Sigma }_{\hat{\mathbf{n}}}\in {\mathbb{R}}^{3\times 3}$、即disk surfel 位置和法向的不确定性，用于surfel融合。

ESM地图定位

找到局部地图${\mathbb{S}}_l$和ESM地图$S_g$之间的匹配关系，通过最小化点面误差配准当前局部地图${\mathbb{S}}_l$：
$$e=\sum _{i=1}^n{e}_i^2,e_i={\hat{\mathbf{n}}}_i^{\top }\left({\mathbf{p}}_i^g-\left(\mathbf{R}{\mathbf{p}}_i^l+\mathbf{t}\right)\right)$$
其中，$\mathbf{R}\in \mathrm{S}\mathrm{O}(3)$和$\mathbf{t}\in {\mathbb{R}}^3$指代变换，$\left({\mathbf{p}}_i^g,{\hat{\mathbf{n}}}_i^g\right)\in {\mathbb{S}}_g$和$\left({\mathbf{p}}_i^l,{\hat{\mathbf{n}}}_i^l\right)\in {\mathbb{S}}_l$分别指代局部地图和全局地图之间的第$i$对surfel匹配。${\hat{\mathbf{n}}}_i=\left({\hat{\mathbf{n}}}_i^g+{\hat{\mathbf{n}}}_i^l\right)/\left|{\hat{\mathbf{n}}}_i^g+{\hat{\mathbf{n}}}_i^{\prime }\right|$。
然后使用高斯牛顿法求解变换。
由于采用的是point-to-plane ICP，所以系统更倾向于选择平面区域的ellipsoid surfels。因此，将采用以下方法融合局部地图和ESM之间匹配的surfel：将ESM全局地图中的surfels替换为局部地图中的surfel，如果局部地图中的surfel拥有更大的${\lambda }_1$、${\lambda }_2$和更小的${\lambda }_3$。
ESM定位结果，将会用于DSM的融合过程。

DSM中Surfel不确定性建模

位置不确定度

因为surfel质心位置是由激光点位置计算得到，所以具有与LiDAR测量相同的不确定性特征，受入射角，环境温度和湿度的影响。surfel的位置不确定度沿着激光射线方向更大，所以，我们将位置不确定度建模为具有三个主轴的椭球。沿着射线方向的不确定度定义为：距离不确定度${\sigma }_r^2$ 和 由入射角引起的附加不确定度 ${\sigma }_i^2$。所以在世界坐标系下，完整的位置不确定度为：
$${\Sigma }_{\mathbf{p}}={}^w{\mathbf{R}}_l{}^l{\mathbf{R}}_b{\Sigma }_b{\left({}^w{\mathbf{R}}_l^l{\mathbf{R}}_b\right)}^{\top }$$
其中，${}^w{\mathbf{R}}_l$和${}^l{\mathbf{R}}_b$分别是 从laser到世界坐标，从beam到laser坐标。而在beam坐标系下，不确定度${\Sigma }_b=\mathrm{diag}\left({\sigma }_x^2,{\sigma }_y^2,{\sigma }_z^2\right)$。如何从${\sigma }_r^2$ 与 ${\sigma }_i^2$得到${\Sigma }_b$，见[18]。

法向不确定度

法向不确定度直接与协方差矩阵的三个特征值相关，而协方差矩阵由激光点邻域得到。有几种邻域分布不利于法向生成：${\lambda }_1$和${\lambda }_2$都很小，局部呈点状分布；${\lambda }_1\gg {\lambda }_2$，局部点云线状分布；${\lambda }_3$较大，局部点云散乱分布。
因为法向保持模值为1，相当于法向端点在以surfel质心为中心的单位球上运动，因此，法向的不确定度的自由度为2。由于流形空间上的不确定度的传播不容易定义，我们提出了一种近似模型，该模型定义了法向矢量尖端处切向空间中两个自由度的不确定性，如图4所示。

surfel法向退化的例子，其中绿线为法向，由局部点云（蓝色点）计算得到，因为局部点云呈线状分布，h法向出现退化。法相的不确定度用随机样本的端点（红色点）描述，用切平面上的椭椭圆（红色椭圆）表示。
为了反应这种点云分布和反向不确定度之间的关系，我们定义切向平面上法向不确定度为$\mathrm{diag}\left({\sigma }_{\theta },{\sigma }_{\varphi }\right)$，它们是特征值的函数。
$$\begin{array}{l}{\sigma }_{\theta }={\left(1+e^{-w\left({\alpha }_{\theta }+{\alpha }_{z1}+{\alpha }_{z2}\right)}\right)}^{-1}\\ {\sigma }_{\varphi }={\left(1+e^{-w\left({\alpha }_{\varphi }+{\alpha }_{z1}+{\alpha }_{z2}\right)}\right)}^{-1}\end{array}$$
其中，
$$\begin{gathered} {\alpha }_{\theta }=a{\lambda }_1^{-1}-0.5 \\ {\alpha }_{\varphi }=b{\lambda }_2^{-1}-0.5 \\ {\alpha }_{z1}=\log \left({\lambda }_3/{\lambda }_1\right)c+0.5 \\ {\alpha }_{z2}=d{\lambda }_3-0.5 \end{gathered}$$
可以看出，${\alpha }_{\theta }$和${\alpha }_{\varphi }$分别惩罚了 ${\lambda }_1$和${\lambda }_2$过小的情况。${\alpha }_{z1}$和${\alpha }_{z2}$则惩罚了${\lambda }_3$过大的情况。$a，b，c，d$是缩放系数，并且是统计确定的。$w$是sigmiod函数的比例因子。
最后，世界坐标系中法向的不确定度为：
$${\Sigma }_{\mathbf{n}}=\mathbf{v}\mathrm{diag}\left({\sigma }_{\theta },{\sigma }_{\varphi },\epsilon \right){\mathbf{v}}^{\top }$$
其中添加了$\epsilon$以防止矩阵求逆中的奇异性问题。特征向量矩阵$v$用于将法线不确定性方向与世界坐标系中的邻域点云分布对齐。

DSM中surfel匹配

这一节描述了如何在全局DSM地图$M_g$和局部DSM地图$M_l$中寻找匹配，为下一步surfel融合做准备。
首先，通过ICP得到的变换，将局部地图$M_l$变换到世界坐标系下。对每个surfel ${\phi }_l\in {\mathbb{M}}_l$，需要找到surfel候选集合${\mathbb{A}}_g$。通过基于八叉树的最近邻居搜索算法选择初始匹配候选者，然后计算源surfel与每个候选surfel之间的分辨率距离$r$、深度距离$d$。分辨率距离$r$要低于分辨率距离阈值${\theta }_r$。而深度距离$d$满足：
$$\begin{gathered} {\sigma }^2={\hat{\mathbf{n}}}_s^{\top }{\Sigma }_s{\hat{\mathbf{n}}}_s+{\hat{\mathbf{n}}}_d^{\top }{\Sigma }_d{\hat{\mathbf{n}}}_d \\ d/\sigma <{\theta }_d \end{gathered}$$
前者表示源surfel和目标surfel的位置不确定度在法线方向上的传播。而后者${\theta }_d$是深度阈值。这种匹配方法使匹配过程可以沿射线方向搜索更多，同时无需体素网格即可有效地保持所需的表面分辨率。
匹配过程算法如下：

两个距离的图示：

Surfel 融合

定义surfel $\phi$的位置$\mathbf{p}\sim \mathcal{N}\left({\mu }_{\mathbf{p}},{\Sigma }_{\mathbf{p}}\right)$和法向$\hat{\mathbf{n}}\sim \mathcal{N}\left({\mu }_{\hat{\mathbf{n}}},{\Sigma }_{\hat{\mathbf{n}}}\right)$。
考虑局部地图surfel ${\phi }_l\in {\mathbb{M}}_l$和其在全局地图中的匹配${\phi }_g\in {\mathbb{M}}_g$，假设两个观测是独立的，则卡尔曼滤波器给出贝叶斯更新公式为:
$$\begin{array}{rl}& {\mu }_g^{\prime }={\Sigma }_g^{\prime }\left({\Sigma }_g^{-1}{\mu }_g+{\Sigma }_l^{-1}{\mu }_l\right)\\ & {\Sigma }_g^{\prime }={\left({\Sigma }_g^{-1}+{\Sigma }_l^{-1}+{\Sigma }_s^{-1}\right)}^{-1}\end{array}$$
但是，因为法向处于流型当中，应采用不同的做法。法向不确定度的规范形式位于单位球面的切向空间上，同时，为了处理不确定性传播，我们通过等式${\Sigma }_{\mathbf{n}}=\mathbf{v}\mathrm{diag}\left({\sigma }_{\theta },{\sigma }_{\varphi },\epsilon \right){\mathbf{v}}^{\top }$将2D法线不确定性提升到3D空间，再采用下式获得融合surfel的法向。
$$\begin{array}{rl}& {\Sigma }_{{\mathbf{n}}_d}^{\prime }\leftarrow {\left({\Sigma }_{{\mathbf{n}}_s}^{-1}+{\Sigma }_{{\mathbf{n}}_d}^{-1}\right)}^{-1}\\ & {\mathbf{n}}_d^{\prime }\leftarrow {\Sigma }_{{\mathbf{n}}_d}^{\prime }\left({\Sigma }_{{\mathbf{n}}_s}^{-1}{\mathbf{n}}_s+{\Sigma }_{{\mathbf{n}}_d}^{-1}{\mathbf{n}}_d\right)\end{array}$$
通常，计算获得的法向不确定度不相切于单位球${\Sigma }_{{\mathbf{n}}_d}^{\prime }$，还要通过一步分解，得到相切分量作为结果。
由于这是一种线性化方法，因此我们将其应用限于表面上两个向量的距离足够小的情况。
在某些情况下，源surfel或目标surfel的法向会因为点云分布而退化。通过查看${\sigma }_{\theta },{\sigma }_{\varphi }$的不确定度比率，可以轻松地找到surfel退化。我们并没有将这些退化的surfel丢弃，并等待其被正确地观测。当目标surfel或源surfel之一退化时，新的法线方向和不确定性将遵循未退化的surfel的方向和不确定性。 在源和目标冲浪都退化的情况下，法线是通过第一主方向的叉积获得的。
具体算法如下：

其中，得到法向不确定度${\sum }_{new}$ 时，添加的不确定度$\mathrm{diag}\left({\sigma }_{\theta }^s,{\sigma }_{\varphi }^s,-{\lambda }_3\right)$是防止surfel对重复的系统误差过拟合，例如由混合像素问题引起的噪声点[23]。
与全局地图不匹配的surfel将作为新的不稳定surfel添加到全局地图中。当重访某个surfel时，将其周围一定范围内的，建立时间超过5min的不稳定surfel删除。

结论

本文，提出了一个新方法——使用概率surfel融合进行密集LIDAR建图。我们构建了两个surfel地图，3D ellipsoid surfel地图（ESM）和2D disk surfel地图（DSM）。我们在稀疏的ESM上对齐点云，并基于贝叶斯滤波更新稠密的DSM。同时，我们对每个surfel的位置和法向量的不确定度进行建模，并考虑了因激光点的分布产生的surfel退化。提出的数据关联方法提高了surfel分辨率，抑制了噪声。
通过模拟数据实验和真是数据实验，与以前的工作相比，我们的方法可以生成更准确的surfel图，并且噪声更少，地图元素最少。 在未来的工作中，我们的方法可以进一步扩展到具有硬件和软件优化功能的实时LiDAR建图，以实时生成准确的密集LiDAR surfel。 这是适用的，因为我们的方法顺序更新了surfel地图，而不是通过批处理来全局优化地图。