"심층 분석": 자율 주행에서 LiDAR 포인트 클라우드 분할 알고리즘 탐색

현재 널리 사용되는 레이저 포인트 클라우드 분할 알고리즘에는 평면 피팅에 기반한 방법과 레이저 포인트 클라우드 데이터의 특성에 기반한 방법이 있습니다.

점군 지면 분할 알고리즘

01 평면 피팅 기반 방법 - Ground Plane Fitting

알고리즘 아이디어: x 방향을 따르는 간단한 처리 방법 (자동차 전면 방향) 공간을 여러 개의 하위 평면으로 나눈 후, 각 하위 평면에 대한 GPF(Ground Plane Fitting Algorithm)를 사용하여 급경사를 처리할 수 있는 지상 분할 방법을 얻습니다. 이 방법은 단일 프레임 포인트 클라우드에 전역 평면을 맞추는 것입니다. 포인트 클라우드의 수가 많을 때 더 잘 작동합니다. 포인트 클라우드가 희박하면 16라인과 같은 감지 누락 및 잘못된 감지가 발생하기 쉽습니다. 라이더.

알고리즘 의사 코드:

의사 코드

알고리즘 프로세스는 주어진 포인트 클라우드 P에 대해 분할의 최종 결과는 두 개의 포인트 클라우드 세트, 지상 포인트 클라우드 및 비-포인트 클라우드입니다. 지상 포인트 클라우드 포인트 클라우드. 이 알고리즘에는 다음과 같은 4가지 중요한 매개변수가 있습니다.

• Niter: 특이값 분해(SVD)를 수행하는 횟수, 즉 최적화 피팅을 수행하는 횟수
• NLPR: 선택하는 데 사용됩니다. LPR Quantity
• Thseed: 시드 포인트 선택을 위한 임계값. 포인트 클라우드의 포인트 높이가 LPR 높이에 이 임계값을 더한 값보다 작으면 해당 포인트를 시드 포인트 세트에 추가합니다
• Thdist: 평면 거리 임계값, 점 구름의 각 점에서 우리가 맞춘 평면의 직교 투영까지의 거리를 계산하며, 이 평면 거리 임계값은 점이 지면에 속하는지 여부를 결정하는 데 사용됩니다

시드 포인트 세트 선택

먼저 시드 포인트 세트(시드 포인트 세트)를 선택합니다. 이러한 시드 포인트는 포인트 클라우드에서 더 작은 높이(즉, z 값)를 갖는 포인트에서 파생됩니다. 지면을 설명하는 초기 평면 모델을 구축하려면 어떻게 선택해야 할까요? 이 종자 컬렉션은 어떻습니까? 최저점 대표(LPR) 개념을 소개합니다. LPR은 NLPR의 가장 낮은 높이 지점의 평균입니다. LPR은 평면 피팅 단계가 측정 노이즈의 영향을 받지 않도록 합니다.

시드 포인트 선택

이 포인트 클라우드의 포인트는 z 방향(예: 높이)을 따라 정렬되었습니다. 평균 높이 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

즉:

여기서

, by 초기 점 집합의 공분산 행렬 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 

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]

각 선에 대해 각도를 정렬합니다

1. 현재 점 p를 중심으로 k/2번째 점 p1과 오른쪽의 k번째 점 p2를 선택합니다
2. p와 p1, p1과 p2를 각각 계산합니다. z 방향의 p와 p2 사이의 거리
3. 코사인 정리에 따라 코사인 각도를 풀어보세요

코사인 각도가 임계값을 충족하고 p1.z-p.z가 임계값 또는 p1.z-p2를 충족하는 경우 .z는 임계값을 충족하고 p.z- p2.z는 임계값을 충족하며 이 지점을 장애물로 간주합니다

star_search_method

이 방법은 포인트 클라우드를 직사각형 세그먼트로 나누고 이러한 모양의 조합은 별과 유사합니다. ; 각 도로 구간에서 이름이 유래된 곳입니다. 가능한 보도 시작점을 추출하고 생성된 알고리즘은 Z 좌표를 기반으로 한 높이 변화에 민감하지 않습니다. 이는 실제로 LiDAR가 상대적으로 기울어진 경우에도 알고리즘이 잘 수행된다는 것을 의미합니다. 원통형 좌표계에서 노면 평면으로 포인트 클라우드를 처리합니다.

구체적인 구현:

star_search_method

위 내용은 "심층 분석": 자율 주행에서 LiDAR 포인트 클라우드 분할 알고리즘 탐색의 상세 내용입니다. 자세한 내용은 PHP 중국어 웹사이트의 기타 관련 기사를 참조하세요!