设所有点云为
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
G,非地面点为
G
c
G^c
Gc,
P
=
G
∪
G
c
\mathcal{P}=G\cup{G^c}
P=G∪Gc.
设
G
^
\hat{G}
G^为算法估计的地面点，
G
c
^
\hat{G^c}
Gc^为估计的非地面点。所以：
G
^
=
T
P
∪
F
P
and
G
c
^
=
F
N
∪
T
N
(1)
\hat{G}=\mathrm{TP} \cup \mathrm{FP} \text { and } \hat{G^c}=\mathrm{FN} \cup \mathrm{TN}\tag{1}
G^=TP∪FP and Gc^=FN∪TN(1)
T
P
:
t
r
u
e
p
o
s
i
t
i
v
e
s
,
F
P
:
f
a
l
s
e
p
o
s
i
t
i
v
e
s
,
T
N
:
t
r
u
e
n
e
g
a
t
i
v
e
,
F
N
:
f
a
l
s
e
n
e
g
a
t
i
v
e
\mathrm{TP}:true positives,\mathrm{FP}:false positives,\mathrm{TN}:true negative,\mathrm{FN}:false negative
TP:truepositives,FP:falsepositives,TN:truenegative,FN:falsenegative。并且同样的
P
=
G
^
∪
G
c
^
\mathcal{P}=\hat{G}\cup{\hat{G^c}}
P=G^∪Gc^。
大部分地面都不是平面的，因此大多数基于平面的估计的方法假设局部地面是平面的。大多数方法将点云按固定角度，沿径向方向划分为均匀网格S（如下图a）。
均匀网格有两个问题：首先在自动驾驶中大部分点云都在lidar附近，如果均匀网格会导致远处的点在网格中过于稀疏（网格中点太少），导致拟合平面失败。其次，离lidar太近的网格，网格太小会导致平面法向量估计失败或者错误。
因此，本文提出了一种非均匀的网格划分方法，记为CZM，简记为
C
\mathcal{C}
C,记所有网格为
⟨
N
⟩
=
{
1
,
2
,
3...
N
}
\lang{N}\rang=\lbrace{1,2,3...N}\rbrace
⟨N⟩={1,2,3...N}(如图b)。然后提出的模型可以表示为：
C
=
⋃
m
∈
⟨
N
Z
⟩
Z
m
\mathcal{C}=\bigcup_{m \in\left\langle N_Z\right\rangle} Z_m
C=m∈⟨NZ⟩⋃Zm
Z
m
Z_m
Zm表示
C
\mathcal{C}
C的第m个区域（zone）.
N
Z
N_Z
NZ表示区域的数量。
$
\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
L_{\min , m},L_{\max , m}
Lmin,m,Lmax,m分别表示
Z
m
Z_m
Zm的最小和最大径向半径。
Z
m
Z_m
Zm被划分为
N
r
,
m
×
N
θ
,
m
N_{r,m}\times N_{\theta,m}
Nr,m×Nθ,m个bins，并且每个区域的bin数量是不同的(按照离lidar的距离划分不均匀区域)。每个bin
S
i
,
j
,
m
\mathcal{S}_{i, j, m}
Si,j,m可以表示为：
S
i
,
j
,
m
=
{
p
k
∈
Z
m
∣
(
i
−
1
)
⋅
Δ
L
m
N
r
,
m
≤
ρ
k
−
L
min
,
m
<
i
⋅
Δ
L
m
N
r
,
m
,
(
j
−
1
)
⋅
2
π
N
θ
,
m
−
π
≤
θ
k
<
j
⋅
2
π
N
θ
,
m
−
π
}
.
\begin{array}{r} \mathcal{S}_{i, j, m}=\left\{\mathbf{p}_k \in Z_m \mid \frac{(i-1) \cdot \Delta L_m}{N_{r, m}} \leq \rho_k-L_{\min , m}<\frac{i \cdot \Delta L_m}{N_{r, m}},\right. \\ \left.\frac{(j-1) \cdot 2 \pi}{N_{\theta, m}}-\pi \leq \theta_k<\frac{j \cdot 2 \pi}{N_{\theta, m}}-\pi\right\} . \end{array}
Si,j,m={pk∈Zm∣Nr,m(i−1)⋅ΔLm≤ρk−Lmin,m<Nr,mi⋅ΔLm,Nθ,m(j−1)⋅2π−π≤θk<Nθ,mj⋅2π−π}.
其中，
ρ
k
=
x
k
2
+
y
k
2
，
θ
k
=
a
r
c
t
a
n
2
(
y
k
,
x
k
)
\rho_k=\sqrt{{x_k}^2+{y_k}^2}，\theta_k=arctan2({y_k,x_k})
ρk=xk2+yk2
，θk=arctan2(yk,xk)。与均匀网格模型相比，提出的CZM模型能够减少网格数量，并且更好的表示点云。本文设置区域为4（即，图b中4个不同颜色区域）。
对每个区域中的bin进行平面拟合。这里采用PCA而不采用RANSAC计算平面的主要参数，是因为PCA速度更快一些。通过PCA得到法向量
n
=
[
a
,
b
,
c
]
T
n=[a,b,c]^T
n=[a,b,c]T,平面系数
d
=
−
n
p
ˉ
d=-n\bar{p}
d=−npˉ,
p
ˉ
\bar{p}
pˉ是bin中所有点的坐标平均值。
设
S
n
S_n
Sn为第n个bin中点云集合，记所有bin的数量为$N_{\mathcal{C}}=\sum_{m=1}^{N_{Z}} N_{r, m} \times N_{\theta, m}
。然后在每个
b
i
n
里面选择最低点，作为初始
(
第一次迭代
)
种子点，对于
。然后在每个bin里面选择最低点，作为初始(第一次迭代)种子点，对于
。然后在每个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
i
n
i
t
z_{init}
zinit为种子点的平均高程值，
z
s
e
e
d
z_{seed}
zseed为高差阈值。
设
G
^
n
l
\hat{G}_{n}^{l}
G^nl为第l次迭代获取的地面点。
G
^
n
l
+
1
\hat{G}_{n}^{l+1}
G^nl+1为第l+1次迭代获取的地面点，
G
^
n
l
+
1
\hat{G}_{n}^{l+1}
G^nl+1可以通过以下方式获得：
G
^
n
l
+
1
=
{
p
k
∈
S
n
∣
d
n
l
−
d
^
k
<
M
d
}
\begin{align} \hat{G}_{n}^{l+1} & = \left\{\mathbf{p}_{k} \in S_{n} \mid d_{n}^{l}-\hat{d}_{k}<M_{d}\right\} \end{align}
G^nl+1={pk∈Sn∣dnl−d^k<Md}
其中，
d
n
l
=
−
(
n
n
l
)
T
p
‾
n
l
d_{n}^{l}=-\left(\mathbf{n}_{n}^{l}\right)^{T} \overline{\mathbf{p}}_{n}^{l}
dnl=−(nnl)Tpnl,
p
‾
n
l
\overline{\mathbf{p}}_{n}^{l}
pnl是
G
^
n
l
\hat{G}_{n}^{l}
G^nl的平均值。
d
^
k
=
−
(
n
n
l
)
T
p
k
\hat{d}_{k}=-\left(\mathbf{n}_{n}^{l}\right)^{T} \mathbf{p}_{k}
d^k=−(nnl)Tpk。
请注意，原始R-GPF[14]和我们的主要区别在于，我们的算法涉及使用自适应初始种子选择来防止R-GPF收敛到局部最小值。（没有理解这个自适应的点在哪）。
为了应对离群点。我们利用了这样一个事实，即仅在Z1（最靠近传感器的那个区域）中的接地点的z值主要分布在−hs附近，其中hs表示传感器高度。因此，当估计
G
^
n
0
\hat{G}_{n}^{0}
G^n0时，如果zk低于Mh·hs，则过滤掉属于Z1的Sn中的pk，其中Mh＜−1是高度阈值。对于不属于Z1的Sn，自适应阈值随着m变大而减小，以避免对可能来自下坡的点（实际上是TP）进行不适当的滤波。
L
(
θ
∣
X
)
=
f
(
X
∣
θ
)
=
∏
n
f
(
X
n
∣
θ
n
)
\mathcal{L}(\theta \mid \mathcal{X})=f(\mathcal{X} \mid \theta)=\prod_{n} f\left(\mathcal{X}_{n} \mid \theta_{n}\right)
L(θ∣X)=f(X∣θ)=n∏f(Xn∣θn)
X
n
,
θ
n
\mathcal{X}_{n}, \theta_{n}
Xn,θn表示每个
G
^
n
\hat{G}_{n}
G^n的参数。
f
(
X
n
∣
θ
n
)
≡
ϕ
(
v
3
,
n
)
⋅
ψ
(
z
ˉ
n
,
r
n
)
⋅
φ
(
ψ
(
z
ˉ
n
,
r
n
)
,
σ
n
)
f\left(\mathcal{X}_{n} \mid \theta_{n}\right) \equiv \phi\left(\mathbf{v}_{3, n}\right) \cdot \psi\left(\bar{z}_{n}, r_{n}\right) \cdot \varphi\left(\psi\left(\bar{z}_{n}, r_{n}\right), \sigma_{n}\right)
f(Xn∣θn)≡ϕ(v3,n)⋅ψ(zˉn,rn)⋅φ(ψ(zˉn,rn),σn)
ϕ
,
ψ
,
φ
\phi, \psi,\varphi
ϕ,ψ,φ表示每个
G
^
n
\hat{G}_{n}
G^n对应的垂直度、高程和平面度,
z
ˉ
n
\bar{z}_{n}
zˉn是高程平均值，
r
n
r_{n}
rn是lidar原点到
S
n
S_n
Sn中心点的距离。
σ
n
=
λ
3
,
n
λ
1
,
n
+
λ
2
,
n
+
λ
3
,
n
\sigma_n=\frac{\lambda_{3, n}}{\lambda_{1, n}+\lambda_{2, n}+\lambda_{3, n}}
σn=λ1,n+λ2,n+λ3,nλ3,n。
垂直度：
ϕ
(
v
3
,
n
)
=
{
1
,
if
v
3
,
n
⋅
z
∥
v
3
,
n
∥
∥
z
∥
>
cos
(
π
2
−
θ
τ
)
0
,
otherwise
\phi\left(\mathbf{v}_{3, n}\right)=\left\{\begin{array}{ll} 1, & \text { if } \frac{\mathbf{v}_{3, n} \cdot \mathbf{z}}{\left\|\mathbf{v}_{3, n}\right\|\|\mathbf{z}\|}>\cos \left(\frac{\pi}{2}-\theta_{\tau}\right) \\ 0, & \text { otherwise } \end{array}\right.
ϕ(v3,n)={1,0, if ∥v3,n∥∥z∥v3,n⋅z>cos(2π−θτ) otherwise
v
3
,
n
\mathbf{v}_{3, n}
v3,n表示
S
n
{S}_{n}
Sn的法向量。
z
=
[
0
,
0
,
1
]
z=[0,0,1]
z=[0,0,1]，
θ
τ
\theta_{\tau}
θτ为阈值。垂直度表示平面法线与z轴的夹角。
高程
ψ
(
z
ˉ
n
,
r
n
)
=
{
(
1
+
e
(
z
ˉ
n
−
κ
(
r
n
)
)
)
−
1
,
if
r
n
<
L
τ
1.
otherwise
\begin{equation}\psi\left(\bar{z}_n, r_n\right)= \begin{cases}\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{cases}\end{equation}
ψ(zˉn,rn)={(1+e(zˉn−κ(rn)))−1,1. if rn<Lτ otherwise
k
(
.
)
k(.)
k(.)表示自适应中点函数，该自适应中点函数根据rn呈指数增加。(文中没有进一步说明)。
L
τ
L_\tau
Lτ为距离阈值。
平面度
φ
(
ψ
(
z
ˉ
n
,
r
n
)
,
σ
n
)
=
{
ζ
e
−
(
σ
n
−
σ
τ
,
m
)
,
if
ψ
(
z
ˉ
n
,
r
n
)
<
0.5
1
,
otherwise
\begin{equation}\varphi\left(\psi\left(\bar{z}_n, r_n\right), \sigma_n\right)= \begin{cases}\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{cases}\end{equation}
φ(ψ(zˉn,rn),σn)={ζe−(σn−στ,m),1, if ψ(zˉn,rn)<0.5 otherwise
ζ
>
1
\zeta>1
ζ>1,
σ
τ
,
m
\sigma_{\tau, m}
στ,m为阈值。
最终的地面点可以通过如下方式得到：
G
^
=
⋃
n
∈
⟨
N
C
⟩
[
f
(
X
n
∣
θ
n
)
>
0.5
]
G
^
n
\hat{G}=\bigcup_{n \in\left\langle N_{\mathcal{C}}\right\rangle}\left[f\left(\mathcal{X}_n \mid \theta_n\right)>0.5\right] \hat{G}_n
G^=n∈⟨NC⟩⋃[f(Xn∣θn)>0.5]G^n
[
.
]
[.]
[.]表示Iverson bracket，如果满足条件返回true，否则返回false。这个公式的意思就是说，如果局部平面拟合(R-GPF)得到的地面点满足概率大于0.5就认为是真正的地面点。
这篇文章创新之处有三个：改进了bin的划分规则，根据距离lidar距离划分；其次改进了局部平面拟合算法；最后提供基于似然函数的地面点筛选方法（原创性，首次提出）。
缺点：参数过多，有些算法细节和结论有待商榷。
参考：
1
