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 也是同样的道理,我就不过多赘述了,聪明的你一定能明白。