PCL入门（三）：矩阵变换实现平移和旋转-编程知识

这里指的是点按照某个方向围绕原点旋转 $\theta$ 角度。比如，三维坐标系里绕着x轴逆时针旋转，如下所示（可参考博客《PCL点云库——旋转平移矩阵》）
在这里插入图片描述
任意方向旋转任意角度，可通过绕着x轴旋转、绕着y轴旋转、绕着z轴旋转等三个基本旋转操作组合而成。而绕着x轴旋转、绕着y轴旋转、绕着z轴旋转可分别通过矩阵表示。

记绕着x轴逆时针旋转 $\theta$ 角度的操作为 $R_x(\theta)$ ，则该操作对应的矩阵为
$p^{\prime}=R_x(\theta)\cdot p= \begin{bmatrix} 1 & 0& 0& 0 \\ 0 & cos\theta& -sin\theta& 0\\ 0& sin\theta& cos\theta& 0 \\ 0& 0& 0& 1 \\ \end{bmatrix}\cdot \begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}= \begin{bmatrix} x \\ y \cdot cos\theta-z\cdot sin\theta \\ y \cdot sin\theta+z\cdot cos\theta \\ 1 \\ \end{bmatrix}$

记绕着y轴逆时针旋转 $\theta$ 角度的操作为 $R_y(\theta)$ ，则该操作对应的矩阵为
$p^{\prime}=R_y(\theta)\cdot p= \begin{bmatrix} cos\theta & 0& sin\theta& 0 \\ 0 & 1&0 & 0\\ -sin\theta&0 & cos\theta& 0 \\ 0& 0& 0& 1 \\ \end{bmatrix}\cdot \begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}= \begin{bmatrix} x \cdot cos\theta+z\cdot sin\theta \\ y \\ -x \cdot sin\theta+z\cdot cos\theta \\ 1 \\ \end{bmatrix}$

记绕着z轴逆时针旋转 $\theta$ 角度的操作为 $R_z(\theta)$ ，则该操作对应的矩阵为
$p^{\prime}=R_z(\theta)\cdot p= \begin{bmatrix} cos\theta & -sin\theta& 0& 0 \\ sin\theta & cos\theta&0& 0\\ 0& 0& 1& 0 \\ 0& 0& 0& 1 \\ \end{bmatrix}\cdot \begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}= \begin{bmatrix} x \cdot cos\theta-y\cdot sin\theta \\ x \cdot sin\theta+y\cdot cos\theta \\ z \\ 1 \\ \end{bmatrix}$

2. 举例：如何做矩阵变换

只要实现矩阵变换，就可以做点云的旋转和平移。PCL库中做矩阵变换有两种方法，分别为Eigen::Matrix4f和Eigen::Affine3f。Eigen::Matrix4f需要手动构建旋转和平移矩阵，较易出错；Eigen::Affine3f则接近于人类操作步骤，将平移操作和旋转操作做成函数，调用即可。

举例：将点云数据先沿着x轴平移2.5，然后绕着z轴逆时针旋转 $\frac{\pi}{4}$ 的角度。

实现主要参考双愚的代码；兔子pcd数据从博客《PCL读取PCD文件的数据》中获取
链接：https://pan.baidu.com/s/1v6mjPjwd7fIqUSjlIGTIGQ
提取码：zspx

matrix_transform.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>int main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());if (pcl::io::loadPCDFile(argv[1], *source_cloud) < 0){std::cout << "Error loading point cloud " << argv[1] << std::endl;return -1;}float theta = M_PI / 4; // 旋转角度//方法一: Matrix4fEigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();transform_1(0, 0) = std::cos(theta);transform_1(0, 1) = -std::sin(theta);transform_1(1, 0) = std::sin(theta);transform_1(1, 1) = std::cos(theta);transform_1(0, 3) = 2.5;printf("method 1: using a matrix4f\n");std::cout << transform_1 << std::endl;//方法二: Affine3fEigen::Affine3f transform_2 = Eigen::Affine3f::Identity();transform_2.translation() << 2.5, 0.0, 0.0;transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));printf("method 2: using an affine3f\n");std::cout << transform_2.matrix() << std::endl;// excuting the transformation（执行平移和旋转操作）pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());pcl::transformPointCloud(*source_cloud, *transformed_cloud, transform_2);// visualization（可视化）pcl::visualization::PCLVisualizer viewer("Matrix transformation example");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler(source_cloud, 255, 255, 255);viewer.addPointCloud(source_cloud, source_cloud_color_handler, "original_cloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler(transformed_cloud, 230, 20, 220);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);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()){viewer.spinOnce();}return 0;}

CMakeLists.txt

cmake_minimum_required(VERSION 2.6 FATAL_ERROR)project(pcl-matrix_transform)find_package(PCL 1.7 REQUIRED)add_executable (matrix_transform matrix_transform.cpp)
target_link_libraries (matrix_transform ${PCL_LIBRARIES})