本节是上一篇《动手学ROS（8）：坐标系变换基础》的代码示例。

两个坐标系间的变换

[公式]

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>
int main(int argc, char const *argv[])
{
    Eigen::Quaterniond q(0.35, 0.2, 0.3, 0.1);
    q.normalize();
    Eigen::Vector3d T(0.3, 0.1, 0.1);

    Eigen::Vector3d p_c(0.5, 0, 0.2);
    // 也可 auto p_c = q * p_c + T;
    auto p_o = q.matrix() * p_c + T;
    std::cout << p_o << std::endl;

    return 0;
}

[公式]

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main(int argc, char** argv) {
  Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();
  q2.normalize();
  Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Eigen::Vector3d p1(0.5, 0, 0.2);

  Eigen::Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  // 原书：Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;
  Eigen::Vector3d p2=T2w.inverse() * T1w * p1;
  std::cout << p2.transpose() << std::endl;
  return 0;
}

这里需要指出，原书作者对于坐标系位姿的相对关系定义可能与我是相反的，故他使用 [公式] ,我们理解即可。

小结

本节仅仅是对上节的辅助示例，主要内容详见《动手学ROS（8）：坐标系变换基础》。