【转】 PCL 利用RANSAC算法拟合平面并旋转

说明

最近的项目用到了PCL里的旋转平面，然后又需要按一定的角度旋转，因此对于给定一个平面的数据集，需要利用RANSAC算法拟合出平面方程，然后根据需要，求出相应的角度并按一定的方式旋转，程序大体上分为两个功能，一个是拟合平面求方程，一个是旋转点云。

方法

拟合平面有两种方法，最小二乘法，和RANSAC算法。PCL库中SACSegmentation类中用的是RANSAC的算法来拟合平面的。关于为什么用RANSAC算法而不用最小二乘法，这篇博客里有详细的讲解。这里就不在赘述了。
关于旋转，PCL官方库里提供了两种方法，这里用的是第二种方法，比第一种方法
形式上更简单并且不容易出错。想了解第一种方法的同学可以看一下官方库，里面
的解析很详细，很好理解。使用矩阵变换点云

代码

#include<iostream>
#include <pcl/ModelCoefficients.h>
#include<pcl\io\pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace std;
typedef pcl::PointXYZ PointType;//旋转平面
void rotate(double A, double B, double C,double D)
{
	cout << "旋转点云" << endl;
	char * pcdFilePath = "D:/rotate_f.pcd";

	// Load file | Works with PCD
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());

	pcl::io::loadPCDFile(pcdFilePath, *source_cloud) < 0;
	//这里的旋转用的官网里的第二种方法
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	float theta = (M_PI * B) / (4 * C); // The angle of rotation in radians 以弧度表示角度
	/*这里theta的值应该是多少取决于你想旋转多少度，然后根据传过来的参数求出相应的角度，我这里是旋转后与x-y面平行*/
										
	transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitX()));//theta为正时 逆时针旋转，theta为负时 顺时针旋转
	//这里是绕x轴旋转，绕Z轴旋转时改为UnitZ，y轴同理。																		

	// Print the transformation
	printf("\nMethod #2: using an Affine3f\n");
	std::cout << transform_2.matrix() << std::endl;

	// Executing the transformation
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
	// You can either apply transform_1 or transform_2; they are the same
	pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

	// Visualization
	printf("\nPoint cloud colors :  white  = original point cloud\n"
		"                        red  = transformed point cloud\n");
	pcl::visualization::PCLVisualizer viewer("Matrix transformation example");

	// Define R,G,B colors for the point cloud
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255);
	// We add the point cloud to the viewer and pass the color handler
	viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(transformed_cloud, 230, 20, 20); // Red
	viewer.addPointCloud(transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

	viewer.addCoordinateSystem(1.0, "cloud", 0);
	viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");

	while (!viewer.wasStopped()) { // Display the visualiser until 'q' key is pressed
		viewer.spinOnce();
	}
}
int main()
{
	double A, B, C, D;//用于接收平面方程的参数
	pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
	pcl::PCDReader reader;   //读取PCD
	reader.read("D:/mt1.pcd", *cloud);
	//创建一个模型参数对象，用于记录拟合结果
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//inliers通过点云序号来记录模型内点
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	//创建一个分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//Optional,设置结果平面展示的点是分割掉的点还是分割剩下的点
	seg.setOptimizeCoefficients(true);//flase 展示的是分割剩下的点
	//Mandatory-设置目标几何形状
	seg.setModelType(pcl::SACMODEL_PLANE);
	//分割方法：随机采样法
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置误差容忍范围，也就是阈值
	seg.setDistanceThreshold(0.01);
	//输入点云
	seg.setInputCloud(cloud);
	//分割点云
	seg.segment(*inliers, *coefficients);
	
	for (size_t i = 0; i < coefficients->values.size(); ++i)
	{
		cout << "  values[" << i << "]: ";
		cout << "  " << coefficients->values[i] << std::endl;
	}
	A = coefficients->values[0];
	B = coefficients->values[1];
	C = coefficients->values[2];
	D = coefficients->values[3];
	/*平面方程：Ax+By+Cz+D=0  上面的输出中A=values[0],B=values[1],C=values[2],D=values[3]*/

	rotate(A, B, C, D);//旋转平面

	system("pause");
}

效果图

x、y、z轴分别对应红绿蓝轴，白色点云为旋转之前的，红色为旋转之后的点云，
可以看到，旋转后点云与x-y面平行。达到了预期目的。代码里用到的数据集是一个坐便器的平面点云，如果需要可以和我联系。用其他的数据集也可以。
有什么疑问或者博客有任何问题欢迎留言交流。

---------------------
作者：FeifeiYa！
来源：CSDN
原文：https://blog.csdn.net/qq_43622114/article/details/102691180
版权声明：本文为作者原创文章，转载请附上博文链接！
内容解析By：CSDN,CNBLOG博客文章一键转载插件