PCL单个平面拟合

#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/extract_indices.h>	//根据索引提取内点--方法2所需头文件

using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main()
{
	//-------------------------- 加载点云 --------------------------
	cout << "->正在加载点云..." << endl;
	PointCloudT::Ptr cloud(new PointCloudT);
	if (pcl::io::loadPCDFile("raw.pcd", *cloud) < 0)
	{
		PCL_ERROR("\a->点云文件不存在！\n");
		system("pause");
		return -1;
	}
	cout << "->加载点云点数：" << cloud->points.size() << endl;
	//========================== 加载点云 ==========================

	//-------------------------- 模型估计 --------------------------
	cout << "->正在估计平面..." << endl;
	pcl::SampleConsensusModelPlane<PointT>::Ptr model_plane(new pcl::SampleConsensusModelPlane<PointT>(cloud));	//选择拟合点云与几何模型
	pcl::RandomSampleConsensus<PointT> ransac(model_plane);	//创建随机采样一致性对象
	ransac.setDistanceThreshold(0.01);	//设置距离阈值，与平面距离小于0.01的点作为内点
	ransac.computeModel();				//执行模型估计

	PointCloudT::Ptr cloud_plane(new PointCloudT);

	//---------- 根据索引提取内点 ----------
	///方法1
	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引
	pcl::copyPointCloud<PointT>(*cloud, inliers, *cloud_plane);

	///方法2,需要pcl/filters/extract_indices.h头文件，较为繁琐。
	//pcl::PointIndices pi;
	//ransac.getInliers(pi.indices);
	//pcl::IndicesPtr index_ptr(new vector<int>(pi.indices));/// 将自定义的点云索引数组pi进行智能指针的转换
	//pcl::ExtractIndices<PointT> extract;
	//extract.setInputCloud(cloud);
	//extract.setIndices(index_ptr);
	//extract.setNegative(false);	/// 提取索引外的点云，若设置为true,则与copyPointCloud提取结果相同
	//extract.filter(*cloud_plane);
	//========== 根据索引提取内点 ==========

	/// 输出模型参数Ax+By+Cz+D=0
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	cout << "平面方程为：\n"
		<< coefficient[0] << "x + "
		<< coefficient[1] << "y + "
		<< coefficient[2] << "z + "
		<< coefficient[3] << " = 0"
		<< endl;
	//========================== 模型估计 ==========================

	//----------------------- 可视化拟合结果 -----------------------
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

	viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");													//添加原始点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

	viewer->addPointCloud<pcl::PointXYZ>(cloud_plane, "plane");												//添加平面点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "plane");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");	//点的大小

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	//======================= 可视化拟合结果 =======================

	return 0;
}

1.8.1版本 VS2019