PCL实现点云关于空间中任意平面对称

mengzhilv11

已于 2022-08-04 17:24:44 修改

阅读量1.5k

点赞数 1

文章标签：平面 c++ 学习算法

于 2022-06-05 12:44:53 首次发布

本文链接：https://blog.csdn.net/mengzhilv11/article/details/125130757

版权

起因：项目需要用到点云的对称，网上没找到代码，查找相关资料后自己写了一个。

实现：基于旋转矩阵，详细请看：(14条消息) 数学笔记（三）之镜面矩阵_tkokof1的博客-CSDN博客_镜面变换矩阵

其中D为平面到原点距离。 A,B,C,D为确定三维空间中平面的系数。注：RANSAC拟合平面后获得的好像就是法向量，不知道是我这一片点云特殊还是怎么滴，有人知道吗？

核心函数：transformPointCloud起到一个旋转点云的作用，cloud_filtered为输入点云，transformedCloud输出的旋转后点云，n为旋转矩阵,以下为n和旋转函数。 $\begin{bmatrix} 1- 2*x_n{}^{2} & -2x_{n}* y_{n} &-2x_{n}*z_{n} &-2*x_{n}*D \\ -2x_{n}* y_{n} & 1- 2*y_n{}^{2} &-2y_{n}*z_{n}&-2*x_{n}*D \\ -2x_{n}*z_{n} &-2y_{n}*z_{n} & 1- 2*z_n{}^{2} &-2*z_{n}*D \\ 0 & 0 & 0 & 1 \end{bmatrix}$

pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);

以下为源码：

因为我的这个点云文件包含了地面，所以我先用ransac分割将地面去除了，我程序提供了两种对称方式，一种是关于ransac拟合出来的平面对称的，一种是关于自己给定的任意平面对称，后面的结果是关于任意平面对称的结果。想用关于ransac拟合得到平面镜像的话，将代码里面相关部分解除注释就行，两种我都试过了，都能用。点云文件不方便提供。博主为半路出家，代码能力不行，有啥问题或者改进代码的请评论区留言。

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/project_inliers.h>
#include <math.h>
#include <pcl/common/transforms.h>
using namespace std;
typedef pcl::PointXYZ PointType;

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data
	pcl::PCDReader reader;//PCD文件读取对象
	// Replace the path below with the path where you saved your file
	reader.read<pcl::PointXYZ>("XXX.pcd", *cloud);//“”内内容修改成你想分割的点云的文件名

		//------------------------------------------PCL分割框架--------------------------------------------------------   
		//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
		pcl::ModelCoefficients::Ptr coefficient(new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
		// 创建分割对象
		pcl::SACSegmentation<pcl::PointXYZ> seg;
		// 可选择配置，设置模型系数需要优化
		seg.setOptimizeCoefficients(true);
		// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
		seg.setModelType(pcl::SACMODEL_PLANE);//设置模型类型
		seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型
		seg.setMaxIterations(500);//最大迭代次数
		seg.setDistanceThreshold(0.005);//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
		seg.setInputCloud(cloud);//输入所需要分割的点云对象
		//引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
		seg.segment(*inliers, *coefficient);
		//---------以下为关于ransac拟合平面对称的旋转矩阵相关参数------------
	/*	float a= coefficient->values[0];
		float b= coefficient->values[1];
		float c= coefficient->values[2]; 
		float d= coefficient->values[3];
		cout << a << endl;
		cout << b << endl;
		cout << c << endl;
		cout << d << endl;
		float d1 = d;
		if (d < 0)
		{
			d1 = -d;
		}
		float e = sqrt(a*a + b * b + c * c);
		float r = d1 / e ;
		cout << "f" << endl;*/
		//----------------------------------------------------

		// extract ground
		// 从点云中抽取分割的处在平面上的点集
		pcl::ExtractIndices<pcl::PointXYZ> extractor;//点提取对象
		extractor.setInputCloud(cloud);
		extractor.setIndices(inliers);
		extractor.setNegative(true);
		extractor.filter(*cloud_filtered);

		// --------------点云关于ransac拟合出来的平面对称------------------
		/*pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
		Eigen::Matrix4f n;//旋转矩阵
		n << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,
			-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,
			-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,
			0, 0, 0, 1;
		cout << n << endl;
		pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);
		//------------------------------------------------------------------

*/
		//---------------点云关于任意平面对称---------------------------------------------------
		pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZ>);
		float A = 1, B = 2, C = 3, D = -4;//A,B,C,D为平面Ax+By+Cz=D
		/*if(D<0)
		{
			D = -D;
			cout << D << endl;
		}*/
		float e = sqrt(A * A + B * B + C * C);//为了后续将平面法向量转化为单位向量
		float a = A / e, b = B / e, c = C / e;
		float r = D / e;

		Eigen::Matrix4f n;//旋转矩阵n
		n << 1 - 2 * a*a, -2 * a*b, -2 * a*c, -2 * a*r,
			-2 * a*b, 1 - 2 * b*b, -2 * b*c, -2 * b*r,
			-2 * a*c, -2 * b*c, 1 - 2 * c*c, -2 * c*r,
			0, 0, 0, 1;
		cout << n << endl;
		pcl::transformPointCloud(*cloud_filtered, *transformedCloud, n);

	   //------------------创建对称面以便后续可视化对称面-----------------------
		pcl::ModelCoefficients plane_coeff;
		plane_coeff.values.resize(4);    // We need 4 values
		plane_coeff.values[0] = A;
		plane_coeff.values[1] = B;
		plane_coeff.values[2] = C;
		plane_coeff.values[3] = D;
		//----------------------------------------------------------------------

		//--------------------------------------可视化--------------------------
		pcl::visualization::PCLVisualizer viewer;
		pcl::visualization::PointCloudColorHandlerCustom<PointType> color(cloud_filtered, 255, 0, 0);
		viewer.addPointCloud(cloud_filtered, color, "cloud");
		pcl::visualization::PointCloudColorHandlerCustom<PointType> color2(transformedCloud, 0, 100, 0);
		viewer.addPointCloud(transformedCloud, color2, "transform");
		viewer.addCoordinateSystem();
		viewer.addPlane(plane_coeff);//添加对称面

	   while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	}

	
	return 0;

}

以下为运行结果：