1. O que é transformação de matriz em PCL?
Quando a nuvem de pontos 3D obtida por meio de sensores a laser e outros equipamentos é unida em uma cena maior, os dados da nuvem de pontos precisam ser girados e traduzidos. As operações de rotação e translação podem ser alcançadas através da transformação de matrizes.
- Representação de pontos
Para um ponto pp na nuvem de pontosp , que é expresso como
p = [ xyz 1 ] p=\begin{bmatrix} x \\ y \\ z \\ 1 \\ \end{bmatrix}p=
xsimz1
这り, ( x , y , z ) (x, y, z)( x ,sim ,z ) é a coordenada espacial do ponto. O que é 1 na quarta dimensão? Na verdade, a quarta dimensão é definida artificialmente para facilitar o uso posterior da multiplicação de matrizes para expressar operações de tradução.
- tradução de ponto
Por exemplo, o pp acimaO ponto p é transladado 8 unidades de distância da origem ao longo do eixo X. Intuitivamente, o resultado após a translação pode ser obtidop ′ p^{\prime}p′
p ′ = [ x + 8 yz 1 ] p^{\prime}=\begin{bmatrix} x+8 \\ y \\ z \\ 1 \\ \end{bmatrix}p'=
x+8simz1
Na verdade, é equivalente à seguinte multiplicação de matrizes
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 {bmatriz}p'=A⋅p=
100001000010abc1
⋅
xsimz1
=
x+asim+bz+c1
Pode-se observar que ao adicionar uma dimensão às coordenadas tridimensionais do ponto, a operação de translação do ponto pode ser realizada de forma muito conveniente através da multiplicação de matrizes. Observe que a matriz AA aquiA é sempre4 × 4 4\vezes44×Matriz de 4 quadrados.
- rotação de ponto
Isso se refere ao ponto girando em torno da origem em uma determinada direção θ \thetaângulo θ . Por exemplo, em um sistema de coordenadas tridimensional, gire no sentido anti-horário em torno do eixo x, conforme mostrado abaixo (consulte o blog"PCL Point Cloud Library - Rotation and Translation Matrix")
para girar em qualquer direção e em qualquer ângulo. Você pode girar em torno do eixo x e em torno de y. É uma combinação de três operações básicas de rotação: rotação do eixo e rotação em torno do eixo z. A rotação em torno do eixo x, a rotação em torno do eixo y e a rotação em torno do eixo z podem ser representadas por matrizes, respectivamente.
Lembre-se de girar no sentido anti-horário em torno do eixo x θ \thetaA operação do ângulo θ éR x ( θ ) R_x(\theta)Rx( θ ) , determine a equação inversa
p ′ = R x ( θ ) ⋅ p = [ 1 0 0 0 0 cos θ − sin θ 0 0 sin θ cos θ 0 0 0 0 1 ] ⋅ [ xyz = [ xy ⋅ cos θ − z ⋅ sen θ y ⋅ sen θ + 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{bmatriz} x \\ y \cdot cos\theta-z\cdot sin\theta \\ y \cdot sin\theta+z\cdot cos\theta \\ 1 \\ \ end{bmatriz}p'=Rx( eu )⋅p=
10000cosθ _s em θ00−s em θ _cosθ _00001
⋅
xsimz1
=
xsim⋅cosθ _-z⋅s em θsim⋅s em θ+z⋅cosθ _1
Lembre-se de girar no sentido anti-horário em torno do eixo y θ \thetaA operação do ângulo θ éR y ( θ ) R_y(\theta)Rvocê( θ ) ,determine a equação inversa
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{bmatriz}\cdot \begin{bmatriz} x\\y\\z\ \ 1 \\ \ fim{bmatriz}= \begin{bmatriz} x \cdot cos\theta+z\cdot syn\theta \\ y \\ -x \cdot sin\theta+z\cdot cos\theta \\ \\\end{bmatriz }p'=Rvocê( eu )⋅p=
cosθ _0−s em θ _00100s em θ0cosθ _00001
⋅
xsimz1
=
x⋅cosθ _+z⋅s em θsim−x _⋅s em θ+z⋅cosθ _1
Lembre-se de girar no sentido anti-horário em torno do eixo z θ \thetaA operação do ângulo θ éR z ( θ ) R_z(\theta)Rz( θ ) , determine a equação inversa
p ′ = R z ( θ ) ⋅ p = [ cos θ − sin θ 0 0 sin θ cos θ 0 0 0 0 1 0 0 0 0 1 ] ⋅ [ xyz = [ x ⋅ cos θ − y ⋅ sen θ x ⋅ sen θ + 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{bmatriz} x \cdot cos\theta-y\cdot sin\theta \\ x \cdot sin\theta+y\cdot cos\theta \\ z \\ 1 \\ \ end{bmatriz}p'=Rz( eu )⋅p=
cosθ _s em θ00−s em θ _cosθ _0000100001
⋅
xsimz1
=
x⋅cosθ _-sim⋅s em θx⋅s em θ+sim⋅cosθ _z1
2. Exemplo: Como fazer transformação de matriz
Desde que a transformação da matriz seja implementada, a rotação e translação da nuvem de pontos podem ser feitas. Existem dois métodos para transformação de matrizes na biblioteca PCL, nomeadamente Eigen::Matrix4fe Eigen::Affine3f. Eigen::Matrix4fÉ necessário construir manualmente as matrizes de rotação e translação, o que está sujeito a erros; Eigen::Affine3festá próximo das etapas da operação humana, e as operações de translação e rotação podem ser transformadas em funções e chamadas.
Exemplo: Primeiro transfira os dados da nuvem de pontos em 2,5 ao longo do eixo x e depois gire no sentido anti-horário em torno do eixo z em π 4 \frac{\pi}{4}4pÂngulo.
A implementação refere-se principalmente ao código de Shuangyu ; os dados do coelho pcd são obtidos no blog "PCL Reading Data from PCD Files"
Link: https://pan.baidu.com/s/1v6mjPjwd7fIqUSjlIGTIGQ
código de extração: zspx
- matriz_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})
- correr
./matrix_transform rabbit.pcd
