PCL之圆形检测和直线检测

一、圆形检测

//搜寻所有的圆，知道找不到符合条件的圆，记录圆的圆心和半径
    int index=0;
    while(1)
    {
        pcl::visualization::PCLVisualizer viewer2("Visualization of original and target point cloud");
        viewer2.setBackgroundColor(255,255,255);

        pcl::PointCloud<pcl::PointXYZ>::Ptr Circle2D(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr tempRemain(new pcl::PointCloud<pcl::PointXYZ>);

        pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr model_circle2D(new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>(Src_Cloud_Planar));
        std::vector<int> inliers;

        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_circle2D);
        ransac.setDistanceThreshold(0.001);
        ransac.computeModel();
        ransac.getInliers(inliers);
        qDebug()<<"点云数量："<<inliers.size();
        //如果找不到圆，退出
        if(inliers.size()==0)
        {
            break;
        }

        pcl::IndicesPtr tempinliers(new std::vector<int>(inliers));
        Eigen::VectorXf modelParas;
        ransac.getModelCoefficients(modelParas);
        std::cout << modelParas<< "\n\n";
        qDebug()<<modelParas(0);
        qDebug()<<modelParas(1);
        qDebug()<<modelParas(2);
        qDebug()<<"002!";

        //获取点和删除点2D
        pcl::ExtractIndices<pcl::PointXYZ> Circle_extract;
        Circle_extract.setInputCloud(Src_Cloud_Planar);//输入点云
        Circle_extract.setIndices(tempinliers);
        Circle_extract.setNegative(false);
        Circle_extract.filter(*Circle2D);//line2D本次分割出的2D线--
        //删除点
        Circle_extract.setNegative(true);
        Circle_extract.filter(*tempRemain);
        *Src_Cloud_Planar = *tempRemain;//本次剩余的点云--

        //如果找不到圆，退出
        if(inliers.size()<=60)
        {
            viewer2.close();
            continue;
        }

        //画圆环
        QVector<pcl::PointXYZ> points;
        double o_x = modelParas(0), o_y=modelParas(1), r = modelParas(2);    //圆心及半径
        for (int i = 0; i < 100; i++)   //计算圆环上点的坐标
        {
            pcl::PointXYZ point;
            double alpha = 2 * M_PI / (100 - 1);
            point.x = o_x + r * cos(i * alpha);
            point.y = o_y + r * sin(i * alpha);
            point.z = 0;
            points.push_back(point);
        }
        //将圆环上的点用线段连起来
        for (int i = 0; i < points.size() - 1; i++)
        {
            viewer2.addLine(points.at(i), points.at(i + 1), QString("CircleLineX%1").arg(index).toStdString(), 0);
            index=index+1;
        }
        viewer2.addLine(points.at(points.size() - 1), points.at(0), QString("CircleLineX%1_%2").arg(points.size()).arg(5).toStdString(), 0);
        index=index+1;
        //-------------------原始点云和过滤目标点云可视化------------------------
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(Src_Cloud_Planar, 0, 255, 0);
        viewer2.addPointCloud<pcl::PointXYZ>(Src_Cloud_Planar, single_color, "Source clouds");
        while (!viewer2.wasStopped())
        {
            viewer2.spinOnce(1);
        }
    }
    qDebug()<<"003!";
    while (!viewer00.wasStopped())
    {
        viewer00.spinOnce(1);
    }

二、直线检测

// 直通滤波获取直线（删除后，多次循环即可）
    pcl::PointCloud<pcl::PointXYZ>::Ptr line2D(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f2D(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr ptCloud2D(new pcl::PointCloud<pcl::PointXYZ>);

    //创建一个模型参数对象，用于记录结果
    pcl::ModelCoefficients::Ptr coefficients3(new pcl::ModelCoefficients);
    //inliers表示误差能容忍的点 记录点云的序号
    pcl::PointIndices::Ptr inliers3(new pcl::PointIndices);
    // 创建一个分割器
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients(true);
    // Mandatory-设置目标几何形状
    seg.setModelType(pcl::SACMODEL_LINE);//SACMODEL_LINE SACMODEL_PLANE注：在这里面修改模型名称
    //分割方法：随机采样法
    seg.setMethodType(pcl::SAC_RANSAC);//SAC_RANSAC
    //最大的迭代的次数
    seg.setMaxIterations(100);
    //设置误差容忍范围
    seg.setDistanceThreshold(0.0005);//注：填写具体数值
    //输入点云
    seg.setInputCloud(Tgt_Cloud_Planar);//输入点云
    //分割点云
    seg.segment(*inliers3, *coefficients3);

    //获取点和删除点2D
    pcl::ExtractIndices<pcl::PointXYZ> extract2;
    extract2.setInputCloud(Tgt_Cloud_Planar);//输入点云
    extract2.setIndices(inliers3);
    extract2.setNegative(false);
    extract2.filter(*line2D);//line2D本次分割出的2D线--
    //删除点（注：如果识别多条直线，则删除已经识别的电力线点云，然后循环）
    extract2.setNegative(true);
    extract2.filter(*cloud_f2D);
    *ptCloud2D = *cloud_f2D;//ptCloud2D本次剩余的点云--

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(line2D, 0, 0, 0);
    viewer00.addPointCloud<pcl::PointXYZ>(line2D, single_color3, "Target clouds90");
    //将直线上的点用线段连起来
    for (int i = 0; i < line2D->size() - 1; i++)
    {
        viewer00.addLine(line2D->points[i], line2D->points[i+1], QString("LineX%1").arg(i).toStdString(), 0);
    }
    viewer00.addLine(line2D->points[line2D->size() - 1], line2D->points[0], QString("LineX%1_%2").arg(line2D->size()).arg(5).toStdString(), 0);

    qDebug()<<"size"<<line2D->size();
    qDebug()<<"003!";
    while (!viewer00.wasStopped())
    {
        viewer00.spinOnce(1);
    }