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

---

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[i].segment (*_p_index_array[i], *_p_coefficients_array[i]);

        ROS_INFO_STREAM(" model = "+std::to_string(i));
        std::string s;
        for (int j = 0;j<_p_coefficients_array[i]->values.size();j++)
        {
            s += std::to_string(_p_coefficients_array[i]->values[j]) +",";
        }
        s = s+" "+std::to_string(_p_index_array[i]->indices.size())+"/"+std::to_string(_pclp->points.size());
        ROS_INFO_STREAM(s);

    }
    ROS_INFO_STREAM("-------------------------");


}

获得的结果中seg[i].segment (*_p_index_array[i], *_p_coefficients_array[i]);，_p_index_array保存了内点点云的索引值，_p_coefficients_array保存了不同模型的结果参数值，不同的模型其中数据的定义不同。
对于本文使用的SACMODEL_LINE模型来说，对单一结果其有6个元素，前三个为直线中的一点，后三个数据是直线的方向向量，如下图所示。


上图即为一次直线匹配的结果，（-12.78,30.11,0.60）是直线上一点，（0.15，-0.988,0）是直线的方向向量，这两个量可以确定一个直线。
至此，大小为X的聚类集合的直线拟合结果会形成一个同为X大小的Coefficients的Vector，下面即开始进行在Rviz中将其可视化。

2 结果Coefficient的可视化

因为结果是大小变动的一组直线，这里消息类型选用了visualization_msgs/Marker，这是ROS下形状标记的方便可用消息类型，其中包括了点、直线、立方体、球、点集等。这里我使用LINE_LIST来进行展示。在wiki中说明，LINE_LIST是根据其中的vector中点的数值来进行分线段的，线段为前一点和后一点相连，即0-1,2-3,4-5这种模式。如下图，因此，需要将上面的方向向量做个转化，变为直线上的两点。

代码如下：
其中_pCoe为上一章提到的Coefficients的Vector。

void CLaserReduce::pubCoefficients(vector<pcl::ModelCoefficients::Ptr>& _pCoe) {
        visualization_msgs::Marker lines;

        lines.header.frame_id  = "/map";
        lines.header.stamp = ros::Time::now();
        lines.ns =  "ransac";
        lines.action = visualization_msgs::Marker::ADD;
        lines.pose.orientation.w  = 1.0;



        lines.id = 0;




        lines.type = visualization_msgs::Marker::LINE_LIST;



        // POINTS markers use x and y scale for width/height respectively
        lines.scale.x = 0.2;

        std_msgs::ColorRGBA color_line;
        color_line.r = 0.5;
        color_line.g = 0.5;
        color_line.b = 0.5;
        color_line.a = 1.0;


        lines.points.clear();
        lines.colors.clear();


        int count = 0; // for vMapColors

        for(int i= 0;i<_pCoe.size();i++)
        {
            geometry_msgs::Point p1,p2;
            p1.x=_pCoe[i]->values[0];
            p1.y=_pCoe[i]->values[1];
            p1.z=_pCoe[i]->values[2];

            p2.x=_pCoe[i]->values[3] + _pCoe[i]->values[0];
            p2.y=_pCoe[i]->values[4] + _pCoe[i]->values[1];
            p2.z=_pCoe[i]->values[5] + _pCoe[i]->values[2];

            lines.points.push_back(p1);
            lines.points.push_back(p2);
            lines.colors.push_back(color_line);
            lines.colors.push_back(color_line);

        }
        pub_ransac_coefficients.publish(lines);
    ROS_INFO_STREAM("CoeffSize = "+ to_string(lines.points.size()));






}

其中，对点的处理为：

geometry_msgs::Point p1,p2;
            p1.x=_pCoe[i]->values[0];
            p1.y=_pCoe[i]->values[1];
            p1.z=_pCoe[i]->values[2];

            p2.x=_pCoe[i]->values[3] + _pCoe[i]->values[0];
            p2.y=_pCoe[i]->values[4] + _pCoe[i]->values[1];
            p2.z=_pCoe[i]->values[5] + _pCoe[i]->values[2];

即将方向向量直接加在点值上做出第二个点，如果需要详细些处理的，可以求出Cluster中点分布的区间范围，然后在该方向上取最大最小值即可。
运行后的结果为：

可以看到，明显的直线聚类是可以获得效果满意的直线参数的。但是对于明显属于曲线的聚类（黑色点）也生成了一个直线，这可以通过判断均方误差来进行模型类别的区别。
至此，可以初步实现用RANSAC来进行点集状况的估计和模型参数的获得。