Table of contents
1. What is matrix transformation in pcl?
When the 3D point cloud obtained through laser sensors and other equipment is spliced into a larger scene, the point cloud data needs to be rotated and translated. Rotation and translation operations can be achieved through matrix transformation.
- Representation of points
For a point pp in the point cloudp , which is expressed as
p = [ xyz 1 ] p=\begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}p=
xyz1
这り（ x , y , z ) (x, y, z)(x,y,z ) is the spatial coordinate of the point. What is 1 in the fourth dimension? In fact, the fourth dimension is artificially set to facilitate later use of matrix multiplication to express translation operations.
- point translation
For example, the above ppPoint p is translated 8 units away from the origin along the x-axis. Intuitively, the result after translation can be obtainedp ′ p^{\prime}p′
p ′ = [ x + 8 y z 1 ] p^{\prime}=\begin{bmatrix} x+8 \\ y \\ z \\ 1 \\ \end{bmatrix} p′=
x+8yz1
In fact, it is equivalent to the following matrix multiplication
p ′ = A ⋅ p = [ 1 0 0 a 0 1 0 b 0 0 1 c 0 0 0 1 ] ⋅ [ xyz 1 ] = [ x + ay + bz + 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
It can be seen that by adding one dimension to the three-dimensional point coordinates, the translation operation of the point can be realized very conveniently through matrix multiplication. Note that the matrix AA hereA is always4 × 4 4\times44×4 square array.
- point rotation
This refers to the point rotating around the origin in a certain direction θ \thetaθ angle. For example, in a three-dimensional coordinate system, rotate counterclockwise around the x-axis, as shown below (please refer to the blog"PCL Point Cloud Library - Rotation and Translation Matrix")
to rotate in any direction at any angle. You can rotate around the x-axis and around the y It is a combination of three basic rotation operations: axis rotation and rotation around the z-axis. Rotation around the x-axis, rotation around the y-axis, and rotation around the z-axis can be represented by matrices respectively.
Remember to rotate counterclockwise around the x-axis θ \thetaThe operation of θ angle isR x ( θ ) R_x(\theta)Rx( θ ) ,determine the inverse equation
p ′ = R x ( θ ) ⋅ p = [ 1 0 0 0 0 cos θ − sin θ 0 0 sin θ cos θ 0 0 0 0 1 ] ⋅ [ xyz = [ xy ⋅ cos θ − z ⋅ sin θ y ⋅ sin θ + z ⋅ cos θ 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 \\ \\ \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( i )⋅p=
10000cosθsinθ00−sinθcosθ00001
⋅
xyz1
=
xy⋅cosθ−z⋅sinθy⋅sinθ+z⋅cosθ1
Remember to rotate counterclockwise around the y-axis θ \thetaThe operation of θ angle isR y ( θ ) R_y(\theta)Ry( θ ) ,determine the inverse equation
p ′ = R y ( θ ) ⋅ p = [ cos θ 0 sin θ 0 0 1 0 0 − sin θ 0 cos θ 0 0 0 0 1 ] ⋅ [ xyz = [ x ⋅ cos θ + z ⋅ sin θ y − x ⋅ sin θ + z ⋅ cos θ 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 syn\theta \\ y \\ -x \cdot sin\theta+z\cdot cos\theta \\ \\\end{bmatrix}p′=Ry( i )⋅p=
cosθ0−sinθ00100sinθ0cosθ00001
⋅
xyz1
=
x⋅cosθ+z⋅sinθy−x⋅sinθ+z⋅cosθ1
Remember to rotate counterclockwise around the z-axis θ \thetaThe operation of θ angle isR z ( θ ) R_z(\theta)Rz( θ ) ,determine the inverse equation
p ′ = R z ( θ ) ⋅ p = [ cos θ − sin θ 0 0 sin θ cos θ 0 0 0 0 1 0 0 0 0 1 ] ⋅ [ xyz = [ x ⋅ cos θ − y ⋅ sin θ x ⋅ sin θ + y ⋅ cos θ 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 \\ \\ \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( i )⋅p=
cosθsinθ00−sinθcosθ0000100001
⋅
xyz1
=
x⋅cosθ−y⋅sinθx⋅sinθ+y⋅cosθz1
2. Example: How to do matrix transformation
As long as the matrix transformation is implemented, the rotation and translation of the point cloud can be done. There are two methods for matrix transformation in the PCL library, namely Eigen::Matrix4fand Eigen::Affine3f. Eigen::Matrix4fIt is necessary to manually construct the rotation and translation matrices, which is prone to errors; Eigen::Affine3fit is close to human operation steps, and the translation and rotation operations can be made into functions and called.
Example: First translate the point cloud data by 2.5 along the x-axis, and then rotate counterclockwise around the z-axis by π 4 \frac{\pi}{4}4pAngle.
The implementation mainly refers to the code of Shuangyu ; the rabbit pcd data is obtained from the blog "PCL Reading Data of PCD Files"
Link: https://pan.baidu.com/s/1v6mjPjwd7fIqUSjlIGTIGQ
extraction code: 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; // 旋转角度
//方法一: Matrix4f
Eigen::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;
//方法二: Affine3f
Eigen::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})
- run
./matrix_transform rabbit.pcd
