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

11 篇文章 0 订阅
7 篇文章 0 订阅

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

  • 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
  • 5
    点赞
  • 35
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值