欧拉角位姿变换

欧拉角姿态变换

姿态B相对于姿态A的变换：

欧拉角为 rx, ry, rz, 绕 Z-Y-X 轴进行旋转。

那么姿态A相对于姿态B的变换：

欧拉角为 -rx, -ry, -rz, 绕 X-Y-Z 轴进行旋转。

    double rx, ry, rz, px, py, pz;
    rx = 10;
    ry = 20;
    rz = 30;
    px = 1;
    py = 2;
    pz = 3;

    std::cout << "rx: " << rx << " ry: " << ry << " rz: " << rz << "\n";
    std::cout << "px: " << px << " py: " << py << " pz: " << pz << std::endl;

    Eigen::AngleAxis roll1(rx / 180 * M_PI, Eigen::Vector3d::UnitX());
    Eigen::AngleAxis pitch1(ry / 180 * M_PI, Eigen::Vector3d::UnitY());
    Eigen::AngleAxis yaw1(rz / 180 * M_PI, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d matrix1 = (yaw1 * pitch1 * roll1).toRotationMatrix();
    std::cout << "matrix1: Z-Y-X" << "\n";
    std::cout << matrix1 << "\n" << std::endl;
    std::cout << "matrix1.inv: " << "\n";
    std::cout << matrix1.inverse() << "\n" << std::endl;

    rx *= -1;
    ry *= -1;
    rz *= -1;

    std::cout << "rx: " << rx << " ry: " << ry << " rz: " << rz << "\n";

    Eigen::AngleAxis roll2(rx / 180 * M_PI, Eigen::Vector3d::UnitX());
    Eigen::AngleAxis pitch2(ry / 180 * M_PI, Eigen::Vector3d::UnitY());
    Eigen::AngleAxis yaw2(rz / 180 * M_PI, Eigen::Vector3d::UnitZ());
    Eigen::Matrix3d matrix2 = (roll2 * pitch2 * yaw2).toRotationMatrix();
    std::cout << "matrix2: X-Y-Z" << "\n";
    std::cout << matrix2 << "\n" << std::endl;
    std::cout << "matrix2.inv: " << "\n";
    std::cout << matrix2.inverse() << "\n" << std::endl;

输出：

    rx: 10 ry: 20 rz: 30
    px: 1 py: 2 pz: 3
    matrix1: Z-Y-X
    0.813798  -0.44097  0.378522
    0.469846  0.882564 0.0180283
    -0.34202  0.163176  0.925417

    matrix1.inv: 
    0.813798  0.469846  -0.34202
    -0.44097  0.882564  0.163176
    0.378522 0.0180283  0.925417

    rx: -10 ry: -20 rz: -30
    matrix2: X-Y-Z
    0.813798  0.469846  -0.34202
    -0.44097  0.882564  0.163176
    0.378522 0.0180283  0.925417

    matrix2.inv: 
    0.813798  -0.44097  0.378522
    0.469846  0.882564 0.0180283
    -0.34202  0.163176  0.925417