pretanslate 相当于左乘,translate 相当于右乘。
在 PCL 中我建立了一个原始坐标系 original 和一个转换之后的坐标系 transformed 来帮助理解。
代码如下
#include <iostream>
#include <Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
int main()
{
pcl::PointXYZ O(0, 0, 0); // 原始坐标系的 4 个点
pcl::PointXYZ X(1, 0, 0);
pcl::PointXYZ Y(0, 1, 0);
pcl::PointXYZ Z(0, 0, 1);
// 把这 4 个点看成一个点云,装在 cloud_original 中
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original (new pcl::PointCloud<pcl::PointXYZ>);
cloud_original->points.push_back(O);
cloud_original->points.push_back(X);
cloud_original->points.push_back(Y);
cloud_original->points.push_back(Z);
pcl::PointXYZ new_point(1, 2, 3); // 新坐标系的原点
// 对原始点云的旋转操作
Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Identity();
Eigen::AngleAxisf rotation_vector(M_PI_4, Eigen::Vector3f::UnitZ()); // 绕 Z 轴旋转 45°
rotation_matrix = rotation_vector.toRotationMatrix();
// 利用 new_point 做平移分量,利用 rotation_matrix 做旋转分量,构造变换矩阵
Eigen::Affine3f T1 = Eigen::Affine3f::Identity();
T1 = Eigen::Translation3f (new_point.getVector3fMap()) * Eigen::AngleAxisf (rotation_matrix);
std::cout << T1.matrix() << "\n" << std::endl;
// 利用 new_point 做平移分量,利用 rotation_matrix 做旋转分量,构造变换矩阵
Eigen::Affine3f T2 = Eigen::Affine3f::Identity();
T2.rotate(rotation_matrix);
T2.pretranslate(new_point.getVector3fMap()); // Applies on the left the translation matrix represented by the vector
std::cout << T2.matrix() << "\n" << std::endl;
std::cout << "T1, T2 是一样的,T1 是先平移再旋转,T2 是先旋转再平移(但平移是按照旋转之前的坐标轴平移的)" << "\n" << std::endl;
// 利用 new_point 做平移分量,利用 rotation_matrix 做旋转分量,构造变换矩阵
Eigen::Affine3f T3 = Eigen::Affine3f::Identity();
T3 = Eigen::AngleAxisf (rotation_matrix) * Eigen::Translation3f (new_point.getVector3fMap());
std::cout << T3.matrix() << "\n" << std::endl;
// 利用 new_point 做平移分量,利用 rotation_matrix 做旋转分量,构造变换矩阵
Eigen::Affine3f T4 = Eigen::Affine3f::Identity();
T4.rotate(rotation_matrix);
T4.translate(new_point.getVector3fMap()); // Applies on the right the translation matrix represented by the vector
std::cout << T4.matrix() << "\n" << std::endl;
std::cout << "T3, T4 是一样的,T3 是先旋转再平移,T4 是先旋转再平移(但平移是按照旋转之后的坐标轴平移的)" << std::endl;
// 利用构造的变换矩阵对原始点云进行变换
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_original, *cloud_transformed, T1);
pcl::PointXYZ OO(cloud_transformed->points[0]);
pcl::PointXYZ XX(cloud_transformed->points[1]);
pcl::PointXYZ YY(cloud_transformed->points[2]);
pcl::PointXYZ ZZ(cloud_transformed->points[3]);
// 可视化部分
pcl::visualization::PCLVisualizer viewer;
viewer.addPointCloud<pcl::PointXYZ> (cloud_original, "cloud_original");
viewer.addPointCloud<pcl::PointXYZ> (cloud_transformed, "cloud_transformed");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 20, "cloud_original");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 20, "cloud_transformed");
viewer.addArrow(X, O, 1, 0, 0, false, "x");
viewer.addArrow(Y, O, 0, 1, 0, false, "y");
viewer.addArrow(Z, O, 0, 0, 1, false, "z");
viewer.addArrow(XX, OO, 1, 0, 0, false, "xx");
viewer.addArrow(YY, OO, 0, 1, 0, false, "yy");
viewer.addArrow(ZZ, OO, 0, 0, 1, false, "zz");
pcl::PointXYZ position(O.x + 0.1f, O.y + 0.1f, O.z + 0.1f);
viewer.addText3D("original", position, 0.05, 1, 1, 1, "original");
pcl::PointXYZ position2(OO.x + 0.1f, OO.y + 0.1f, OO.z + 0.1f);
viewer.addText3D("transformed", position2, 0.05, 1, 1, 1, "transformed");
while (!viewer.wasStopped())
{
viewer.spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return 0;
}
代码中,T2 的构造是先调用了 rotate 函数,再调用了 pretranslate 函数,即先旋转再平移;而 T4 的构造是先调用了 rotate 函数,但接下来调用的是 translate 函数,也是先旋转再平移。那这两个平移有什么不一样吗?
对于 T2 而言,它先旋转再平移,但平移是在旋转发生之前的坐标轴下进行的,这也就是为什么 pretranslate 函数名称带 pre 前缀的原因。
对于 T4 而言,它也是先旋转再平移,但平移是在旋转发生之后的坐标轴下进行的,也就是针对当前状态下的坐标轴进行的平移。
开头说道,pretanslate 相当于左乘,translate 相当于右乘。那这怎么理解呢?
看看 T1 和 T3 的构造你就明白了,因为 T1 和 T2 是一样的,但 T1 我是用矩阵左乘的方式构造的;同理,T3 和 T4 是一样的,但 T3 我是用矩阵右乘的方式实现的。
程序将输出:
由此可以看出,pretranslate 是在旋转之前的坐标轴上进行的平移操作,而 translate 是在旋转之后
代码中的可视化部分见下图
Eigen 中的 rotate 和 prerotate 也是同样的道理,我就不过多赘述了,聪明的你一定能明白。
评论(0)
您还未登录,请登录后发表或查看评论