首頁 > 文章 > 科技週邊 > 「深度解析」：探究自動駕駛中的光達點雲分割演算法

「深度解析」：探究自動駕駛中的光達點雲分割演算法

王林轉載: 2023-04-23 16:46:221015瀏覽

目前常見的雷射點雲分割演算法有基於平面擬合的方法和基於雷射點雲資料特徵的方法兩類。具體如下：

點雲地面分割演算法

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

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

演算法偽代碼：

「深度解析」：探究自動駕駛中的光達點雲分割演算法

#偽代碼

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

Niter : 進行奇異值分解(SVD)的次數，也即進行最佳化擬合的次數
NLPR : 用於選取LPR的最低高度點的數量
#Thseed : 用來選取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_的點作為種子點。

具體程式碼實作如下

/*
@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

平面模型

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

ax by cz d=0

即：「深度解析」：探究自動駕駛中的光達點雲分割演算法

其中「深度解析」：探究自動駕駛中的光達點雲分割演算法

####,透過初始點集的協方差矩陣C來解n，從而確定一個平面，種子點集作為初始點集，其協方差矩陣為######

「深度解析」：探究自動駕駛中的光達點雲分割演算法

这个协方差矩阵 C 描述了种子点集的散布情况，其三个奇异向量可以通过奇异值分解（SVD）求得，这三个奇异向量描述了点集在三个主要方向的散布情况。由于是平面模型，垂直于平面的法向量 n 表示具有最小方差的方向，可以通过计算具有最小奇异值的奇异向量来求得。

那么在求得了 n 以后， d 可以通过代入种子点集的平均值 ,s(它代表属于地面的点) 直接求得。整个平面模型计算代码如下：

/*
@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();


// 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
}

优化平面主循环

extract_initial_seeds_(laserCloudIn);
g_ground_pc = g_seeds_pc;
// Ground plane fitter mainloop
for (int i = 0; i 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++) points.push_back(laserCloudIn_org[r]);
}
}
}

得到这个初始的平面模型以后，我们会计算点云中每一个点到该平面的正交投影的距离，即 points * normal_，并且将这个距离与设定的阈值(即th_dist_d_) 比较，当高度差小于此阈值，我们认为该点属于地面，当高度差大于此阈值，则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化。

02 基于雷达数据本身特点的方法-Ray Ground Filter

代码

https://www.php.cn/link/a8d3b1e36a14da038a06f675d1693dd8

算法思想

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，认为是地面点。

/*!
 *
 * @param[in] in_cloud Input Point Cloud to be organized in radial segments
 * @param[out] out_organized_points Custom Point Cloud filled with XYZRTZColor data
 * @param[out] out_radial_divided_indices Indices of the points in the original cloud for each radial segment
 * @param[out] out_radial_ordered_clouds Vector of Points Clouds, each element will contain the points ordered
 */
void PclTestCore::XYZI_to_RTZColor(const pcl::PointCloud::Ptr in_cloud,
 PointCloudXYZIRTColor &out_organized_points,
 std::vector &out_radial_divided_indices,
 std::vector &out_radial_ordered_clouds)
{
out_organized_points.resize(in_cloud->points.size());
out_radial_divided_indices.clear();
out_radial_divided_indices.resize(radial_dividers_num_);
out_radial_ordered_clouds.resize(radial_dividers_num_);

for (size_t i = 0; i points.size(); i++)
{
PointXYZIRTColor new_point;
//计算radius和theta
//方便转到柱坐标下。
auto radius = (float)sqrt(
in_cloud->points[i].x * in_cloud->points[i].x + in_cloud->points[i].y * in_cloud->points[i].y);
auto theta = (float)atan2(in_cloud->points[i].y, in_cloud->points[i].x) * 180 / M_PI;
if (theta points[i];
new_point.radius = radius;
new_point.theta = theta;
new_point.radial_div = radial_div;
new_point.concentric_div = concentric_div;
new_point.original_index = i;

out_organized_points[i] = new_point;

//radial divisions更加角度的微分组织射线
out_radial_divided_indices[radial_div].indices.push_back(i);

out_radial_ordered_clouds[radial_div].push_back(new_point);

} //end for

//将同一根射线上的点按照半径（距离）排序
#pragma omp for
for (size_t i = 0; i