平面拟合算法实现点云中的地面部分提取
由于上一篇提出的基于ROS的通过计算夹角实现点云中的地面部分提取中存在以下不足：
1、存在少量噪点，不能彻底过滤出地面;
2、非地面的点容易被错误分类，造成非地面点缺失;
3、对于目标接近激光雷达盲区的情况，会出现误分割，即将非地面点云分割为地面。
本文记录一种新的地面分割方法，在可靠性、分割精度方面均优于上一篇提出的基于ROS的通过计算夹角实现点云中的地面部分提取方法。我们将其称之为地面平面拟合方法。
出自论文：Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications
一、基本流程：
首先对点云以z轴大小进行排序，选取若干个z轴最小的点的Z值均值，种子点集就是在高度附近（自己设定阈值）的点构成的点集。使用线性模型用于平面模型估计：
然后计算其协方差矩阵C和最小奇异值的奇异向量求解n,
通过n来确定一个平面，并采用种子点作为初始点集。其协方差矩阵为：
得到这个初始的平面模型以后，再计算点云中每一个点到该平面的正交投影的距离，即 points * normal_，并且将这个距离与设定的阈值Th dist(即th_dist_d_) 比较，当高度差小于此阈值，我们 认为该点属于地面，当高度差大于此阈值，则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化，求出结果。
二、实现：
伪代码：
伪代码解释：
1、N iter : 进行奇异值分解（singular value decomposition，SVD）的次数，也即进行优化拟合的次数
2、N L P R N：用于选取LPR的最低高度点的数量
3、T h s e e d : 用于选取种子点的阈值，当点云内的点的高度小于LPR的高度加上此阈值时，我们将该点加入种子点集
4、T h d i s t Th : 平面距离阈值，我们会计算点云中每一个点到我们拟合的平面的正交投影的距离，而这个平面距离阈值，就是用来判定点是否属于地面
种子点集的选取
/*
@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<VPoint> &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 < p_sorted.points.size() && cnt < num_lpr_; i++)
{
sum += p_sorted.points[i].z;
cnt++;
}
double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0
g_seeds_pc->clear();
// iterate pointcloud, filter those height is less than lpr.height+th_seeds_
for (int i = 0; i < p_sorted.points.size(); i++)
{
if (p_sorted.points[i].z < lpr_height + th_seeds_)
{
g_seeds_pc->points.push_back(p_sorted.points[i]);
}
}
// return seeds points
}
/*
输入是一个点云，这个点云内的点已经沿着z方向（即高度）做了排序，取 num_lpr_ 个最小点，求得高度平均值 lpr_height（即LPR），选取高度小于 lpr_height + th_seeds_的点作为种子点。
*/
计算平面模型
/*
@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<MatrixXf> 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<3>();
// 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 < num_iter_; i++)
{
estimate_plane_();
g_ground_pc->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++) << p.x, p.y, p.z;
}
// ground plane model
VectorXf result = points * normal_;
// threshold filter
for (int r = 0; r < result.rows(); r++)
{
if (result[r] < th_dist_d_)
{
g_all_pc->points[r].label = 1u; // means ground
g_ground_pc->points.push_back(laserCloudIn_org[r]);
}
else
{
g_all_pc->points[r].label = 0u; // means not ground and non clusterred
g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
}
}
}
/*
计算点云中每一个点到该平面的正交投影的距离，即 points * normal_，并且将这个距离与设定的阈值 T h d i s t Th_{dist}Th
dist
(即th_dist_d_) 比较，当高度差小于此阈值，我们认为该点属于地面，当高度差大于此阈值，则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集，迭代优化。
*/
ROS实践
在上一篇的基础上，进一步滤除雷达过近处和过高处的点，因为雷达安装位置的原因，近处的车身反射会对Detection造成影响，需要滤除; 过高的目标，如大树、高楼，对于无人车的雷达感知意义也不大，我们对过近过高的点进行切除，代码如下：
void PlaneGroundFilter::clip_above(const pcl::PointCloud<VPoint>::Ptr in,
const pcl::PointCloud<VPoint>::Ptr out)
{
pcl::ExtractIndices<VPoint> cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
if (in->points[i].z > clip_height_)
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
void PlaneGroundFilter::remove_close_far_pt(const pcl::PointCloud<VPoint>::Ptr in,
const pcl::PointCloud<VPoint>::Ptr out)
{
pcl::ExtractIndices<VPoint> cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);
if ((distance < min_distance_) || (distance > max_distance_))
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
编写launch文件：
<launch>
<arg name="input_topic" default="/velodyne_points" />
<arg name="no_ground_point_topic" default="/points_no_ground"/>
<arg name="ground_point_topic" default="/points_ground"/>
<arg name="all_points_topic" default="/all_points"/>
<!-- clip the points above sensor_height+clip_height -->
<arg name="clip_height" default="4.0"/>
<!-- the height of lidar position -->
<arg name="sensor_height" default="1.77"/>
<!-- the min distance to be remove -->
<arg name="min_distance" default="2.0"/>
<!-- the max distance to be remove -->
<arg name="max_distance" default="75.0"/>
<!-- we use 32 lidar -->
<arg name="sensor_model" default="32"/>
<!-- Num of Iteration -->
<arg name="num_iter" default="3"/>
<!-- Num of LPR -->
<arg name="num_lpr" default="20"/>
<!-- Seeds Threshold -->
<arg name="th_seeds" default="1.2"/>
<!-- Distance Threshold -->
<arg name="th_dist" default="0.3"/>
<node pkg="plane_ground_filter" type="plane_ground_filter_node" name="plane_ground_filter_node" output="screen">
<param name="input_topic" value="$(arg input_topic)"/>
<param name="no_ground_point_topic" value="$(arg no_ground_point_topic)"/>
<param name="ground_point_topic" value="$(arg ground_point_topic)"/>
<param name="all_points_topic" value="$(arg all_points_topic)"/>
<param name="sensor_height" value="$(arg sensor_height)" />
<param name="clip_height" value="$(arg clip_height)" />
<param name="min_distance" value="$(arg min_distance)" />
<param name="max_distance" value="$(arg max_distance)" />
<param name="sensor_model" value="$(arg sensor_model)" />
<param name="num_iter" value="$(arg num_iter)" />
<param name="num_lpr" value="$(arg num_lpr)" />
<param name="th_seeds" value="$(arg th_seeds)" />
<param name="th_dist" value="$(arg th_dist)" />
</node>
</launch>
三、运行：
1、在pcl_test下进行编译和更新：
catkin_make
source devel/setup.sh
2、启动这个节点：
roslaunch plane_fit_ground_filter plane_ground_filter.launch
3、启动rviz(要在launch之后运行):
rviz
4、运行包：
rosbag play 11-12-boliu2.bag
//需要↑这个包可私信
/*
或者：from AdamShan:
rosbag play --clock test.bag
测试bag下载链接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w
*/
实验结果如下：
原始点云地图：
提取的地面：
分割出非地面点云之后，我们就可以让Lidar Detection的代码工作在/filtered_points_no_ground上了，从而排除了地面对于Lidar聚类以及Detection的影响。
参考：无人驾驶汽车系统入门（二十七）——基于地面平面拟合的激光雷达地面分割方法和ROS实现
扫一扫
2266
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
