pcl RANSAC平面分割单帧激光点云地面提取

最新推荐文章于 2025-07-03 01:31:14 发布

原创最新推荐文章于 2025-07-03 01:31:14 发布 · 1.6k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶

SLAM 专栏收录该内容

9 篇文章

订阅专栏

本文介绍了一种使用RANSAC算法在ROS环境下从Velodyne激光雷达单帧点云中快速提取地面点的方法。该方法通过设置Z轴阈值并结合RANSAC平面拟合技术实现地面点云的增量式提取。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

介绍

ros下velodyne激光雷达的回调函数, 利用RANSAC平面分割, 从单帧激光点云中提取出地面点

该方法可用于incremental的地面提取, 优势是速度快;
但由于地面并不总是平面, 所以该方法不如batch方法, 但batch方法要对全部激光点云进行计算, 速度较慢.

代码

void vldHandle(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    double timeCur = laserCloudMsg->header.stamp.toSec();
    time_now = timeCur;
    pcl::PointCloud<PointType>::Ptr laserCloudIn;
    laserCloudIn.reset(new pcl::PointCloud<PointType>());
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
    // Remove Nan points
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

    pcl::PointCloud<PointType>::Ptr ground_cloud, ground_cloud_candidate;
    ground_cloud.reset(new pcl::PointCloud<PointType>());
    ground_cloud_candidate.reset(new pcl::PointCloud<PointType>());
    pcl::PassThrough<PointType> pass;
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<PointType> seg;
    pcl::ExtractIndices<PointType> extract;

    // 初始化RANSAC分割器
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(ground_RANSAC_threshold);
    //seg.setAxis(Eigen::Vector3f(0, 0, 1.0));

    // 提取地面点
    pass.setInputCloud(laserCloudIn);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(ground_lower_threshold - sensorMountHeight, ground_upper_threshold - sensorMountHeight);
    pass.filter(*ground_cloud_candidate);

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.1);

    seg.setInputCloud(ground_cloud_candidate);
    seg.segment(*inliers, *coefficients);

    extract.setInputCloud(ground_cloud_candidate);
    extract.setIndices(inliers);
    extract.setNegative(false);
    extract.filter(*ground_cloud);

    if(ground_cloud_time_que.size() == 0 || ground_cloud_time_que.back() != timeCur)
    {
        //increaseGroundCloud(ground_cloud);
        ground_cloud_que.push(ground_cloud);
        ground_cloud_time_que.push(timeCur);
        ground_cloud_current.reset(new pcl::PointCloud<PointType>());
        *ground_cloud_current += *ground_cloud;
    }

    if(obstacle_cloud_time_que.size() == 0 || obstacle_cloud_time_que.back() != timeCur)
    {
        obstacle_cloud_que.push(laserCloudIn);
        obstacle_cloud_time_que.push(timeCur);
        obstacle_cloud_current.reset(new pcl::PointCloud<PointType>());
        *obstacle_cloud_current += *laserCloudIn;
    }
    //cout<<"receive vld, time: "<<setprecision(14)<<timeCur<<endl;
}

其中的部分全局变量

pcl::PointCloud<PointType>::Ptr ground_cloud_current; // 当前帧地面点云
pcl::PointCloud<PointType>::Ptr obstacle_cloud_current; // 当前帧全部点云
queue<pcl::PointCloud<PointType>::Ptr> ground_cloud_que; // 存放地面点云的queue
queue<double> ground_cloud_time_que; // 存放地面点云time_stamp的queue

queue<pcl::PointCloud<PointType>::Ptr> obstacle_cloud_que; // 存放全部点云的queue
queue<double> obstacle_cloud_time_que; // 存放全部点云time_stamp读queue