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

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

算法偽代碼：

偽代碼

算法流程是對于給定的點云 P ,分割的最終結果為兩個點云集合，地面點云 和非地面點云。此算法有四個重要參數，如下：

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

NLPR : 用于選取LPR的最低高度點的數量

Thseed : 用于選取種子點的閾值，當點云內的點的高度小于LPR的高度加上此閾值時，我們將該點加入種子點集

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

種子點集的選擇

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

種子點的選擇

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

具體代碼實現如下

平面模型

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

ax+by+cz+d=0

即：

其中

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

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

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

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

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

基于雷達數據本身特點的方法-Ray Ground Filter

代碼

https://github.com/suyunzzz/ray_filter_ground

算法思想

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

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

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

為了方便對同一角度的線束進行處理，要將原來直角坐標系的點云轉換成柱坐標描述的點云數據結構。對同一夾角的線束上的點按照半徑的大小進行排序，通過前后兩點的坡度是否大于我們事先設定的坡度閾值，從而判斷點是否為地面點。

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

通過如下公式轉換成柱坐標的形式：

轉換成柱坐標的公式

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

偽代碼

偽代碼

local_max_slope_ ：設定的同條射線上鄰近兩點的坡度閾值。

general_max_slope_ ：整個地面的坡度閾值

遍歷1800條射線，對于每一條射線進行如下操作：

1.計算當前點和上一個點的水平距離pointdistance

2.根據local_max_slope_和pointdistance計算當前的坡度差閾值height_threshold

3.根據general_max_slope_和當前點的水平距離計算整個地面的高度差閾值general_height_threshold

4.若當前點的z坐標小于前一個點的z坐標加height_threshold并大于前一個點的z坐標減去height_threshold：

5.若當前點z坐標小于雷達安裝高度減去general_height_threshold并且大于相加，認為是地面點

6.否則：是非地面點。

7.若pointdistance滿足閾值并且前點的z坐標小于雷達安裝高度減去height_threshold并大于雷達安裝高度加上height_threshold，認為是地面點。

基于雷達數據本身特點的方法-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

首先將數據組織成[channels][thetas]

對于每一條線，對角度進行排序

以當前點p為中心，向左選k個點，向右選k個點

分別計算左邊及右邊k個點與當前點在x和y方向差值的均值

同時計算左邊及右邊k個點的最大z值max1及max2

根據余弦定理求解余弦角

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

x_zero_method

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

x_zero_method

首先將數據組織成[channels][thetas]

對于每一條線，對角度進行排序

以當前點p為中心，向右選第k/2個點p1和第k個點p2

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

根據余弦定理求解余弦角

如果余弦角度滿足閾值且p1.z-p.z滿足閾值或p1.z-p2.z滿足閾值且p.z-p2.z滿足閾值，認為此點為障礙物

star_search_method

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

具體實現：

star_search_method

參考文獻

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

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