PCL入门（三）：矩阵变换实现平移和旋转
目录
-
-
-
- 1. pcl中的矩阵变换是什么
- 2. 举例：如何做矩阵变换
-
-
1. pcl中的矩阵变换是什么
通过激光传感器等设备获得的3d点云在拼接成更大场景时，需要对点云数据进行旋转和平移操作。而旋转和平移操作，就可以通过矩阵变换来实现。
- 点的表示
对于点云中的一个点 p p p，其表示为
p = [ x y z 1 ] p=\begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix} p= xyz1
这里， ( x , y , z ) (x, y, z) (x,y,z)为点的空间坐标，第4维的1是什么？其实第4维是人为设置的，方便后面用矩阵乘法表示平移操作。
- 点的平移
如将上面的 p p p点沿着x轴远离原点平移8个单位，直观上就可以得到平移后的结果 p ′ p^{\prime} p′
p ′ = [ x + 8 y z 1 ] p^{\prime}=\begin{bmatrix} x+8 \\ y \\ z \\ 1 \\ \end{bmatrix} p′= x+8yz1
其实等价于下面的矩阵乘法
p ′ = A ⋅ p = [ 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 ] ⋅ [ x y z 1 ] = [ x + a y + b z + c 1 ] p^{\prime}=A\cdot p= \begin{bmatrix} 1 & 0& 0& a \\ 0 & 1& 0& b\\ 0& 0& 1& c \\ 0& 0& 0& 1 \\ \end{bmatrix}\cdot \begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}= \begin{bmatrix} x+a \\ y+b \\ z+c \\ 1 \\ \end{bmatrix} p′=A⋅p= 100001000010abc1 ⋅ xyz1 = x+ay+bz+c1
可以看到，通过给三维点坐标增加一维，可以非常方便的通过矩阵乘法实现点的平移操作。注意到，这里的矩阵 A A A总是 4 × 4 4\times4 4×4方阵。
- 点的旋转
这里指的是点按照某个方向围绕原点旋转 θ \theta θ角度。比如，三维坐标系里绕着x轴逆时针旋转，如下所示（可参考博客《PCL点云库——旋转平移矩阵》）
任意方向旋转任意角度，可通过绕着x轴旋转、绕着y轴旋转、绕着z轴旋转等三个基本旋转操作组合而成。而绕着x轴旋转、绕着y轴旋转、绕着z轴旋转可分别通过矩阵表示。
记绕着x轴逆时针旋转 θ \theta θ角度的操作为 R x ( θ ) R_x(\theta) Rx(θ)，则该操作对应的矩阵为
p ′ = R x ( θ ) ⋅ p = [ 1 0 0 0 0 c o s θ − s i n θ 0 0 s i n θ c o s θ 0 0 0 0 1 ] ⋅ [ x y z 1 ] = [ x y ⋅ c o s θ − z ⋅ s i n θ y ⋅ s i n θ + z ⋅ c o s θ 1 ] 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} p′=Rx(θ)⋅p= 10000cosθsinθ00−sinθcosθ00001 ⋅ xyz1 = xy⋅cosθ−z⋅sinθy⋅sinθ+z⋅cosθ1
记绕着y轴逆时针旋转 θ \theta θ角度的操作为 R y ( θ ) R_y(\theta) Ry(θ)，则该操作对应的矩阵为
p ′ = R y ( θ ) ⋅ p = [ c o s θ 0 s i n θ 0 0 1 0 0 − s i n θ 0 c o s θ 0 0 0 0 1 ] ⋅ [ x y z 1 ] = [ x ⋅ c o s θ + z ⋅ s i n θ y − x ⋅ s i n θ + z ⋅ c o s θ 1 ] 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} p′=Ry(θ)⋅p= cosθ0−sinθ00100sinθ0cosθ00001 ⋅ xyz1 = x⋅cosθ+z⋅sinθy−x⋅sinθ+z⋅cosθ1
记绕着z轴逆时针旋转 θ \theta θ角度的操作为 R z ( θ ) R_z(\theta) Rz(θ)，则该操作对应的矩阵为
p ′ = R z ( θ ) ⋅ p = [ c o s θ − s i n θ 0 0 s i n θ c o s θ 0 0 0 0 1 0 0 0 0 1 ] ⋅ [ x y z 1 ] = [ x ⋅ c o s θ − y ⋅ s i n θ x ⋅ s i n θ + y ⋅ c o s θ z 1 ] 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} p′=Rz(θ)⋅p= cosθsinθ00−sinθcosθ0000100001 ⋅ xyz1 = x⋅cosθ−y⋅sinθx⋅sinθ+y⋅cosθz1
2. 举例：如何做矩阵变换
只要实现矩阵变换，就可以做点云的旋转和平移。PCL库中做矩阵变换有两种方法，分别为Eigen::Matrix4f和Eigen::Affine3f。Eigen::Matrix4f需要手动构建旋转和平移矩阵，较易出错；Eigen::Affine3f则接近于人类操作步骤，将平移操作和旋转操作做成函数，调用即可。
举例：将点云数据先沿着x轴平移2.5，然后绕着z轴逆时针旋转 π 4 \frac{\pi}{4} 4π的角度。
实现主要参考双愚的代码；兔子pcd数据从博客《PCL读取PCD文件的数据》中获取
链接：https://pan.baidu.com/s/1v6mjPjwd7fIqUSjlIGTIGQ
提取码：zspx
- matrix_transform.cpp
#include - 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})
- 运行
./matrix_transform rabbit.pcd
