二、基于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;
}

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;
}