2023-04-13 10:07:10來(lái)源：智駕最前沿
目前常見(jiàn)的激光點(diǎn)云分割算法有基于平面擬合的方法和基于激光點(diǎn)云數(shù)據(jù)特點(diǎn)的方法兩類(lèi)。具體如下：
點(diǎn)云地面分割算法
(相關(guān)資料圖)
算法思想：一種簡(jiǎn)單的處理方法就是沿著x方向（車(chē)頭的方向）將空間分割成若干個(gè)子平面，然后對(duì)每個(gè)子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。該方法是在單幀點(diǎn)云中擬合全局平面，在點(diǎn)云數(shù)量較多時(shí)效果較好，點(diǎn)云稀疏時(shí)極易帶來(lái)漏檢和誤檢，比如16線(xiàn)激光雷達(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è)平面距離閾值，就是用來(lái)判定點(diǎn)是否屬于地面種子點(diǎn)集的選擇我們首先選取一個(gè)種子點(diǎn)集(seed point set)，這些種子點(diǎn)來(lái)源于點(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)如下
/* @brief Extract initial seeds of the given pointcloud sorted segment. This function filter ground seeds points accoring to heigt. This function will set the `g_ground_pc` to `g_seed_pc`. @param p_sorted: sorted pointcloud @param ::num_lpr_: num of LPR points @param ::th_seeds_: threshold distance of seeds @param ::*/void PlaneGroundFilter::extract_initial_seeds_(const pcl::PointCloud平面模型&p_sorted){ // LPR is the mean of low point representative double sum = 0; int cnt = 0; // Calculate the mean height value. for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++) { sum += p_sorted.points[i].z; cnt++; } double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0 g_seeds_pc->clear(); // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ for (int i = 0; i < p_sorted.points.size(); i++) { if (p_sorted.points[i].z < lpr_height + th_seeds_) { g_seeds_pc->points.push_back(p_sorted.points[i]); } } // return seeds points}
接下來(lái)我們建立一個(gè)平面模型，點(diǎn)云中的點(diǎn)到這個(gè)平面的正交投影距離小于閾值Thdist，則認(rèn)為該點(diǎn)屬于地面，否則屬于非地面。采用一個(gè)簡(jiǎn)單的線(xiàn)性模型用于平面模型估計(jì)，如下：
ax+by+cz+d=0
即：
其中
,通過(guò)初始點(diǎn)集的協(xié)方差矩陣C來(lái)求解n，從而確定一個(gè)平面，種子點(diǎn)集作為初始點(diǎn)集，其協(xié)方差矩陣為
這個(gè)協(xié)方差矩陣 C 描述了種子點(diǎn)集的散布情況，其三個(gè)奇異向量可以通過(guò)奇異值分解（SVD）求得，這三個(gè)奇異向量描述了點(diǎn)集在三個(gè)主要方向的散布情況。由于是平面模型，垂直于平面的法向量 n 表示具有最小方差的方向，可以通過(guò)計(jì)算具有最小奇異值的奇異向量來(lái)求得。
那么在求得了 n 以后， d 可以通過(guò)代入種子點(diǎn)集的平均值 ,s(它代表屬于地面的點(diǎn)) 直接求得。整個(gè)平面模型計(jì)算代碼如下：
/* @brief The function to estimate plane model. The model parameter `normal_` and `d_`, and `th_dist_d_` is set here. The main step is performed SVD(UAV) on covariance matrix. Taking the sigular vector in U matrix according to the smallest sigular value in A, as the `normal_`. `d_` is then calculated according to mean ground points. @param g_ground_pc:global ground pointcloud ptr. */void PlaneGroundFilter::estimate_plane_(void){ // Create covarian matrix in single pass. // TODO: compare the efficiency. Eigen::Matrix3f cov; Eigen::Vector4f pc_mean; pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean); // Singular Value Decomposition: SVD JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); // use the least singular vector as normal normal_ = (svd.matrixU().col(2)); // mean ground seeds value Eigen::Vector3f seeds_mean = pc_mean.head<3>(); // according to normal.T*[x,y,z] = -d d_ = -(normal_.transpose() * seeds_mean)(0, 0); // set distance threhold to `th_dist - d` th_dist_d_ = th_dist_ - d_; // return the equation parameters} 優(yōu)化平面主循環(huán)extract_initial_seeds_(laserCloudIn);g_ground_pc = g_seeds_pc;// Ground plane fitter mainloopfor (int i = 0; i < num_iter_; i++){ estimate_plane_(); g_ground_pc->clear(); g_not_ground_pc->clear(); //pointcloud to matrix MatrixXf points(laserCloudIn_org.points.size(), 3); int j = 0; for (auto p : laserCloudIn_org.points) { points.row(j++) << p.x, p.y, p.z; } // ground plane model VectorXf result = points * normal_; // threshold filter for (int r = 0; r < result.rows(); r++) { if (result[r] < th_dist_d_) { g_all_pc->points[r].label = 1u; // means ground g_ground_pc->points.push_back(laserCloudIn_org[r]); } else { g_all_pc->points[r].label = 0u; // means not ground and non clusterred g_not_ground_pc->points.push_back(laserCloudIn_org[r]); } }}得到這個(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)過(guò)分類(lèi)以后的所有地面點(diǎn)被當(dāng)作下一次迭代的種子點(diǎn)集，迭代優(yōu)化。
02 基于雷達(dá)數(shù)據(jù)本身特點(diǎn)的方法-Ray Ground Filter代碼??https://github.com/suyunzzz/ray_filter_ground??
算法思想Ray Ground Filter算法的核心是以射線(xiàn)（Ray）的形式來(lái)組織點(diǎn)云。將點(diǎn)云的 (x, y, z)三維空間降到（x,y）平面來(lái)看，計(jì)算每一個(gè)點(diǎn)到車(chē)輛x正方向的平面夾角 θ, 對(duì)360度進(jìn)行微分，分成若干等份，每一份的角度為0.2度。
激光線(xiàn)束等間隔劃分示意圖（通常以激光雷達(dá)角度分辨率劃分）
同一角度范圍內(nèi)激光線(xiàn)束在水平面的投影以及在Z軸方向的高度折線(xiàn)示意圖
為了方便對(duì)同一角度的線(xiàn)束進(jìn)行處理，要將原來(lái)直角坐標(biāo)系的點(diǎn)云轉(zhuǎn)換成柱坐標(biāo)描述的點(diǎn)云數(shù)據(jù)結(jié)構(gòu)。對(duì)同一夾角的線(xiàn)束上的點(diǎn)按照半徑的大小進(jìn)行排序，通過(guò)前后兩點(diǎn)的坡度是否大于我們事先設(shè)定的坡度閾值，從而判斷點(diǎn)是否為地面點(diǎn)。
線(xiàn)激光線(xiàn)束縱截面與俯視示意圖（n=4）
通過(guò)如下公式轉(zhuǎn)換成柱坐標(biāo)的形式：
轉(zhuǎn)換成柱坐標(biāo)的公式
radius表示點(diǎn)到lidar的水平距離(半徑)，theta是點(diǎn)相對(duì)于車(chē)頭正方向(即x方向)的夾角。對(duì)點(diǎn)云進(jìn)行水平角度微分之后，可得到1800條射線(xiàn)，將這些射線(xiàn)中的點(diǎn)按照距離的遠(yuǎn)近進(jìn)行排序。通過(guò)兩個(gè)坡度閾值以及當(dāng)前點(diǎn)的半徑求得高度閾值，通過(guò)判斷當(dāng)前點(diǎn)的高度（即點(diǎn)的z值）是否在地面加減高度閾值范圍內(nèi)來(lái)判斷當(dāng)前點(diǎn)是為地面。
偽代碼偽代碼
local_max_slope_ ：設(shè)定的同條射線(xiàn)上鄰近兩點(diǎn)的坡度閾值。general_max_slope_ ：整個(gè)地面的坡度閾值遍歷1800條射線(xiàn)，對(duì)于每一條射線(xiàn)進(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滿(mǎn)足閾值并且前點(diǎn)的z坐標(biāo)小于雷達(dá)安裝高度減去height_threshold并大于雷達(dá)安裝高度加上height_threshold，認(rèn)為是地面點(diǎn)。
