基于平面擬合的方法-Ground Plane Fitting

算法思想：一種簡(jiǎn)單的處理方法就是沿著x方向（車頭的方向）將空間分割成若干個(gè)子平面，然后對(duì)每個(gè)子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。該方法是在單幀點(diǎn)云中擬合全局平面，在點(diǎn)云數(shù)量較多時(shí)效果較好，點(diǎn)云稀疏時(shí)極易帶來漏檢和誤檢，比如16線激光雷達(dá)。

算法偽代碼：

偽代碼

算法流程是對(duì)于給定的點(diǎn)云 P ,分割的最終結(jié)果為兩個(gè)點(diǎn)云集合，地面點(diǎn)云 和非地面點(diǎn)云。此算法有四個(gè)重要參數(shù)，如下：

Niter : 進(jìn)行奇異值分解(SVD)的次數(shù)，也即進(jìn)行優(yōu)化擬合的次數(shù)

NLPR : 用于選取LPR的最低高度點(diǎn)的數(shù)量

Thseed : 用于選取種子點(diǎn)的閾值，當(dāng)點(diǎn)云內(nèi)的點(diǎn)的高度小于LPR的高度加上此閾值時(shí)，我們將該點(diǎn)加入種子點(diǎn)集

Thdist : 平面距離閾值，我們會(huì)計(jì)算點(diǎn)云中每一個(gè)點(diǎn)到我們擬合的平面的正交投影的距離，而這個(gè)平面距離閾值，就是用來判定點(diǎn)是否屬于地面

種子點(diǎn)集的選擇

我們首先選取一個(gè)種子點(diǎn)集(seed point set)，這些種子點(diǎn)來源于點(diǎn)云中高度（即z值）較小的點(diǎn)，種子點(diǎn)集被用于建立描述地面的初始平面模型，那么如何選取這個(gè)種子點(diǎn)集呢？我們引入最低點(diǎn)代表（Lowest Point Representative, LPR）的概念。LPR就是NLPR個(gè)最低高度點(diǎn)的平均值，LPR保證了平面擬合階段不受測(cè)量噪聲的影響。

種子點(diǎn)的選擇

輸入是一幀點(diǎn)云，這個(gè)點(diǎn)云內(nèi)的點(diǎn)已經(jīng)沿著z方向（即高度）做了排序，取 num_lpr_ 個(gè)最小點(diǎn)，求得高度平均值 lpr_height（即LPR），選取高度小于 lpr_height + th_seeds_的點(diǎn)作為種子點(diǎn)。

具體代碼實(shí)現(xiàn)如下

平面模型

接下來我們建立一個(gè)平面模型，點(diǎn)云中的點(diǎn)到這個(gè)平面的正交投影距離小于閾值Thdist，則認(rèn)為該點(diǎn)屬于地面，否則屬于非地面。采用一個(gè)簡(jiǎn)單的線性模型用于平面模型估計(jì)，如下：

ax+by+cz+d=0

即：

其中

,通過初始點(diǎn)集的協(xié)方差矩陣C來求解n，從而確定一個(gè)平面，種子點(diǎn)集作為初始點(diǎn)集，其協(xié)方差矩陣為

這個(gè)協(xié)方差矩陣 C 描述了種子點(diǎn)集的散布情況，其三個(gè)奇異向量可以通過奇異值分解（SVD）求得，這三個(gè)奇異向量描述了點(diǎn)集在三個(gè)主要方向的散布情況。由于是平面模型，垂直于平面的法向量 n 表示具有最小方差的方向，可以通過計(jì)算具有最小奇異值的奇異向量來求得。

那么在求得了 n 以后， d 可以通過代入種子點(diǎn)集的平均值  ,s(它代表屬于地面的點(diǎn)) 直接求得。整個(gè)平面模型計(jì)算代碼如下：

優(yōu)化平面主循環(huán)

得到這個(gè)初始的平面模型以后，我們會(huì)計(jì)算點(diǎn)云中每一個(gè)點(diǎn)到該平面的正交投影的距離，即 points * normal_，并且將這個(gè)距離與設(shè)定的閾值(即th_dist_d_) 比較，當(dāng)高度差小于此閾值，我們認(rèn)為該點(diǎn)屬于地面，當(dāng)高度差大于此閾值，則為非地面點(diǎn)。經(jīng)過分類以后的所有地面點(diǎn)被當(dāng)作下一次迭代的種子點(diǎn)集，迭代優(yōu)化。

基于雷達(dá)數(shù)據(jù)本身特點(diǎn)的方法-Ray Ground Filter

代碼

https://github.com/suyunzzz/ray_filter_ground

算法思想

Ray Ground Filter算法的核心是以射線（Ray）的形式來組織點(diǎn)云。將點(diǎn)云的 (x, y, z)三維空間降到（x,y）平面來看，計(jì)算每一個(gè)點(diǎn)到車輛x正方向的平面夾角 θ, 對(duì)360度進(jìn)行微分，分成若干等份，每一份的角度為0.2度。

激光線束等間隔劃分示意圖（通常以激光雷達(dá)角度分辨率劃分）

同一角度范圍內(nèi)激光線束在水平面的投影以及在Z軸方向的高度折線示意圖

為了方便對(duì)同一角度的線束進(jìn)行處理，要將原來直角坐標(biāo)系的點(diǎn)云轉(zhuǎn)換成柱坐標(biāo)描述的點(diǎn)云數(shù)據(jù)結(jié)構(gòu)。對(duì)同一夾角的線束上的點(diǎn)按照半徑的大小進(jìn)行排序，通過前后兩點(diǎn)的坡度是否大于我們事先設(shè)定的坡度閾值，從而判斷點(diǎn)是否為地面點(diǎn)。

線激光線束縱截面與俯視示意圖（n=4）

通過如下公式轉(zhuǎn)換成柱坐標(biāo)的形式：

轉(zhuǎn)換成柱坐標(biāo)的公式

radius表示點(diǎn)到lidar的水平距離(半徑)，theta是點(diǎn)相對(duì)于車頭正方向(即x方向)的夾角。對(duì)點(diǎn)云進(jìn)行水平角度微分之后，可得到1800條射線，將這些射線中的點(diǎn)按照距離的遠(yuǎn)近進(jìn)行排序。通過兩個(gè)坡度閾值以及當(dāng)前點(diǎn)的半徑求得高度閾值，通過判斷當(dāng)前點(diǎn)的高度（即點(diǎn)的z值）是否在地面加減高度閾值范圍內(nèi)來判斷當(dāng)前點(diǎn)是為地面。

偽代碼

偽代碼

local_max_slope_ ：設(shè)定的同條射線上鄰近兩點(diǎn)的坡度閾值。

general_max_slope_ ：整個(gè)地面的坡度閾值

遍歷1800條射線，對(duì)于每一條射線進(jìn)行如下操作：

1.計(jì)算當(dāng)前點(diǎn)和上一個(gè)點(diǎn)的水平距離pointdistance

2.根據(jù)local_max_slope_和pointdistance計(jì)算當(dāng)前的坡度差閾值height_threshold

3.根據(jù)general_max_slope_和當(dāng)前點(diǎn)的水平距離計(jì)算整個(gè)地面的高度差閾值general_height_threshold

4.若當(dāng)前點(diǎn)的z坐標(biāo)小于前一個(gè)點(diǎn)的z坐標(biāo)加height_threshold并大于前一個(gè)點(diǎn)的z坐標(biāo)減去height_threshold：

5.若當(dāng)前點(diǎn)z坐標(biāo)小于雷達(dá)安裝高度減去general_height_threshold并且大于相加，認(rèn)為是地面點(diǎn)

6.否則：是非地面點(diǎn)。

7.若pointdistance滿足閾值并且前點(diǎn)的z坐標(biāo)小于雷達(dá)安裝高度減去height_threshold并大于雷達(dá)安裝高度加上height_threshold，認(rèn)為是地面點(diǎn)。

基于雷達(dá)數(shù)據(jù)本身特點(diǎn)的方法-urban road filter

原文?

Real-Time LIDAR-based Urban Road and Sidewalk Detection for Autonomous Vehicles

代碼?

https://github.com/jkk-research/urban_road_filter

z_zero_method

z_zero_method

首先將數(shù)據(jù)組織成[channels][thetas]

對(duì)于每一條線，對(duì)角度進(jìn)行排序

以當(dāng)前點(diǎn)p為中心，向左選k個(gè)點(diǎn)，向右選k個(gè)點(diǎn)

分別計(jì)算左邊及右邊k個(gè)點(diǎn)與當(dāng)前點(diǎn)在x和y方向差值的均值

同時(shí)計(jì)算左邊及右邊k個(gè)點(diǎn)的最大z值max1及max2

根據(jù)余弦定理求解余弦角

如果余弦角度滿足閾值且max1減去p.z滿足閾值或max2減去p.z滿足閾值且max2-max1滿足閾值，認(rèn)為此點(diǎn)為障礙物，否則就認(rèn)為是地面點(diǎn)。

x_zero_method

X-zero和Z-zero方法可以找到避開測(cè)量的X和Z分量的人行道，X-zero和Z-zero方法都考慮了體素的通道數(shù)，因此激光雷達(dá)必須與路面平面不平行，這是上述兩種算法以及整個(gè)城市道路濾波方法的已知局限性。X-zero方法去除了X方向的值，使用柱坐標(biāo)代替。

x_zero_method

首先將數(shù)據(jù)組織成[channels][thetas]

對(duì)于每一條線，對(duì)角度進(jìn)行排序

以當(dāng)前點(diǎn)p為中心，向右選第k/2個(gè)點(diǎn)p1和第k個(gè)點(diǎn)p2

分別計(jì)算p及p1、p1及p2、p及p2間z方向的距離

根據(jù)余弦定理求解余弦角

如果余弦角度滿足閾值且p1.z-p.z滿足閾值或p1.z-p2.z滿足閾值且p.z-p2.z滿足閾值，認(rèn)為此點(diǎn)為障礙物

star_search_method

該方法將點(diǎn)云劃分為矩形段，這些形狀的組合像一顆星；這就是名字的來源，從每個(gè)路段提取可能的人行道起點(diǎn)，其中創(chuàng)建的算法對(duì)基于Z坐標(biāo)的高度變化不敏感，這意味著在實(shí)踐中，即使當(dāng)激光雷達(dá)相對(duì)于路面平面傾斜時(shí)，該算法也會(huì)表現(xiàn)良好，在柱坐標(biāo)系中處理點(diǎn)云。

具體實(shí)現(xiàn)：

star_search_method

參考文獻(xiàn)

1.https://blog.csdn.net/AdamShan/article/details/84569000

2.https://zhuanlan.zhihu.com/p/553575548