ホームページ > 記事 > テクノロジー周辺機器 > 「詳細な分析」: 自動運転における LiDAR 点群セグメンテーション アルゴリズムの調査
現在、一般的なレーザー点群セグメンテーション アルゴリズムには、平面フィッティングに基づく方法とレーザー点群データの特性に基づく方法が含まれます。詳細は次のとおりです。
#点群地面分割アルゴリズム
アルゴリズムの考え方: 単純な処理方法は、空間を x 方向 (車のヘッドの方向) に沿っていくつかのサブ平面に分割することです。 ) を実行し、それぞれを実行します。サブプレーンは、地表平面フィッティング アルゴリズム (GPF) を使用して、急な傾斜を処理できる地表セグメンテーション方法を取得します。この方法は、グローバル平面を単一フレームの点群に当てはめる方法であり、点群の数が多い場合に効果的ですが、点群がまばらな場合、16 ラインなどの検出漏れや誤検出が発生しやすくなります。ライダー。
アルゴリズム擬似コード:擬似コード
#アルゴリズム プロセスでは、特定の点群 P について、セグメンテーションの最終結果は 2 つの点群コレクション (地上点群と非地上点群) になります。このアルゴリズムには、次の 4 つの重要なパラメーターがあります。
#シード ポイントの選択
入力はポイントのフレームですcloud, この点群内の点は、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
フラット モデル
ax by cz d=0
つまり:
は、初期点の共分散行列 C によって計算されますset Solve for 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_) 比较,当高度差小于此阈值,我们认为该点属于地面,当高度差大于此阈值,则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集,迭代优化。
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值)是否在地面加减高度阈值范围内来判断当前点是为地面。
伪代码
遍历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
Real-Time LIDAR-Based Urban Road and Sidewalk Detection for Autonomous Vehicles
https://www.php.cn/link/305fa4e2c0e76dd586553d64c975a626
z_zero_method
首先将数据组织成[channels][thetas]
对于每一条线,对角度进行排序
如果余弦角度满足阈值且max1减去p.z满足阈值或max2减去p.z满足阈值且max2-max1满足阈值,认为此点为障碍物,否则就认为是地面点。
X-zero和Z-zero方法可以找到避开测量的X和Z分量的人行道,X-zero和Z-zero方法都考虑了体素的通道数,因此激光雷达必须与路面平面不平行,这是上述两种算法以及整个城市道路滤波方法的已知局限性。X-zero方法去除了X方向的值,使用柱坐标代替。
x_zero_method
首先将数据组织成[channels][thetas]
各直線について、角度を並べ替えます
コサイン角がしきい値を満たし、p1.z-p.z がしきい値を満たしているか、または p1.z-p2.z がしきい値を満たし、p.z-p2.z がしきい値を満たしている場合、この点は障害物とみなされます
このメソッドは点群を長方形のセグメントに分割し、これらの形状の組み合わせは星に似ています; これが名前の由来で、各道路セグメントから可能な歩道の開始点を抽出し、作成されたアルゴリズムは Z 座標に基づく高さの変化に影響されません。つまり、実際には、LIDAR がオンになっている場合でもアルゴリズムが適切に実行されます。円筒座標で道路平面に対して傾斜 システムは点群を処理します。
具体的な実装:
##star_search_method
以上が「詳細な分析」: 自動運転における LiDAR 点群セグメンテーション アルゴリズムの調査の詳細内容です。詳細については、PHP 中国語 Web サイトの他の関連記事を参照してください。