两点云坐标点的转化
两点云坐标点的转化,就是把一个点云从自己的坐标系变换到另一个坐标系,配准,拼接都用得到。有点类似相机和激光外参的标定(将激光坐标系转换到相机的坐标系。都是Rt变换)。
(1)基本知识
R为旋转矩阵,X为原始点,t为平移量,X'为变换后的点(目标点)
R*X+t=X' (Rt*X=X')
但是所求Rt矩阵用一个矩阵表示如下:
/* Reminder: how transformation matrices work :
|-------> This column is the translation
| 1 0 0 x | \
| 0 1 0 y | }-> The identity 3x3 matrix (no rotation) on the left
| 0 0 1 z | /
| 0 0 0 1 | -> We do not use this line (and it has to stay 0,0,0,1)
(2)举例说明
将一个点(坐标系)绕自身z轴旋转(正)M_PI/4,然后再沿着旋转后得到的x轴方向(正)平移2.5。求Rt矩阵?
方法1:
直接数学方法得到并赋值:
METHOD #1: Using a Matrix4f
This is the "manual" method, perfect to understand but error prone !
*/
Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI/4; // The angle of rotation in radians
transform_1 (0,0) = cos (theta); //第1行第1列个元素
transform_1 (0,1) = -sin(theta); //第1行第 2 列个元素
transform_1 (1,0) = sin (theta);
transform_1 (1,1) = cos (theta);
// (row, column)
// Define a translation of 2.5 meters on the x axis.
// transform_1 (0,3) = 2.5;
transform_1 (0,3) = 0;
// Print the transformation
printf ("Method #1: using a Matrix4f\n");
std::cout << transform_1 << std::endl;
方法2:
通过程序计算矩阵:
/* METHOD #2: Using a Affine3f
This method is easier and less error prone
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
// Define a translation of 2.5 meters on the x axis.
transform_2.translation() << 2.5, 0.0, 0.0;
// The same rotation matrix as before; theta radians arround Z axis
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
// Print the transformation
printf ("\nMethod #2: using an Affine3f\n");
std::cout << transform_2.matrix() << std::endl;
最后转化:
pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);