一个点在不同坐标系下的表示练习

引用地址：https://github.com/cckaixin/Practical_Homework_for_slambook14

推荐自己下载写一下。

问题描述：

有两个右手系1和2,其中2系的x轴与1系的y轴方向相同，2系的y轴与1系z轴方向相反，2系的z轴与1系的x轴相反,两个坐标系原点重合，求R12，求1系中(1,1,1)在2系中的坐标。请自己编写一个c++程序实现它，并用Cmake编译，得到能输出答案的可执行文件。

我的代码：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#define PI 3.14159265358979323846

int main(int argc, char const *argv[])
{
    //hint 1: Find the Euler Angle from Frame1 to Frame2
    double roll = -PI / 2;
    double pitch = -PI / 2;
    double yaw = 0;
    //hint 2: Convert the Euler Angle into the rotation matrix
    Eigen::AngleAxisd rollRotation(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchRotation(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawRotation(yaw, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d rotation_matrix = rollRotation.toRotationMatrix()*pitchRotation.toRotationMatrix()*yawRotation.toRotationMatrix();
    // hint 3: Try to convert a known point under the Frame1, like (1,1,1), to Frame2 using the rotation matrix,
    //         and then you can check if your answer is correct intuitively. (The answer is (1, -1, -1))
    Eigen::Vector3d point1(1, 1, 1);
    Eigen::Vector3d point2;
    point2 = rotation_matrix.inverse() * point1;
    std::cout << point2 << std::endl;
    return 0;
}

作者代码请自己在下载的文件中浏览。