无人驾驶汽车系统入门(二十七)——基于地面平面拟合的激光雷达地面分割方法和ROS实现

无人驾驶汽车系统入门(二十七)——基于地面平面拟合的激光雷达地面分割方法和ROS实现

在博客的第二十四篇中,我们介绍了一种基于射线的地面过滤方法,此方法能够很好的完成地面分割,但是存在几点不足:第一,存在少量噪点,不能彻底过滤出地面;第二,非地面的点容易被错误分类,造成非地面点缺失;第三,对于目标接近激光雷达盲区的情况,会出现误分割,即将非地面点云分割为地面。通过本文我们一起学习一种新的地面分割方法,出自论文:Fast segmentation of 3D point clouds: A paradigm on LiDAR data for autonomous vehicle applications,在可靠性、分割精度方面均优于基于射线的地面分割方法。我们将其称之为地面平面拟合方法。

地面平面拟合(Ground Plane Fitting)

我们采用平面模型(Plane Model)来拟合当前的地面,通常来说,由于现实的地面并不是一个“完美的”平面,而且当距离较大时激光雷达会存在一定的测量噪声,单一的平面模型并不足以描述我们现实的地面。要很好的完成地面分割,就必须要处理存在一定坡度变化的地面的情况(不能将这种坡度的变化视为非地面,不能因为坡度的存在而引入噪声),一种简单的处理方法就是沿着x方向(车头的方向)将空间分割成若干个子平面,然后对每个子平面使用地面平面拟合算法(GPF)从而得到能够处理陡坡的地面分割方法。

那么如何进行平面拟合呢?其伪代码如下:

在这里插入图片描述

我们来详细的了解这一流程:对于给定的点云 P P P ,分割的最终结果为两个点云集合 P g P_g Pg :地面点云 和 P n g P_{ng} Png :非地面点云。该算法有四个重要的参数,分别是:

  • N i t e r N_{iter} Niter : 进行奇异值分解(singular value decomposition,SVD)的次数,也即进行优化拟合的次数
  • N L P R N_{LPR} NLPR : 用于选取LPR的最低高度点的数量
  • T h s e e d Th_{seed} Thseed : 用于选取种子点的阈值,当点云内的点的高度小于LPR的高度加上此阈值时,我们将该点加入种子点集
  • T h d i s t Th_{dist} Thdist : 平面距离阈值,我们会计算点云中每一个点到我们拟合的平面的正交投影的距离,而这个平面距离阈值,就是用来判定点是否属于地面

种子点集的选取

我们首先选取一个种子点集(seed point set),这些种子点来源于点云中高度(即z值)较小的点,种子点集被用于建立描述地面的初始平面模型,那么如何选取这个种子点集呢?我们引入最低点代表(Lowest Point Representative, LPR)的概念。LPR就是 N L P R N_{LPR} NLPR 个最低高度点的平均值,LPR保证了平面拟合阶段不受测量噪声的影响。这个LPR被当作是整个点云 P P P 的最低点,点云P中高度在阈值 T h s e e d Th_{seed} Thseed 内的点被当作是种子点,由这些种子点构成一个种子点集合。

种子点集的选取代码如下:

/*
    @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_的点作为种子点。

平面模型

接下来我们需要确定一个平面,点云中的点到这个平面的正交投影距离小于阈值 T h d i s t Th_{dist} Thdist 则认为该点属于地面,否则属于非地面,我们采用一个简单的线性模型用于平面模型估计,如下:

a x + b y + c z + d = 0 ax+by+cz+d = 0 ax+by+cz+d=0

也即:

n T x = − d \bm{n}^T \bm{x} = -d nTx=d

其中 n = [ a , b , c ] T \bm{n} = [a, b, c]^T n=[a,b,c]T , x = [ x , y , z ] T \bm{x} = [x, y, z]^T x=[x,y,z]T ,通过初始点集的协方差矩阵 C ∈ R 3 x 3 C \in R^{3x3} CR3x3 来求解 n \bm{n} n (即a, b, c), 从而确定一个平面, 我们采用种子点集 S ∈ R 3 S \in R^{3} SR3 作为初始点集,其协方差矩阵为:

C = ∑ i = 1 : ∣ S ∣ ( s i − s ^ ) ( s i − s ^ ) T C = \sum_{i=1:|S|}(s_i - \widehat{s})(s_i - \widehat{s})^T C=i=1:S(sis )(sis )T

其中, s ^ \widehat{s} s 是所有点的均值。这个协方差矩阵 C C C 描述了种子点集的散布情况,其三个奇异向量可以通过奇异值分解(Singular Value Decomposition,SVD)求得,这三个奇异向量描述了点集在三个主要方向的散布情况。由于是平面模型,垂直于平面的法向量 n \bm{n} n 表示具有最小方差的方向,可以通过计算具有最小奇异值的奇异向量来求得。

那么在求得了 n \bm{n} n 以后, d d d 可以通过代入种子点集的平均值 s ^ \widehat{s} 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<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} Thdist (即th_dist_d_) 比较,当高度差小于此阈值,我们认为该点属于地面,当高度差大于此阈值,则为非地面点。经过分类以后的所有地面点被当作下一次迭代的种子点集,迭代优化。

ROS实践

下面我们编写一个简单的ROS节点实现这一地面分割算法。我们仍然采用第24篇博客(激光雷达的地面-非地面分割和pcl_ros实践)中的bag进行实验,这个bag来自Velodyne的VLP32C,在此算法的基础上我们要进一步滤除雷达过近处和过高处的点,因为雷达安装位置的原因,近处的车身反射会对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);
}

编译并运行该ROS节点,使用我们在第二十四篇博客中提供的bag进行测试,得到如下结果:

在这里插入图片描述

完整代码:https://github.com/AbangLZU/plane_fit_ground_filter

AdamShan CSDN认证博客专家 图像处理 深度学习 TensorFlow
奔驰高级自动驾驶扫地僧,谷歌认证机器学习专家,兰州大学无人驾驶团队创始人,主攻深度学习,无人驾驶汽车方向,著有《无人驾驶原理与实践》一书。
已标记关键词 清除标记
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付 29.90元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值