PCL点云库——旋转平移矩阵

旋转平移矩阵

　　点云摆正、点云配准等点云操作实质上是对点云进行旋转平移，使点云变换至目标位姿。PCL中使用的是矩阵乘列向量的计算方法。
　　下面分别给出了点A(x,y,z)绕X轴、Y轴、Z轴逆时针旋转角度θ的示意图与旋转矩阵（图1、图2、图3），以及“点云库Point Cloud Library”点云按照旋转矩阵实现的程序运行出的效果（图4、图5、图6）。

图1 绕X轴逆时针旋转角度θ 图2 绕Y轴逆时针旋转角度θ 图3 绕Z轴逆时针旋转角度θ 图4 绕X轴逆时针旋转90° 图5 绕Y轴逆时针旋转90° 图6 绕Z轴逆时针旋转90°

　　平移矩阵如下：

　　点A由(x,y,z)移动至点B(x+a,y+b,z+c)，可视为点A沿着向量AB平移至点B，则向量AB=B-A=(x+a,y+b,z+c)-(x,y,z)=(a,b,c)，即是平移矩阵的第四列。例如：将点云的质心移至坐标原点，点A为质心，点B为原点，则向量AB=(-cloudCentroid[0],-cloudCentroid[1],-cloudCentroid[2])，效果如图7所示。

图7 将点云质心移至坐标原点

　　评论中有人问点云怎么绕质心旋转，可能我的回答被“吞”了，导致大家看不到，我在这里回答一下。
　　由于PCL中点云旋转都是绕原点旋转的，你要绕质心旋转，则应该先将点云质心移至原点，绕原点旋转后，再将点云质心移回原来的位置，便达到了点云绕质心旋转的目的。

　　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;
}