« Analyse approfondie » : exploration de l'algorithme de segmentation des nuages ​​de points LiDAR dans la conduite autonome

Les algorithmes de segmentation de nuages ​​de points laser actuellement courants incluent des méthodes basées sur l'ajustement plan et des méthodes basées sur les caractéristiques des données de nuages ​​de points laser. Les détails sont les suivants :

Algorithme de segmentation au sol des nuages ​​de points

01 Méthode basée sur l'ajustement de plan - Ajustement du plan de sol

Idée d'algorithme : Une méthode de traitement simple est dans la direction x (La direction de l'avant de la voiture) divise l'espace en plusieurs sous-plans, puis utilise l'algorithme d'ajustement du plan de sol (GPF) pour chaque sous-plan afin d'obtenir une méthode de segmentation du sol capable de gérer les pentes raides. Cette méthode consiste à adapter un plan global dans un nuage de points à image unique. Elle fonctionne mieux lorsque le nombre de nuages ​​de points est important. Lorsque le nuage de points est clairsemé, il est facile de provoquer des détections manquées et des détections fausses, telles que 16 lignes. lidar.

Pseudo-code de l'algorithme :

Pseudo-code

Le processus de l'algorithme est que pour un nuage de points donné P, le résultat final de la segmentation est deux ensembles de nuages ​​de points, un nuage de points au sol et un nuage de points non- Nuage de points au sol Nuage de points. Cet algorithme a quatre paramètres importants, comme suit :

• Niter : le nombre de fois pour effectuer une décomposition en valeurs singulières (SVD), c'est-à-dire le nombre d'ajustements d'optimisation
• NLPR : utilisé pour sélectionner la hauteur la plus basse point de quantité LPR
• Thseed : Seuil de sélection des points de départ Lorsque la hauteur d'un point dans le nuage de points est inférieure à la hauteur de LPR plus ce seuil, nous ajoutons le point à l'ensemble de points de départ
• . Thdist : Seuil de distance dans le plan, nous calculerons la distance de chaque point du nuage de points à la projection orthogonale du plan que nous ajustons, et ce seuil de distance dans le plan est utilisé pour déterminer si le point appartient au sol

Sélection d'un ensemble de points de départ

Nous sélectionnons d'abord un ensemble de points de départ (ensemble de points de départ). Ces points de départ sont dérivés de points avec des hauteurs plus petites (c'est-à-dire des valeurs z) dans le nuage de points. modèle plan initial décrivant le sol Alors comment sélectionner ? Qu'en est-il de cette collection de graines ? Nous introduisons le concept de représentant du point le plus bas (LPR). LPR est la moyenne des points de hauteur les plus bas du NLPR garantit que l'étage de montage plan n'est pas affecté par le bruit de mesure.

Sélection des points de départ

L'entrée est un cadre de nuage de points. Les points de ce nuage de points ont été triés dans la direction z (c'est-à-dire la hauteur). Prenez num_lpr_ points minimum et trouvez. Obtenez la hauteur moyenne lpr_height (c'est-à-dire LPR) et sélectionnez le point avec une hauteur inférieure à lpr_height + th_seeds_ comme point de départ.

L'implémentation spécifique du code est la suivante

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

Modèle d'avion

Ensuite, nous construisons un modèle d'avion si la distance de projection orthogonale d'un point dans le nuage de points à ce plan est inférieure à la distance de projection orthogonale d'un point dans le nuage de points à ce plan. seuil Thdist, le point est considéré comme appartenant au sol, sinon il appartient au non-sol. Un modèle linéaire simple est utilisé pour l'estimation du modèle plan, comme suit :

ax+by+cz+d=0

soit :

où

, par La matrice de covariance C de l'ensemble de points initial est utilisée pour résoudre n afin de déterminer un plan. L'ensemble de points de départ est utilisé comme ensemble de points initial, et sa matrice de covariance est

.

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

Pour chaque ligne, triez les angles

1. Avec le point actuel p comme centre, sélectionnez le k/2ème point p1 et le kème point p2 à droite
2. Calculez p et p1, p1 et p2 respectivement, la distance entre p et p2 dans la direction z
3. Résolvez l'angle cosinus selon le théorème du cosinus

Si l'angle cosinus atteint le seuil et que p1.z-p.z atteint le seuil ou p1.z-p2 .z atteint le seuil et p.z- p2.z satisfait le seuil et considère ce point comme un obstacle

star_search_method

Cette méthode divise le nuage de points en segments rectangulaires, et la combinaison de ces formes ressemble à une étoile ; c'est de là que vient le nom, de chaque segment de route En extrayant les points de départ possibles du trottoir, l'algorithme créé est insensible aux changements de hauteur basés sur la coordonnée Z, ce qui signifie qu'en pratique l'algorithme fonctionnera bien même lorsque le lidar est incliné par rapport au plan de la surface de la route, dans un système de coordonnées cylindriques Traiter les nuages ​​de points.

Mise en œuvre spécifique :

star_search_method

Ce qui précède est le contenu détaillé de. pour plus d'informations, suivez d'autres articles connexes sur le site Web de PHP en chinois!