SLAM整理-2-Eigen 中 pretranslate 和 translate 的区别

57 阅读 0 评论 38 点赞

我是靠谱客的博主娇气彩虹，最近开发中收集的这篇文章主要介绍SLAM整理-2-Eigen 中 pretranslate 和 translate 的区别，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

相对于动坐标系（新坐标系，或者以叫以自身为参考系）右乘。
相对于静坐标系（变化过程中参考的坐标系始终不变）左乘

pretranslate之前的pre表示的是平移在旋转之前的坐标原点的基础上再平移，而不是在新的旋转坐标系下再进行平移
代码中，T2的构造是先调用了 rotate 函数，再调用了 pretranslate 函数，即先旋转再平移；而 T4 的构造是先调用了 rotate 函数，但接下来调用的是 translate 函数，也是先旋转再平移。那这两个平移有什么不一样吗？
- 对于 T2 而言，它先旋转再平移，但平移是在旋转发生之前的坐标轴下进行的，这也就是为什么 pretranslate 函数名称带 pre 前缀的原因。
- 对于 T4 而言，它也是先旋转再平移，但平移是在旋转发生之后的坐标轴下进行的，也就是针对当前状态下的坐标轴进行的平移。
pretanslate 相当于左乘，translate 相当于右乘。
T1 和 T2 是一样的，但 T1 我是用矩阵左乘的方式构造的；
同理，T3 和 T4 是一样的，但 T3 我是用矩阵右乘的方式实现的。

#include <iostream>
#include <chrono>
#include <thread>
#include <Eigen/Dense>

int main()

{
    // 原始坐标系中的一个点p1
    Eigen::Vector3d p1(0, 0, 0);

    //新坐标系的原点
    Eigen::Vector3d new_point(1, 2, 3);

    //构建R 绕 Z 轴旋转 45°
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI_4, Eigen::Vector3d::UnitZ()); // 绕 Z 轴旋转 45°
    rotation_matrix = rotation_vector.toRotationMatrix();

    // 利用 new_point 做平移分量，利用 rotation_matrix 做旋转分量，构造变换矩阵
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    //先平移，后旋转
    T1 = Eigen::Translation3d(new_point) * Eigen::AngleAxisd(rotation_matrix);
    std::cout << "T1: " << std::endl
              << T1.matrix() << std::endl;

    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    //先旋转，后平移
    T2.rotate(rotation_matrix);
    T2.pretranslate(new_point); // Applies on the left the translation matrix represented by the vector
    std::cout << "T2:" << std::endl
              << T2.matrix() << std::endl;

    std::cout << "T1, T2 是一样的，T1 是先平移再旋转，T2 是先旋转再平移（但平移是按照旋转之前的坐标轴平移的）"
              << std::endl;

    //**************************
    //**************************
    //利用 new_point 做平移分量，利用 rotation_matrix 做旋转分量，构造变换矩阵
    Eigen::Isometry3d T3 = Eigen::Isometry3d::Identity();
    // 先旋转，后平移
    T3 = Eigen::AngleAxisd(rotation_matrix) * Eigen::Translation3d(new_point);
    std::cout << "T3: " << std::endl
              << T3.matrix() << std::endl;

    Eigen::Isometry3d T4 = Eigen::Isometry3d::Identity();
    T4.rotate(rotation_matrix);
    T4.translate(new_point); // Applies on the right the translation matrix represented by the vector
    std::cout << "T4: " << std::endl
              << T4.matrix() << std::endl;

    std::cout << "T3, T4 是一样的，T3 是先旋转再平移，T4 是先旋转再平移（但平移是按照旋转之后的坐标轴平移的）" << std::endl;

    //最终看一下这个p1点怎么变化
    Eigen::Vector3d p2(0, 0, 0);
    p2 = T1 * p1;
    std::cout << "p2: 1-> " << p2.transpose() << std::endl;
    p2 = T2 * p1;
    std::cout << "p2: 2-> " << p2.transpose() << std::endl;
    p2 = T3 * p1;
    std::cout << "p2: 3-> " << p2.transpose() << std::endl;
    p2 = T4 * p1;
    std::cout << "p2: 4-> " << p2.transpose() << std::endl;

    std::cout << "test pretranslate" << std::endl;
    return 0;
}

Output

T1: 
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         2
        0         0         1         3
        0         0         0         1
T2:
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         2
        0         0         1         3
        0         0         0         1
T1, T2 是一样的，T1 是先平移再旋转，T2 是先旋转再平移（但平移是按照旋转之前的坐标轴平移的）
T3: 
 0.707107 -0.707107         0 -0.707107
 0.707107  0.707107         0   2.12132
        0         0         1         3
        0         0         0         1
T4: 
 0.707107 -0.707107         0 -0.707107
 0.707107  0.707107         0   2.12132
        0         0         1         3
        0         0         0         1
T3, T4 是一样的，T3 是先旋转再平移，T4 是先旋转再平移（但平移是按照旋转之后的坐标轴平移的）
p2: 1-> 1 2 3
p2: 2-> 1 2 3
p2: 3-> -0.707107   2.12132         3
p2: 4-> -0.707107   2.12132         3
test pretranslate