基於(yu) 平麵擬合的方法-Ground Plane Fitting

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

算法偽(wei) 代碼：

偽(wei) 代碼

算法流程是對於(yu) 給定的點雲(yun)  P ,分割的最終結果為(wei) 兩(liang) 個(ge) 點雲(yun) 集合，地麵點雲(yun)  和非地麵點雲(yun) 。此算法有四個(ge) 重要參數，如下：

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

NLPR : 用於(yu) 選取LPR的最低高度點的數量

Thseed : 用於(yu) 選取種子點的閾值，當點雲(yun) 內(nei) 的點的高度小於(yu) LPR的高度加上此閾值時，我們(men) 將該點加入種子點集

Thdist : 平麵距離閾值，我們(men) 會(hui) 計算點雲(yun) 中每一個(ge) 點到我們(men) 擬合的平麵的正交投影的距離，而這個(ge) 平麵距離閾值，就是用來判定點是否屬於(yu) 地麵

種子點集的選擇

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

種子點的選擇

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

具體(ti) 代碼實現如下

平麵模型

接下來我們(men) 建立一個(ge) 平麵模型，點雲(yun) 中的點到這個(ge) 平麵的正交投影距離小於(yu) 閾值Thdist，則認為(wei) 該點屬於(yu) 地麵，否則屬於(yu) 非地麵。采用一個(ge) 簡單的線性模型用於(yu) 平麵模型估計，如下：

ax+by+cz+d=0

即：

其中

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

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

那麽(me) 在求得了 n 以後， d 可以通過代入種子點集的平均值  ,s(它代表屬於(yu) 地麵的點) 直接求得。整個(ge) 平麵模型計算代碼如下：

優(you) 化平麵主循環

得到這個(ge) 初始的平麵模型以後，我們(men) 會(hui) 計算點雲(yun) 中每一個(ge) 點到該平麵的正交投影的距離，即 points * normal_，並且將這個(ge) 距離與(yu) 設定的閾值(即th_dist_d_) 比較，當高度差小於(yu) 此閾值，我們(men) 認為(wei) 該點屬於(yu) 地麵，當高度差大於(yu) 此閾值，則為(wei) 非地麵點。經過分類以後的所有地麵點被當作下一次迭代的種子點集，迭代優(you) 化。

基於(yu) 雷達數據本身特點的方法-Ray Ground Filter

代碼

https://github.com/suyunzzz/ray_filter_ground

算法思想

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

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

同一角度範圍內(nei) 激光線束在水平麵的投影以及在Z軸方向的高度折線示意圖

為(wei) 了方便對同一角度的線束進行處理，要將原來直角坐標係的點雲(yun) 轉換成柱坐標描述的點雲(yun) 數據結構。對同一夾角的線束上的點按照半徑的大小進行排序，通過前後兩(liang) 點的坡度是否大於(yu) 我們(men) 事先設定的坡度閾值，從(cong) 而判斷點是否為(wei) 地麵點。

線激光線束縱截麵與(yu) 俯視示意圖（n=4）

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

轉換成柱坐標的公式

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

偽(wei) 代碼

偽(wei) 代碼

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

general_max_slope_ ：整個(ge) 地麵的坡度閾值

遍曆1800條射線，對於(yu) 每一條射線進行如下操作：

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

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

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

4.若當前點的z坐標小於(yu) 前一個(ge) 點的z坐標加height_threshold並大於(yu) 前一個(ge) 點的z坐標減去height_threshold：

5.若當前點z坐標小於(yu) 雷達安裝高度減去general_height_threshold並且大於(yu) 相加，認為(wei) 是地麵點

6.否則：是非地麵點。

7.若pointdistance滿足閾值並且前點的z坐標小於(yu) 雷達安裝高度減去height_threshold並大於(yu) 雷達安裝高度加上height_threshold，認為(wei) 是地麵點。

基於(yu) 雷達數據本身特點的方法-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]

對於(yu) 每一條線，對角度進行排序

以當前點p為(wei) 中心，向左選k個(ge) 點，向右選k個(ge) 點

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

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

根據餘(yu) 弦定理求解餘(yu) 弦角

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

x_zero_method

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

x_zero_method

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

對於(yu) 每一條線，對角度進行排序

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

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

根據餘(yu) 弦定理求解餘(yu) 弦角

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

star_search_method

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

具體(ti) 實現：

star_search_method

參考文獻

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

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