PCL点云的旋转平移矩阵

目录

（一）左乘右乘

1.左乘

2.右乘

（二）旋转平移矩阵公式

1.绕X轴逆时针旋转角度θ

2.绕Y轴逆时针旋转角度θ

3.绕Z轴逆时针旋转角度θ

4.平移

（三）旋转平移矩阵的逆

（四）PCL实现

---

旋转平移变换矩阵是先旋转再平移的T=rotation*translation。 在固定坐标系下面，我们用左乘的方法；在非固定的坐标系下面，用右乘。

（一）左乘右乘

1.左乘

每次采取的变化，都是围绕着基座标的话，我们便可以用“左乘”，比如说，我们想要先进行一次旋转变化A，然后再进行一次平移变化B，采用的变化就是B·A的矩阵描述总变换。

此时平移变化B是在基坐标上进行的。

2.右乘

在第一个变化生效的时候，第二个变化的基座标系已经发生了改变，需要基于第二的坐标系再变化，即右乘为A·B。

此时平移变化B是在第一次旋转变化后的坐标上进行的。

（二）旋转平移矩阵公式

1.绕X轴逆时针旋转角度θ

2.绕Y轴逆时针旋转角度θ

3.绕Z轴逆时针旋转角度θ

4.平移

（三）旋转平移矩阵的逆

已知坐标系B相对于A的位姿，要想求坐标系A相对于B的位姿，则需要求齐次变换矩阵的逆，即：

下面展示一个示例，求T的逆矩阵：

（四）PCL实现

//****变换点云位置****//

#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("点云库字样.pcd", *cloud);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);

	//Eigen::Matrix4f rotation_x = Eigen::Matrix4f::Identity();//定义绕X轴的旋转矩阵，并初始化为单位阵
	//double angle_x = M_PI / 2;//旋转90°
	//rotation_x(1, 1) = cos(angle_x);
	//rotation_x(1, 2) = -sin(angle_x);
	//rotation_x(2, 1) = sin(angle_x);
	//rotation_x(2, 2) = cos(angle_x);
	//pcl::transformPointCloud(*cloud, *cloud_transformed, rotation_x);

	//Eigen::Matrix4f rotation_y = Eigen::Matrix4f::Identity();//定义绕Y轴的旋转矩阵，并初始化为单位阵
	//double angle_y = M_PI / 2;//旋转90°
	//rotation_y(0, 0) = cos(angle_y);
	//rotation_y(0, 2) = sin(angle_y);
	//rotation_y(2, 0) = -sin(angle_y);
	//rotation_y(2, 2) = cos(angle_y);
	//pcl::transformPointCloud(*cloud, *cloud_transformed, rotation_y);

	//Eigen::Matrix4f rotation_z = Eigen::Matrix4f::Identity();//定义绕Z轴的旋转矩阵，并初始化为单位阵
	//double angle_z = M_PI / 2;//旋转90°
	//rotation_z(0, 0) = cos(angle_z);
	//rotation_z(0, 1) = -sin(angle_z);
	//rotation_z(1, 0) = sin(angle_z);
	//rotation_z(1, 1) = cos(angle_z);
	//pcl::transformPointCloud(*cloud, *cloud_transformed, rotation_z);

	Eigen::Vector4f cloudCentroid;
	pcl::compute3DCentroid(*cloud, cloudCentroid);//计算点云质心
	Eigen::Matrix4f translation = Eigen::Matrix4f::Identity();//定义平移矩阵，并初始化为单位阵
	translation(0, 3) = -cloudCentroid[0];
	translation(1, 3) = -cloudCentroid[1];
	translation(2, 3) = -cloudCentroid[2];
	pcl::transformPointCloud(*cloud, *cloud_transformed, translation);
	
	//for (int i = 0; i < cloud->size(); i++)//点云中的每个点减去质心，也可实现平移
	//{
	//	pcl::PointXYZ temp;
	//	temp.x = cloud->points[i].x - cloud_Centroid[0];
	//	temp.y = cloud->points[i].y - cloud_Centroid[1];
	//	temp.z = cloud->points[i].z - cloud_Centroid[2];
	//	cloud_transformed->push_back(temp);
	//}

	//可视化
	pcl::visualization::PCLVisualizer viewer("PCLVisualizer");
	viewer.initCameraParameters();

	int v1(0);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.addCoordinateSystem(200, v1);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v1);
	viewer.addText("Cloud before transforming", 10, 10, "v1 test", v1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 0, 255, 0);
	viewer.addPointCloud(cloud, color, "cloud", v1);

	int v2(0);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer.addCoordinateSystem(200, v2);
	viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v2);
	viewer.addText("Cloud after transforming", 10, 10, "v2 test", v2);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_transformed(cloud_transformed, 0, 255, 0);
	viewer.addPointCloud(cloud_transformed, color_transformed, "cloud_transformed", v2);

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