二、基于PCL的RANSAC拟合点云中所有直线或平面——3D点云处理系列

RANSAC原理：略。
其他博客大多都是介绍拟合单条直线或平面的代码案例，本文介绍如何拟合多条直线或平面，其实是在单个拟合的基础上接着拟合，以此类推。
注意：步骤中的直线模型是每次随机在点云中取点计算的。
步骤：
1.根据所设参数（点到直线模型的最大距离）把点云分为了内点和外点，对内点进行直线拟合，得到第一次拟合的直线；
2.提取上一步的外点，按照步骤1再次进行内点和外点的划分，对内点拟合直线，得到第二次拟合的直线，并将直线点云叠加到步骤1得到的直线点云中；
3.设置循环终止的条件，重复步骤1-2，最终拟合出点云中所有直线。
多平面拟合的思想如出一辙，概不赘述。

1.RANSAC拟合点云所有直线

//RANSAC拟合多条直线
pcl::PointCloud<pcl::PointXYZ>::Ptr LineFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    //内点点云合并
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lines(new pcl::PointCloud<pcl::PointXYZ>());
    while (cloud->size() > 20)//循环条件
    {
        pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_line);
        ransac.setDistanceThreshold(0.05);	//内点到模型的最大距离
        ransac.setMaxIterations(100);		//最大迭代次数
        ransac.computeModel();				//直线拟合
        //根据索引提取内点
        std::vector<int> inliers;
        ransac.getInliers(inliers);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_line);
        //若内点尺寸过小，不用继续拟合，跳出循环
        if (cloud_line->width * cloud_line->height < 20) {
            break;
        }
        *cloud_lines = *cloud_lines + *cloud_line;
        //pcl::io::savePCDFile(path1+ strcount +"_"+ str + ".pcd", *cloud_line);
        //提取外点
        pcl::PointCloud<pcl::PointXYZ>::Ptr outliers(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointIndices::Ptr inliersPtr(new pcl::PointIndices);
        inliersPtr->indices = inliers;
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud);
        extract.setIndices(inliersPtr);
        extract.setNegative(true);  // 设置为true表示提取外点
        extract.filter(*outliers);
        //pcl::io::savePCDFile("C:/pclpoint/data/cp1_lineout"+str+".pcd", *outliers);
        //cout << outliers->size() << endl;
        cloud->clear();
        *cloud = *outliers;
    }
    return cloud_lines;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

2.RANSAC拟合点云所有平面

pcl::PointCloud<pcl::PointXYZ>::Ptr planeFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    //内点点云合并
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planes(new pcl::PointCloud<pcl::PointXYZ>());
    while (cloud->size() > 100)//循环条件
    {
        //--------------------------RANSAC拟合平面--------------------------
        pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);
        ransac.setDistanceThreshold(0.01);	//设置距离阈值，与平面距离小于0.1的点作为内点
        //ransac.setMaxIterations(100);		//最大迭代次数
        ransac.computeModel();				//执行模型估计
        //-------------------------根据索引提取内点--------------------------
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
        std::vector<int> inplanes;				//存储内点索引的容器
        ransac.getInliers(inplanes);			//提取内点索引
        pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inplanes, *cloud_plane);
        //若内点尺寸过小，不用继续拟合，跳出循环
        if (cloud_plane->width * cloud_plane->height < 100) {
            break;
        }
        *cloud_planes = *cloud_planes + *cloud_plane;
        //提取外点
        pcl::PointCloud<pcl::PointXYZ>::Ptr outplanes(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointIndices::Ptr inplanePtr(new pcl::PointIndices);
        inplanePtr->indices = inplanes;
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud);
        extract.setIndices(inplanePtr);
        extract.setNegative(true);  // 设置为true表示提取外点
        extract.filter(*outplanes);
        //pcl::io::savePCDFile("C:/pclpoint/data/cp1_lineout"+str+".pcd", *outliers);
        //cout << outliers->size() << endl;
        cloud->clear();
        *cloud = *outplanes;
    }
    //----------------------------输出模型参数---------------------------
 /*   Eigen::VectorXf coefficient;
    ransac.getModelCoefficients(coefficient);
    cout << "平面方程为：\n" << coefficient[0] << "x + " << coefficient[1] << "y + " << coefficient[2] << "z + "
        << coefficient[3] << " = 0" << endl;*/
    //返回最终的拟合结果点云
    return cloud_planes;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43