使用PLC_Ransac分析2D激光点云的模型参数并在ROS_Rviz中显示结果

最新推荐文章于 2023-05-10 17:31:59 发布

Plaggable

最新推荐文章于 2023-05-10 17:31:59 发布

阅读量1.4k

点赞数

分类专栏： c++ 感知 ROS 文章标签：算法聚类可视化 pcl

本文链接：https://blog.csdn.net/weixin_42269667/article/details/117252984

版权

目录导航

0 背景
1 RANSAC处理
2 结果Coefficient的可视化

0 背景

RANSAC是一种可以在大量噪声点（外点）的数据中提取出符合预设模型参数的算法，算法通过多次迭代可以在含有50%以上非模型点的数据中选出模型点（内点），因此适用于点云数据中形状信息的提取，如下图所示。
本文使用2D已经聚类的激光数据，运用Ransac算法来计算单一聚类中是否适合模型种类，选取的模型为{pcl::SACMODEL_CIRCLE2D,pcl::SACMODEL_LINE}，也就是2d圆和直线模型，来用以获得聚类中的具体特征数据。然后，在ROS Rviz中进行可视化展示。
在这里插入图片描述

1 RANSAC处理

数据为2D激光雷达数据，已经进行了聚类分析，将聚类结果保存在vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> vCluster中。然后循环将每个聚类点云通过函数doRansac来进行分析，同时，一次会进行两种模型的尝试，分别为{pcl::SACMODEL_CIRCLE2D,pcl::SACMODEL_LINE}。下面即为单次Ransac算法的代码，其中_size = 可能模型的个数，这里_size = 2。

void CRansac::doRansac(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _pclp, pcl::SacModel *_p_model_array,
                       pcl::PointIndices::Ptr *_p_index_array, pcl::ModelCoefficients::Ptr *_p_coefficients_array,
                       int _size) {
   



    // Optional
    for(int i = 0;i<_size;i++)
    {
   
        // Create the segmentation object
        pcl::SACSegmentation<pcl::PointXYZRGB> seg[_size];
        seg[i].setOptimizeCoefficients (true);
        // Mandatory
        seg[i].setModelType (_p_model_array[i]);
        seg[i].setMethodType (pcl::SAC_RANSAC);
        seg[i].setDistanceThreshold (0.05);

        seg[i].setInputCloud (_pclp);
        seg[