SLAM整理-1-坐标变换

• 假设两个物体位于世界坐标系中，
• 物体1号位姿为$q_1$(四元数表示旋转)，t1(表示平移)；
• 物体2号位姿为$q_2$(四元数表示旋转)，t2(表示平移)；
• 现已知物体1号看到某个点在自身坐标系下的坐标为$p_1$，求该点在物体2号坐标系下的坐标$p_2$

---

• 记世界坐标系为$W$，物体坐标系分别为$R_1$，$R_2$。
• $T$表示坐标系之间的变换关系
• 由于
  $T_{W2}\ast p_2=T_{W1}\ast p_1$
• 得到
  $p_2=T_{W2}^{-1}\ast T_{W1}\ast p_1$
• 注意：四元数使用之前需要归一化。

#include <eigen3/Eigen/Dense>
#include <iostream>

int main()
{
    //
    Eigen::Vector3d p1(1, 1, 1);

    Eigen::Quaterniond q1(1, 0, 0, 0);
    Eigen::Quaterniond q2(0.5, 0.4, 0.3, 0.3);

    //normalize and normalized different
    q1.normalize();
    q2.normalize();

    //translation
    Eigen::Vector3d t1(1, 2, 3);
    Eigen::Vector3d t2(4, 5, 6);

    //T_{1W} T_{2W}
    Eigen::Isometry3d T1w(q1);
    Eigen::Isometry3d T2w(q2);

    T1w.pretranslate(t1);
    T2w.pretranslate(t2);

    std::cout << "T1W t :" << T1w.translation().transpose() << std::endl;
    std::cout << "T2W t :" << T2w.translation().transpose() << std::endl;

    std::cout << "T1W r :" << std::endl
              << T1w.rotation() << std::endl;
    std::cout << "T2W r :" << std::endl
              << T2w.rotation() << std::endl;

    // p_2 = T_{2W}*T_{W1}*p1
    Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;
    std::cout << std::endl
              << p2.transpose() << std::endl;
    return 0;
}

• 参考链接 https://blog.csdn.net/weixin_40224537/article/details/104950137