《PCL从零开始》-1.读取pcd文件并对原始点云进行矩阵(旋转和平移)变换

首先默认你环境已经配置好了
运行环境为:vs2015 + pcl1.8.1 +win10
开始 实例 代码参考官方文档 + 被我忘记的博客

首先加载pcl 库 我也不知道库关系 先用起来

//加载头文件
#include 
#include 
#include 
#include 
#include 
#include 
#include 

加载点云 pcd 文件 你可以自己改ply文件
加载的是rabbit.pcd 比较经典

//定义一个句柄用来存放待读取的点云。
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);

	//读取点云    读取加判断  
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("rabbit.pcd", *source_cloud) == -1) //* load the file
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n");	
		return (-1);
	}

	//输出pcd文件 点的数目
	std::cout << "Loaded "
		<< source_cloud->width * source_cloud->height
		<< " data points from test_pcd.pcd with the following fields: "
		<< std::endl;
	
	//打印每个点的  x  y  z  
	for (size_t i = 0; i < source_cloud->points.size(); ++i)
		std::cout << source_cloud->points[i].x
		<< " " << source_cloud->points[i].y
		<< " " << source_cloud->points[i].z << std::endl;
	

创建变换矩阵 自己补一下知识

/* Reminder: how transformation matrices work 
	65
	66           |-------> This column is the translation
	67    | 1 0 0 x |  \
	68    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
	69    | 0 0 1 z |  /
	70    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)
	*/

	//创建矩阵的方法有两种  第一 :手动创建Matrix4f  transform_1
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
	float theta = M_PI / 4; // The angle of rotation in radians
	transform_1(0, 0) = std::cos(theta);
	transform_1(0, 1) = -sin(theta);
	transform_1(1, 0) = sin(theta);
	transform_1(1, 1) = std::cos(theta);
	transform_1(0, 3) = 2.5;//x轴产生 2.5的位移
	printf("Method #1: using a Matrix4f\n");
	std::cout << transform_1 << std::endl;//打印矩阵的样子



	//创建矩阵的方法有两种  第二 :创建Affine3f  transform_2
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	transform_2.translation() << 2.5, 0.0, 0.0;  //x轴平移
	transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); //绕Z轴旋转 pi/4度

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

开始操作了啊 ~!!!!

//对原始点云 进行矩阵操作 

	//创建点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());

	//执行操作   对原始点云 进行操作  输出 transformed_cloud   使用transform_2矩阵
	pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

保存点云 写入

//写入点云
	pcl::io::savePCDFileASCII("test_pcd.pcd", *transformed_cloud);

显示点云的样子


	//显示点云样子
	pcl::visualization::PCLVisualizer viewer("Matrix transformation example");

	//定义原始点云的颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255);

	//将上个语句的颜色 加入到pcl  viewer中去 
	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
	
	//加入到pcl  viewer中去 
	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();
		
	}

总体代码如下

//加载pcd  或者 ply文件  使用平移和转动矩阵 对  点云进行操作 

//加载头文件
#include 
#include 
#include 
#include 
#include 
#include 
#include 

//主程序
int main()
{

	//定义一个句柄用来存放待读取的点云。
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);

	//读取点云    读取加判断  
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("rabbit.pcd", *source_cloud) == -1) //* load the file
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n");	
		return (-1);
	}

	//输出pcd文件 点的数目
	std::cout << "Loaded "
		<< source_cloud->width * source_cloud->height
		<< " data points from test_pcd.pcd with the following fields: "
		<< std::endl;
	/*
	//打印每个点的  x  y  z  
	for (size_t i = 0; i < source_cloud->points.size(); ++i)
		std::cout << source_cloud->points[i].x
		<< " " << source_cloud->points[i].y
		<< " " << source_cloud->points[i].z << std::endl;
	*/

	/* Reminder: how transformation matrices work 
	65
	66           |-------> This column is the translation
	67    | 1 0 0 x |  \
	68    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
	69    | 0 0 1 z |  /
	70    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)
	*/

	//创建矩阵的方法有两种  第一 :手动创建Matrix4f  transform_1
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
	float theta = M_PI / 4; // The angle of rotation in radians
	transform_1(0, 0) = std::cos(theta);
	transform_1(0, 1) = -sin(theta);
	transform_1(1, 0) = sin(theta);
	transform_1(1, 1) = std::cos(theta);
	transform_1(0, 3) = 2.5;//x轴产生 2.5的位移
	printf("Method #1: using a Matrix4f\n");
	std::cout << transform_1 << std::endl;//打印矩阵的样子



	//创建矩阵的方法有两种  第二 :创建Affine3f  transform_2
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	transform_2.translation() << 2.5, 0.0, 0.0;  //x轴平移
	transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); //绕Z轴旋转 pi/4度

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




	//对原始点云 进行矩阵操作 

	//创建点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());

	//执行操作   对原始点云 进行操作  输出 transformed_cloud   使用transform_2矩阵
	pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);

	//写入点云
	pcl::io::savePCDFileASCII("test_pcd.pcd", *transformed_cloud);

	

	//显示点云样子
	pcl::visualization::PCLVisualizer viewer("Matrix transformation example");

	//定义原始点云的颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255);

	//将上个语句的颜色 加入到pcl  viewer中去 
	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
	
	//加入到pcl  viewer中去 
	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();
		
	}
	return 0;
}

《PCL从零开始》-1.读取pcd文件并对原始点云进行矩阵(旋转和平移)变换_第1张图片
《PCL从零开始》-1.读取pcd文件并对原始点云进行矩阵(旋转和平移)变换_第2张图片

你可能感兴趣的:(PCL,c++)