PCL：RANSAC拟合多条直线，输出直线方程

#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h> // 拟合直线
#include <pcl/filters/extract_indices.h>
#include <iostream>    
using namespace std;

//RANSAC拟合多条直线
pcl::PointCloud<pcl::PointXYZ>::Ptr LineFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    //内点点云合并
    int i(1);  
    pcl::visualization::PCLVisualizer viewer("pass_through");
    viewer.addPointCloud(cloud, "pc");
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lines(new pcl::PointCloud<pcl::PointXYZ>());
 
    while (cloud->size() > 20)//循环条件
    {
        cout << "点云点数为：" << cloud->points.size() << endl;
        pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
        pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_line);
        ransac.setDistanceThreshold(0.15);	//内点到模型的最大距离
        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 < 10) {
            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;
        Eigen::VectorXf coef;
        ransac.getModelCoefficients(coef);
        cout << "直线方程为：\n"
            << "   (x - " << coef[0] << ") / " << coef[3]
            << " = (y - " << coef[1] << ") / " << coef[4]
            << " = (z - " << coef[2] << ") / " << coef[5] << endl;
        pcl::PointXYZ p1(-1000,(-1000-coef[0])/coef[3]*coef[4]+coef[1], (-1000 - coef[0]) / coef[3] * coef[5] + coef[2]);
        pcl::PointXYZ p2(1000,(1000-coef[0])/coef[3]*coef[4]+coef[1], (1000 - coef[0]) / coef[3] * coef[5] + coef[2]);
        i++;        
        viewer.addLine(p1,p2, "line"+std::to_string(i));       
    }
    while (!viewer.wasStopped())
    {
        viewer.spinOnce();
    }
    return cloud_lines;
}

int main(int argc, char** argv) {
    // 加载点云模型
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);  
    pcl::io::loadPCDFile("边角直线.pcd", *cloud);
    
    LineFitting(cloud);
    system("pause");
    return 0;

}

原点云：

                

运行效果：

若直线数目不符合预期，则修改参数：

这两位大佬写的原代码，参考资料：PCL拟合多条直线的代码-学习解读

                                                          PCL Ransac拟合直线