基于平面擬合的方法-Ground Plane Fitting
算法思想：一種簡單的處理方法就是沿著x方向（車頭的方向）將空間分割成若干個子平面，然后對每個子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。該方法是在單幀點云中擬合全局平面，在點云數(shù)量較多時效果較好，點云稀疏時極易帶來漏檢和誤檢，比如16線激光雷達。
算法偽代碼：
偽代碼
算法流程是對于給定的點云 P ,分割的最終結(jié)果為兩個點云集合，地面點云 和非地面點云。此算法有四個重要參數(shù)，如下：
Niter : 進行奇異值分解(SVD)的次數(shù)，也即進行優(yōu)化擬合的次數(shù)
NLPR : 用于選取LPR的最低高度點的數(shù)量
Thseed : 用于選取種子點的閾值，當點云內(nèi)的點的高度小于LPR的高度加上此閾值時，我們將該點加入種子點集
Thdist : 平面距離閾值，我們會計算點云中每一個點到我們擬合的平面的正交投影的距離，而這個平面距離閾值，就是用來判定點是否屬于地面
種子點集的選擇
我們首先選取一個種子點集(seed point set)，這些種子點來源于點云中高度（即z值）較小的點，種子點集被用于建立描述地面的初始平面模型，那么如何選取這個種子點集呢？我們引入最低點代表（Lowest Point Representative, LPR）的概念。LPR就是NLPR個最低高度點的平均值，LPR保證了平面擬合階段不受測量噪聲的影響。
種子點的選擇
輸入是一幀點云，這個點云內(nèi)的點已經(jīng)沿著z方向（即高度）做了排序，取 num_lpr_ 個最小點，求得高度平均值 lpr_height（即LPR），選取高度小于 lpr_height + th_seeds_的點作為種子點。
具體代碼實現(xiàn)如下
平面模型
接下來我們建立一個平面模型，點云中的點到這個平面的正交投影距離小于閾值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_) 比較，當高度差小于此閾值，我們認為該點屬于地面，當高度差大于此閾值，則為非地面點。經(jīng)過分類以后的所有地面點被當作下一次迭代的種子點集，迭代優(yōu)化。
基于雷達數(shù)據(jù)本身特點的方法-Ray Ground Filter
代碼
https://github.com/suyunzzz/ray_filter_ground
算法思想
Ray Ground Filter算法的核心是以射線（Ray）的形式來組織點云。將點云的 (x, y, z)三維空間降到（x,y）平面來看，計算每一個點到車輛x正方向的平面夾角 θ, 對360度進行微分，分成若干等份，每一份的角度為0.2度。
激光線束等間隔劃分示意圖（通常以激光雷達角度分辨率劃分）
同一角度范圍內(nèi)激光線束在水平面的投影以及在Z軸方向的高度折線示意圖
為了方便對同一角度的線束進行處理，要將原來直角坐標系的點云轉(zhuǎn)換成柱坐標描述的點云數(shù)據(jù)結(jié)構(gòu)。對同一夾角的線束上的點按照半徑的大小進行排序，通過前后兩點的坡度是否大于我們事先設定的坡度閾值，從而判斷點是否為地面點。
線激光線束縱截面與俯視示意圖（n=4）
通過如下公式轉(zhuǎn)換成柱坐標的形式：
轉(zhuǎn)換成柱坐標的公式
radius表示點到lidar的水平距離(半徑)，theta是點相對于車頭正方向(即x方向)的夾角。對點云進行水平角度微分之后，可得到1800條射線，將這些射線中的點按照距離的遠近進行排序。通過兩個坡度閾值以及當前點的半徑求得高度閾值，通過判斷當前點的高度（即點的z值）是否在地面加減高度閾值范圍內(nèi)來判斷當前點是為地面。
偽代碼
偽代碼
local_max_slope_ ：設定的同條射線上鄰近兩點的坡度閾值。
general_max_slope_ ：整個地面的坡度閾值
遍歷1800條射線，對于每一條射線進行如下操作：
1.計算當前點和上一個點的水平距離pointdistance
2.根據(jù)local_max_slope_和pointdistance計算當前的坡度差閾值height_threshold
3.根據(jù)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，認為是地面點。
基于雷達數(shù)據(jù)本身特點的方法-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]
對于每一條線，對角度進行排序
以當前點p為中心，向左選k個點，向右選k個點
分別計算左邊及右邊k個點與當前點在x和y方向差值的均值
同時計算左邊及右邊k個點的最大z值max1及max2
根據(jù)余弦定理求解余弦角
如果余弦角度滿足閾值且max1減去p.z滿足閾值或max2減去p.z滿足閾值且max2-max1滿足閾值，認為此點為障礙物，否則就認為是地面點。
x_zero_method
X-zero和Z-zero方法可以找到避開測量的X和Z分量的人行道，X-zero和Z-zero方法都考慮了體素的通道數(shù)，因此激光雷達必須與路面平面不平行，這是上述兩種算法以及整個城市道路濾波方法的已知局限性。X-zero方法去除了X方向的值，使用柱坐標代替。
x_zero_method
首先將數(shù)據(jù)組織成[channels][thetas]
對于每一條線，對角度進行排序
以當前點p為中心，向右選第k/2個點p1和第k個點p2
分別計算p及p1、p1及p2、p及p2間z方向的距離
根據(jù)余弦定理求解余弦角
如果余弦角度滿足閾值且p1.z-p.z滿足閾值或p1.z-p2.z滿足閾值且p.z-p2.z滿足閾值，認為此點為障礙物
star_search_method
該方法將點云劃分為矩形段，這些形狀的組合像一顆星；這就是名字的來源，從每個路段提取可能的人行道起點，其中創(chuàng)建的算法對基于Z坐標的高度變化不敏感，這意味著在實踐中，即使當激光雷達相對于路面平面傾斜時，該算法也會表現(xiàn)良好，在柱坐標系中處理點云。
具體實現(xiàn)：
star_search_method
參考文獻
1.https://blog.csdn.net/AdamShan/article/details/84569000
2.https://zhuanlan.zhihu.com/p/553575548
轉(zhuǎn)載請注明出處。
相關文章
熱門資訊
精彩導讀
關注我們
