如何利用ransac演算法實現雷射雷達的地面與非地面點雲分割
利用雷射雷達做感知輸出首先要分割出地面點雲以減少對障礙物聚類的影響。本文首先介紹ransac的基本原理,並依據ransac在ros中實現對地面點雲的分割。接著,引入pcl點雲庫,pcl點雲庫中有標準的ransac演算法介面,我們可以通過呼叫它實現更加快速,穩定地濾除地面點雲。
ransac是 random sample consensus的縮寫,翻譯過來是隨機抽樣一致的意思。它表示可以從一組包含「局外點」的資料集中,通過迭代的方式估計某個數學模型的引數。這是一種包含概率統計思想的「不確定演算法」,它的每次迭代並不能保證獲得乙個正確合理的結果,這是帶有概率性的,要想提高獲得這種合理結果的概率,就需要增加迭代次數。
ransac的基本假設
整個資料集中同時包含好的點和不好的點,我們將它們分別稱為局內點和局外點;
資料的分布可以通過某個數學模型來描述,而局內點就是可以適應該模型的點,局外點是不可以適應該模型的點;
隨意給定一組點(可能是局內點也有可能是局外點),我們假設這組點都是局內點,它們滿足某個數學模型,我們利用這個數學模型去估算其他的點,如果有足夠多的點通過計算能被歸類為假設的局內點,那麼這個模型就足夠合理。
ransac的演算法流程
隨機在資料集中選取幾個點假設為局內點,點的個數選擇要依據模型的特徵確定,一般來說選擇適應於模型的最小資料個數。比如說,要在一堆點中擬合一條直線,那就選取兩個點,因為兩點確定一條直線;如果要擬合乙個平面,那你至少需要選取三個點且不共線。
計算適合這幾個點的數學模型;
根據設定的閾值計算資料集中的其他點是否滿足該數學模型,如果滿足,記為該模型的局內點;
記錄下該模型的局內點數量;
重複迭代多次,每次產生的模型要麼因為局內點個數太少而被捨棄,要麼就比現存模型更好而被選用;
滿足迭代退出條件,退出迴圈,得到整個迭代過程中最合理的解。
如何確定迭代的退出條件?
用w表示隨機抽到局內點的概率,用p表示置信度,這個引數表示ransac演算法在執行後提供有用結果的期望概率。n表示計算模型引數需要選取的資料個數,k表示迭代次數,則
**如下:
#include
#include
#include
#include
#include
#include
using namespace std;
intmain()
// 需要至少三個點 才能找到地面
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
auto itr = inliers.
begin()
;//auto 自動型別
x1 = cloud->points[
*itr]
.x; y1 = cloud->points[
*itr]
.y; z1 = cloud->points[
*itr]
.z; itr++
; x2 = cloud->points[
*itr]
.x; y2 = cloud->points[
*itr]
.y; z2 = cloud->points[
*itr]
.z; itr++
; x3 = cloud->points[
*itr]
.x; y3 = cloud->points[
*itr]
.y; z3 = cloud->points[
*itr]
.z;//計算平面係數
float a, b, c, d, sqrt_abc;
a =(y2 - y1)
*(z3 - z1)
-(z2 - z1)
*(y3 - y1)
; b =
(z2 - z1)
*(x3 - x1)
-(x2 - x1)
*(z3 - z1)
; c =
(x2 - x1)
*(y3 - y1)
-(y2 - y1)
*(x3 - x1)
; d =
-(a * x1 + b * y1 + c * z1)
; sqrt_abc =
sqrt
(a * a + b * b + c * c)
;//分別計算這些點到平面的距離
for(
int i =
0; i < num_points; i++
) pcl:
:pointxyz point = cloud->points[i]
;float x = point.x;
float y = point.y;
float z = point.z;
float dist =
fabs
(a * x + b * y + c * z + d)
/ sqrt_abc;
// calculate the distance between other points and plane
float distancetol =
0.3;
if(dist < distancetol)
//將inliersresult 中的內容不斷更新,因為地面的點一定是最多的,所以迭代40次 找到的inliersresult最大時,也就相當於找到了地面
//inliersresult 中儲存的也就是地面上的點雲
if(inliers.
size()
> inliersresult.
size()
)}//cout << inliers.size() << endl;
}//迭代結束後,所有屬於平面上的內點都存放在inliersresult中
//std::unordered_setinliersresult;
cout << inliersresult.
size()
<< endl;
//1633
//建立兩片點雲,一片存放地面,一片存放其他點
pcl:
:pointcloud
:pointxyz>
::ptr out_plane
(new pcl:
:pointcloud
:pointxyz>);
pcl:
:pointcloud
:pointxyz>
::ptr in_plane
(new pcl:
:pointcloud
:pointxyz>);
for(
int i =
0; i < num_points; i++
)else
}/*pcl::pcdwriter writer;
writer.write("c:\\users\\administrator\\downloads\\求助\\求助\\tree-2-rend.pcd", *cloud_filtered1);*/
pcl:
:visualization:
:pclvisualizer:
:ptr viewer
(new pcl:
:visualization:
:pclvisualizer
("顯示視窗"))
;//視窗顯示點雲
viewer->
addpointcloud
(in_plane,
"*cloud");
viewer->
resetcamera()
;//相機點重置
viewer->
spin()
;system
("pause");
return(0);}
參考: RANSAC 隨機取樣一致性演算法
ransac範例的正式描述如下 首先,要給定 1乙個模型,該模型需要最少n個資料點去例項化它的自由引數 2一組資料點p,p中包含資料點的數量 p 大於n。然後,從p中隨機地選擇n個點 組成p的乙個子集s1 並例項化這個模型 構造成m1 接下來,利用例項化的模型m1去測定p中點的某個子集s1 這些點相...
隨機取樣一致性演算法RANSAC
作用1 點雲分割通過 空間平面 直線 二維或三維圓 圓球 錐體等模型 進行分割。作用2 點雲的配準對的剔除 例子 五個點雲,三個配準,利用ransac可以剔除另外兩個 演算法簡介 從樣本中抽取乙個子集,通過該演算法最小方差對這個樣本計算出這個該模型的引數 例如模型是一條直線,那麼計算該樣本所有的點到...
隨機取樣演算法
最近學習程式設計珠璣,看到隨機取樣演算法,很不錯,這裡也整理下 首先來看乙個簡單的取樣演算法,這裡假定在不考慮重複的情況下,從1 n之間取出m個數來 void rand int m,int n 這樣子的話,很容易得到如下的輸出 這樣子的話,倒是適合 石頭 剪刀 布 一類的小遊戲 但很多情況下,我們需...