"Analisis Mendalam": Meneroka Algoritma Segmentasi Awan Titik LiDAR dalam Pemanduan Autonomi

Pada masa ini, algoritma pembahagian awan titik laser biasa termasuk kaedah berdasarkan pemasangan satah dan kaedah berdasarkan ciri data awan titik laser. Butirannya adalah seperti berikut:

Algoritma pembahagian tanah awan titik

01 berdasarkan kaedah Pemasangan satah - Pemasangan Satah Tanah

Idea algoritma: Kaedah pemprosesan mudah ialah membahagikan ruang kepada beberapa sub-satah di sepanjang arah x (arah kepala kereta ), dan kemudian bahagikan setiap Sub-satah menggunakan algoritma pemasangan satah tanah (GPF) untuk mendapatkan kaedah pembahagian tanah yang boleh mengendalikan cerun curam. Kaedah ini adalah untuk memuatkan satah global dalam awan titik bingkai tunggal Ia berfungsi lebih baik apabila bilangan awan titik adalah besar Apabila awan titik jarang, ia mudah menyebabkan pengesanan terlepas dan pengesanan palsu, seperti 16 baris. lidar.

Pseudokod Algoritma:

Pseudokod

The proses algoritma ialah untuk awan titik P tertentu, hasil akhir pembahagian ialah set awan dua titik, awan titik bumi dan awan titik bukan tanah. Algoritma ini mempunyai empat parameter penting, seperti berikut:

• Niter: bilangan kali untuk melakukan penguraian nilai tunggal (SVD), iaitu bilangan kali untuk melakukan pengoptimuman sesuai
• NLPR: Bilangan titik ketinggian terendah yang digunakan untuk memilih LPR
• Thseed: Ambang yang digunakan untuk memilih titik benih, apabila ketinggian daripada titik dalam awan titik adalah kurang daripada Apabila ketinggian LPR ditambah pada ambang ini, kami menambah titik pada set titik benih
• Ketiga: Ambang jarak satah, kami akan hitung setiap titik dalam awan titik ke satah yang kita muatkan Jarak unjuran ortogon, dan ambang jarak satah ini digunakan untuk menentukan sama ada titik itu tergolong dalam tanah

Pemilihan set titik benih

Kami mula-mula memilih set titik benih (set titik benih ini diperoleh daripada titik dengan ketinggian yang lebih kecil (iaitu nilai z) dalam awan titik . Set mata benih digunakan untuk mewujudkan model satah awal yang menerangkan tanah, kemudian Bagaimana untuk memilih set titik benih ini? Kami memperkenalkan konsep Perwakilan Titik Terendah (LPR). LPR ialah purata titik ketinggian terendah NLPR memastikan peringkat pemasangan pesawat tidak terjejas oleh hingar pengukuran.

Pemilihan mata benih

Input ialah bingkai titik cloud, Titik dalam awan titik ini telah diisih mengikut arah z (iaitu, tinggi, ambil num_lpr_ mata minimum, cari purata ketinggian lpr_height (iaitu, LPR), dan pilih titik dengan ketinggian kurang daripada lpr_height + th_seeds_ sebagai benih mata.

Kod khusus dilaksanakan seperti berikut

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

Model rata

Seterusnya kita membina model satah Jika jarak unjuran ortogon bagi titik dalam awan titik ke satah ini adalah kurang daripada ambang Thdist, maka titik itu dianggap milik tanah, jika tidak, ia tergolong dalam bukan. -tanah. Model linear ringkas digunakan untuk anggaran model satah, seperti berikut:

ax+by+cz+d=0

Iaitu:

di mana

, melalui kovarians awal set titik Matriks C digunakan untuk menyelesaikan n untuk menentukan satah Set titik benih digunakan sebagai set titik awal, dan matriks kovariansnya ialah

这个协方差矩阵 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 

03 基于雷达数据本身特点的方法-urban road filter

原文

Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles

代码

​​https://www.php.cn/link/305fa4e2c0e76dd586553d64c975a626​​

z_zero_method

z_zero_method

首先将数据组织成[channels][thetas]

对于每一条线，对角度进行排序

1. 以当前点p为中心，向左选k个点，向右选k个点
2. 分别计算左边及右边k个点与当前点在x和y方向差值的均值
3. 同时计算左边及右边k个点的最大z值max1及max2
4. 根据余弦定理求解余弦角

如果余弦角度满足阈值且max1减去p.z满足阈值或max2减去p.z满足阈值且max2-max1满足阈值，认为此点为障碍物，否则就认为是地面点。

x_zero_method

X-zero和Z-zero方法可以找到避开测量的X和Z分量的人行道，X-zero和Z-zero方法都考虑了体素的通道数，因此激光雷达必须与路面平面不平行，这是上述两种算法以及整个城市道路滤波方法的已知局限性。X-zero方法去除了X方向的值，使用柱坐标代替。

x_zero_method

首先将数据组织成[channels][thetas]

Untuk setiap baris, susun sudut

1. Dengan titik p semasa sebagai pusat, pilih titik k/2 p1 dan titik kth p2 di sebelah kanan
2. Kira jarak dalam arah z antara p dan p1, p1 dan p2, p dan p2 masing-masing
3. Cari sudut kosinus mengikut teorem kosinus

Jika sudut kosinus memenuhi ambang dan p1.z-p.z memenuhi ambang atau p1.z-p2.z memenuhi ambang dan p.z-p2.z memenuhi ambang , titik ini dianggap sebagai penghalang

kaedah_cari_bintang

Kaedah ini membahagikan awan titik kepada segmen segi empat tepat, gabungan bentuk ini menyerupai bintang ; dari sinilah nama itu berasal, dari setiap segmen jalan untuk mengekstrak titik permulaan kaki lima yang mungkin, algoritma yang dibuat di dalamnya tidak sensitif terhadap perubahan ketinggian berdasarkan koordinat Z, yang bermaksud bahawa dalam amalan algoritma akan berfungsi dengan baik walaupun apabila lidar adalah condong berbanding satah permukaan jalan, dalam koordinat silinder Sistem memproses awan titik.

Pelaksanaan khusus:

star_search_method

Atas ialah kandungan terperinci "Analisis Mendalam": Meneroka Algoritma Segmentasi Awan Titik LiDAR dalam Pemanduan Autonomi. Untuk maklumat lanjut, sila ikut artikel berkaitan lain di laman web China PHP!