地面分割--Patchwork

patchwork是一种比较优秀的地面分割方法。其过程主要分为三个部分：同心圆环区域(CZM:concentric Zone Model)，按照区域划分的平面拟合(R-GPF:region-wise ground plane fitting),地面似然估计(GLE:Ground Likelihood estimation)

1问题定义

设所有点云为
P = { p 1 , p 2 , … , p k , … , p N } \mathcal{P}=\left\{\mathbf{p}_1, \mathbf{p}_2, \ldots, \mathbf{p}_k, \ldots, \mathbf{p}_N\right\} P={p1​,p2​,…,pk​,…,pN​}
设地面点为$G$,非地面点为$G^c$,$\mathcal{P}=G\cup G^c$.
设$\hat{G}$为算法估计的地面点，$\widehat{G^c}$为估计的非地面点。所以：
$$\hat{G}=\mathrm{TP}\cup \mathrm{FP}\text{ and }\widehat{G^c}=\mathrm{FN}\cup \mathrm{TN}\text{\hspace{1em}(1)}$$
$\mathrm{TP}:truepositives,\mathrm{FP}:falsepositives,\mathrm{TN}:truenegative,\mathrm{FN}:falsenegative$。并且同样的$\mathcal{P}=\hat{G}\cup \widehat{G^c}$。

2同心区域模型

大部分地面都不是平面的，因此大多数基于平面的估计的方法假设局部地面是平面的。大多数方法将点云按固定角度，沿径向方向划分为均匀网格S（如下图a）。
均匀网格有两个问题：首先在自动驾驶中大部分点云都在lidar附近，如果均匀网格会导致远处的点在网格中过于稀疏（网格中点太少），导致拟合平面失败。其次，离lidar太近的网格，网格太小会导致平面法向量估计失败或者错误。
因此，本文提出了一种非均匀的网格划分方法，记为CZM，简记为$\mathcal{C}$,记所有网格为 ⟨ N ⟩ = { 1 , 2 , 3... N } \lang{N}\rang=\lbrace{1,2,3...N}\rbrace ⟨N⟩={1,2,3...N}(如图b)。然后提出的模型可以表示为：
$$\mathcal{C}=\bigcup _{m\in ⟨N_Z⟩}{Z}_m$$
$Z_m$表示$\mathcal{C}$的第m个区域（zone）.$N_Z$表示区域的数量。
$
\text { Let } Z_m=\left{\mathbf{p}{k} \in \mathcal{P} \mid L{\min , m} \leq \rho_k<L_{\max , m}\right}
$
$L_{\min ,m},L_{\max ,m}$分别表示$Z_m$的最小和最大径向半径。$Z_m$被划分为$N_{r,m}\times N_{\theta ,m}$个bins，并且每个区域的bin数量是不同的(按照离lidar的距离划分不均匀区域)。每个bin${\mathcal{S}}_{i,j,m}$可以表示为：
$$\begin{array}{r}{\mathcal{S}}_{i,j,m}=\{{\mathbf{p}}_k\in Z_m\mid \frac{(i-1)\cdot \Delta L_m}{N_{r,m}}\le {\rho }_k-L_{\min ,m}<\frac{i\cdot \Delta L_m}{N_{r,m}},\\ \frac{(j-1)\cdot 2\pi }{N_{\theta ,m}}-\pi \le {\theta }_k<\frac{j\cdot 2\pi }{N_{\theta ,m}}-\pi \}.\end{array}$$
其中，${\rho }_k=\sqrt{{x_k}^2+{y_k}^2}，{\theta }_k=arctan2(y_k,x_k)$。与均匀网格模型相比，提出的CZM模型能够减少网格数量，并且更好的表示点云。本文设置区域为4（即，图b中4个不同颜色区域）。

3按照区域划分的平面拟合

对每个区域中的bin进行平面拟合。这里采用PCA而不采用RANSAC计算平面的主要参数，是因为PCA速度更快一些。通过PCA得到法向量$n=[a,b,c{]}^T$,平面系数$d=-n\bar{p}$,$\bar{p}$是bin中所有点的坐标平均值。
设$S_n$为第n个bin中点云集合，记所有bin的数量为$N_{\mathcal{C}}=\sum_{m=1}^{N_{Z}} N_{r, m} \times N_{\theta, m} $。然后在每个bin里面选择最低点，作为初始(第一次迭代)种子点，对于$S_n$初始地面点，通过如下方式获取：
G ^ n 0 = { p k ∈ S n ∣ z ( p k ) < z ˉ init  + z seed  } \hat{G}_{n}^{0}=\left\{\mathbf{p}_{k} \in S_{n} \mid z\left(\mathbf{p}_{k}\right)<\bar{z}_{\text {init }}+z_{\text {seed }}\right\} G^n0​={pk​∈Sn​∣z(pk​)<zˉinit ​+zseed ​}
$z_{init}$为种子点的平均高程值，$z_{seed}$为高差阈值。
设${\hat{G}}_n^l$为第l次迭代获取的地面点。${\hat{G}}_n^{l+1}$为第l+1次迭代获取的地面点，${\hat{G}}_n^{l+1}$可以通过以下方式获得：
$$\begin{array}{rl}{\hat{G}}_n^{l+1}& =\left\{{\mathbf{p}}_k\in S_n\mid d_n^l-{\hat{d}}_k<M_d\right\}\end{array}$$
其中，$d_n^l=-{\left({\mathbf{n}}_n^l\right)}^T{\overline{\mathbf{p}}}_n^l$,${\overline{\mathbf{p}}}_n^l$是${\hat{G}}_n^l$的平均值。${\hat{d}}_k=-{\left({\mathbf{n}}_n^l\right)}^T{\mathbf{p}}_k$。
请注意，原始R-GPF[14]和我们的主要区别在于，我们的算法涉及使用自适应初始种子选择来防止R-GPF收敛到局部最小值。（没有理解这个自适应的点在哪）。
为了应对离群点。我们利用了这样一个事实，即仅在Z1（最靠近传感器的那个区域）中的接地点的z值主要分布在−hs附近，其中hs表示传感器高度。因此，当估计${\hat{G}}_n^0$时，如果zk低于Mh·hs，则过滤掉属于Z1的Sn中的pk，其中Mh＜−1是高度阈值。对于不属于Z1的Sn，自适应阈值随着m变大而减小，以避免对可能来自下坡的点（实际上是TP）进行不适当的滤波。

4地面点似然估计（GLE）

$$\mathcal{L}(\theta \mid \mathcal{X})=f(\mathcal{X}\mid \theta )=\prod _{n}f\left({\mathcal{X}}_n\mid {\theta }_n\right)$$
${\mathcal{X}}_n,{\theta }_n$表示每个${\hat{G}}_n$的参数。
$$f\left({\mathcal{X}}_n\mid {\theta }_n\right)\equiv \varphi \left({\mathbf{v}}_{3,n}\right)\cdot \psi \left({\bar{z}}_n,r_n\right)\cdot \phi \left(\psi \left({\bar{z}}_n,r_n\right),{\sigma }_n\right)$$
$\varphi ,\psi ,\phi$表示每个${\hat{G}}_n$对应的垂直度、高程和平面度,${\bar{z}}_n$是高程平均值，$r_n$是lidar原点到$S_n$中心点的距离。${\sigma }_n=\frac{{\lambda }_{3,n}}{{\lambda }_{1,n}+{\lambda }_{2,n}+{\lambda }_{3,n}}$。
垂直度：
$$\varphi \left({\mathbf{v}}_{3,n}\right)=\{\begin{array}{ll}1,& \text{ if }\frac{{\mathbf{v}}_{3,n}\cdot \mathbf{z}}{\Vert {\mathbf{v}}_{3,n}\Vert \Vert \mathbf{z}\Vert }>\cos \left(\frac{\pi }{2}-{\theta }_{\tau }\right)\\ 0,& \text{ otherwise }\end{array}$$
${\mathbf{v}}_{3,n}$表示$S_n$的法向量。$z=[0,0,1]$，${\theta }_{\tau }$为阈值。垂直度表示平面法线与z轴的夹角。
高程
$$\begin{array}{c}\psi \left({\bar{z}}_n,r_n\right)=\{\begin{array}{ll}{\left(1+e^{\left({\bar{z}}_n-\kappa \left(r_n\right)\right)}\right)}^{-1},& \text{ if }{r}_n<L_{\tau }\\ 1.& \text{ otherwise }\end{array}\end{array}$$
$k(.)$表示自适应中点函数，该自适应中点函数根据rn呈指数增加。(文中没有进一步说明)。$L_{\tau }$为距离阈值。
平面度
$$\begin{array}{c}\phi \left(\psi \left({\bar{z}}_n,r_n\right),{\sigma }_n\right)=\{\begin{array}{ll}\zeta e^{-\left({\sigma }_n-{\sigma }_{\tau ,m}\right)},& \text{ if }\psi \left({\bar{z}}_n,r_n\right)<0.5\\ 1,& \text{ otherwise }\end{array}\end{array}$$
$\zeta >1$,${\sigma }_{\tau ,m}$为阈值。
最终的地面点可以通过如下方式得到：
$$\hat{G}=\bigcup _{n\in ⟨N_{\mathcal{C}}⟩}\left[f\left({\mathcal{X}}_n\mid {\theta }_n\right)>0.5\right]{\hat{G}}_n$$
$[.]$表示Iverson bracket，如果满足条件返回true，否则返回false。这个公式的意思就是说，如果局部平面拟合(R-GPF)得到的地面点满足概率大于0.5就认为是真正的地面点。

总结

这篇文章创新之处有三个：改进了bin的划分规则，根据距离lidar距离划分；其次改进了局部平面拟合算法；最后提供基于似然函数的地面点筛选方法（原创性，首次提出）。
缺点：参数过多，有些算法细节和结论有待商榷。
参考：
1