PCL圆形平面圆心坐标及半径测量

通过圆形平面的点云数据获取该平面圆心坐标及圆半径。

一、获取点云数据

pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("文件路径", *cloud) < 0)
	{
		PCL_ERROR("\a点云文件不存在！\n");
		system("pause");
		return -1;
	}
	cout << "->加载数据点的个数：" << cloud->points.size() << endl;

二、采用RANSAC方法定位平面

pcl::PointIndices::Ptr  index_plane(new pcl::PointIndices);
	pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> sacSegmentationFromNormals;
	pcl::ModelCoefficients::Ptr mdelCoefficients_plane(new pcl::ModelCoefficients);
	sacSegmentationFromNormals.setInputCloud(cloud);
	sacSegmentationFromNormals.setOptimizeCoefficients(true);//设置对估计的模型系数需要进行优化
	sacSegmentationFromNormals.setModelType(pcl::SACMODEL_NORMAL_PLANE); //设置分割模型
	sacSegmentationFromNormals.setNormalDistanceWeight(0.1);//设置表面法线权重系数
	sacSegmentationFromNormals.setMethodType(pcl::SAC_RANSAC);//设置采用RANSAC作为算法的参数估计方法
	sacSegmentationFromNormals.setMaxIterations(500); //设置迭代的最大次数
	sacSegmentationFromNormals.setDistanceThreshold(0.03); //设置内点到模型的距离允许最大值
	sacSegmentationFromNormals.setInputCloud(cloud);
	sacSegmentationFromNormals.setInputNormals(normals);
	sacSegmentationFromNormals.segment(*index_plane, *mdelCoefficients_plane);
	std::cerr << "Plane coefficients: " << *mdelCoefficients_plane << std::endl;
	
	pcl::ExtractIndices<pcl::PointXYZ> extractIndices;
	pcl::PointCloud<pcl::PointXYZ>::Ptr  cloud_p(new pcl::PointCloud<pcl::PointXYZ>);
	extractIndices.setInputCloud(cloud);
	extractIndices.setIndices(index_plane);
	extractIndices.setNegative(false);
	extractIndices.filter(*cloud_p);

三、把点云投影到第二步得到的平面上

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(mdelCoefficients_plane);
	proj.filter(*cloud_projected);

四、提取投影平面的轮廓

pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_boundary = PointCloudBoundary2(cloud_projected);
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_boundary1(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PassThrough<pcl::PointXYZRGB> pass1; // 声明直通滤波
	pass1.setInputCloud(cloud_boundary); 			// 传入点云数据
	pass1.setFilterFieldName("z"); 		// 设置操作的坐标轴
	pass1.setFilterLimits(0.12, 2.5); 	// 设置坐标范围
	pass1.setNegative(false); // 保留数据函数
	pass1.filter(*cloud_boundary1);  		// 进行滤波输出

五、对轮廓进行圆形拟合，得到圆心坐标、圆的直径和圆法向量

pcl::SampleConsensusModelCircle3D<pcl::PointXYZRGB>::Ptr model_circle3D(new pcl::SampleConsensusModelCircle3D<pcl::PointXYZRGB>(cloud_boundary1));
	pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_circle3D);
	ransac.setDistanceThreshold(0.05);	// 距离阈值，与模型距离小于0.05的点作为内点
	ransac.setMaxIterations(500);		        // 最大迭代次数
	ransac.computeModel();				        // 拟合3D圆
	pcl::IndicesPtr inliers(new vector <int>());// 存储内点索引的向量
	ransac.getInliers(*inliers);			    // 提取内点对应的索引

	pcl::PointCloud<pcl::PointXYZ>::Ptr circle_3D(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZRGB>(*cloud_boundary1, *inliers, *circle_3D);
	cout << "->圆上数据点的个数：" << circle_3D->points.size() << endl;

	Eigen::VectorXf coeff;
	ransac.getModelCoefficients(coeff);

	cout << "圆心的x坐标：" << coeff[0] << "\n"
		<< "圆心的y坐标：" << coeff[1] << "\n"
		<< "圆心的z坐标：" << coeff[2] << "\n"
		<< "圆的半径：" << coeff[3] << "\n"
		<< "圆法向量：" << coeff[4] << ","
		<< coeff[5] << ","
		<< coeff[6] << endl;