【论文笔记】基于点云柱面投影图的平面特征提取SLAM：　Fast planar surface 3D SLAM using LIDAR 2017

论文首先将三维点云投影在了三个平面上得到无直线畸变的投影视图，然后利用《Fast segmentation of range imagery into planar regions》中的方法对投影图进行分割得到连通的近似平面子集，使用《Place recognition based on matching of planar surfaces and line segments》中的方法对场景中的平面特征进行表示（高斯不确定性模型），实现了对场景中平面特征的高校提取、表达与匹配。

本文提出了可以在大场景下实时运行的SLAM系统，其使用平面表面段（planar surface segments）来表示环境，因为它们在室内和室外城市空间中都很普遍。

本文的三个创新点：

使用了现有的方法（原本是设计用于RGBD数据），在全景视图上对点云进行分割：将点云划分为三个双通道的全景图上，基于Delaunay三角剖分以及区域合并，实现点云的2.5D快速分割。

《Fast pose tracking based on ranked 3D planar patch correspondences》
利用现有的基于平面分割匹配的相对位姿估计模型（《Place recognition based on matching of planar surfaces and line segments》）在点云全景图上进行操作，极大地减少了位姿约束计算中outlier的数目，提高了处理速度。
本文提出了一个基于平面分割段的全局地图构建方法，减少了地图表达时需要的平面表面分割段的数目。其减少的原因在于，不同时间获得的场景观测会有重叠部分。这些重叠部分在全局地图中不会形成新的平面分割段。

论文方法

一、局部地图构建

局部地图包含了从点云中提取的planar surface segment（表平面分割段）。
局部图用于创建全局地图，并用于计算相对位姿，其可作为SLAM姿态图中的姿态约束。

1.1 特征提取：Detection of planar surface segments

点云投影
特征提取过程基于三角化（2D Delaunay triangulation），因此需要对点云进行合适的2D平面投影。然而，点云的柱面全景投影视图中，三维空
间中的直线会发生畸变变成曲线。因此，通过Delaunay三角剖分法获得的三角形应用于这种投影实际上并不表示三角表面。
作者的方法是将点云分别投影在三个平面上,得到三个双通道的柱面投影图：

这样每个点云经过投影后会有三个图像坐标(ui,vi),i=1,2,3：
xi=−sn(i)Z+cn(i)Xyi=Yzi=cn(i)Z+sn(i)Xui=fuxizi+ucvi=fvyizi+vc\begin{array}{l} x_{i}=-s_{n}(i) Z+c_{n}(i) X \\ y_{i}=Y \\ z_{i}=c_{n}(i) Z+s_{n}(i) X \\ u_{i}=f_{u} \frac{x_{i}}{z_{i}}+u_{c} \\ v_{i}=f_{v} \frac{y_{i}}{z_{i}}+v_{c} \end{array} xi=−sn(i)Z+cn(i)Xyi=Yzi=cn(i)Z+sn(i)Xui=fuzixi+ucvi=fvziyi+vc
其中:sn={0,sin⁡(2π/3),−sin⁡(2π/3)},cn={1,cos⁡(2π/3),cos⁡(2π/3)}s_{n}=\{0, \sin (2 \pi / 3),-\sin (2 \pi / 3)\}, \\ c_{n}=\{1, \cos (2 \pi / 3), \cos (2 \pi / 3)\} sn={0,sin(2π/3),−sin(2π/3)},cn={1,cos(2π/3),cos(2π/3)}
最后对每个点，取其图像坐标满足ui>=0,ui<=umax⁡,vi>=0，且vi<=vmax⁡u_{i}>=0, u_{i}<=u_{\max }, v_{i}>=0，且v_{i}<=v_{\max }ui>=0,ui<=umax,vi>=0，且vi<=vmax的一个作为在三个投影图中最终的所在投影图。
这样就得到了三个点云的2.5D投影图。

投影图分割
使用《Fast segmentation of range imagery into planar regions》中提出的分割合并算法对对投影图进行分割得到连通的近似平面子集。其中分割合并算法由迭代Delaunay三角剖分方法和区域合并组成。得到的表平面分割段用于SLAM系统中的环境表达和轨迹估计：

支撑平面分割段的平面的参数由分割段的支撑点（supporting points）与平面之间的最小二乘拟合来确定。
每个平面分割段都分配有一个支撑点集合的中心原点，以及一个与支撑平面平行的Z轴正方向。支撑平面的x，y轴由支撑点集的协方差矩阵的特征值决定。此外，每个平面分割段还分配有一个参考帧S_F,用于后续的表面分割段匹配以及基于EKF的位姿估计。

1.２特征表达：Representation　of　planar　surface　segments

表平面分割段的表示方法与论文**《Place recognition based on matching of planar surfaces and line segments》**中的相同，即用一系列的2D多边形来表示。每个2D多边形是根据3D支撑平面以及内点、外点的数目来决定的：
(Fn)T⋅Fp=Fρ\left({ }^{F} n\right)^{\mathrm{T}} \cdot{ }^{F} p={ }^{F} \rho (Fn)T⋅Fp=Fρ
上式一共有三项，第一项代表表平面分割段的参考平面S_F的单位法向量。第二项代表 S_F中的任意一点，第三项代表参考平面S_F的原点，

支撑平面的参数是根据三个随机变量构建的扰动向量q=[sx,sy,r]Tq=\left[s_{x}, s_{y}, r\right]^{\mathrm{T}}q=[sx,sy,r]T来描述的，这三个变量描述了真实平面参数与被测平面参数的偏差。理想情况下，被测平面与真实平面相同，真实平面的法向量与S_F的z轴坐标相同，因此Fn=[0,0,1]T,while Fρ=0{ }^{F} n=[0,0,1]^{\mathrm{T}}, \text { while }{ }^{F} \rho=0Fn=[0,0,1]T, while Fρ=0.
在实际情况中，使用上述三个随机变量来表述真实平面的法向量与S_F的z轴之间的偏差。

随机变量r代表真实平面与S_F原点的距离：Fρi,j=r{ }^{F} \rho_{i, j}=rFρi,j=r

真实平面的单位法向量可以表示为：
F=1sx2+sy2+1[sxsy1]TF=\frac{1}{\sqrt{s_{x}^{2}+s_{y}^{2}+1}}\left[\begin{array}{lll}s_{x} & s_{y} & 1\end{array}\right]^{\mathrm{T}}F=sx2+sy2+11[sxsy1]T

论文使用高斯不确定性模型，其中扰动矢量q以0均值和协方差矩阵∑q为正态分布。协方差矩阵Σ是对角矩阵，方差为σsx2,σsy2\sigma_{s x}^{2}, \sigma_{s y}^{2}σsx2,σsy2 and σr2\sigma_{r}^{2}σr2

最终从点云P_L中提取出的表平面分割段F可以表示为：
F=(FR,Ft,Σq,Σp)，前两项是SF与点云位姿PL之间的相对位姿变换，F=\left({ }^{F} R,{ }^{F} t, \quad \Sigma_{q}, \quad \Sigma_{p}\right)，\\ 前两项是S_F与点云位姿P_L之间的相对位姿变换， F=(FR,Ft,Σq,Σp)，前两项是SF与点云位姿PL之间的相对位姿变换，
对于在第j个local map中的第i个平面分割段，记为：
Fi,j=(FRi,j,Fti,j,Σqi,j,Σpi,j)F_{i, j}=\left({ }^{F} R_{i, j},{ }^{F} t_{i, j}, \Sigma_{q_{i, j}}, \Sigma_{p_{i, j}}\right) Fi,j=(FRi,j,Fti,j,Σqi,j,Σpi,j)

有关特征提取和表达部分的实验结果：