RPY角和Euler角

• RPY角，fix轴(旋转增量左乘)，旋转顺序为 XYZ
• Euler角，active轴(旋转增量右乘)，旋转顺序为ZYX

        RPY角和Euler角本质上是一样的，表达的是相同的旋转。

1、RPY角

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

int main(int argc, char const *argv[])
{
    // Frame 1 ---[roll=-pi/2, pitch=pi/3, yaw=pi/4]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 定轴，左乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // XYZ 左乘，即 Z*Y*X
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}
 

2、Euler

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#define PI 3.14159265358979323846
 
int main(int argc, char const *argv[])
{
    // Frame 1 ---[z=pi/4, y=pi/3, x=-pi/2]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 动轴，右乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // ZYX 右乘，即 Z*Y*X
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}
 

3、结果

        RPY：

***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************

        Euler：

***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************

4、获取RPY角/Euler角的方式 

        Eigen 库：eulerAngles()函数

        ROS tf 库 ：getRPY()函数

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <tf/transform_datatypes.h> 
#define PI 3.14159265358979323846

int main(int argc, char const *argv[])
{
    // Frame 1 ---[roll=-pi/2, pitch=pi/3, yaw=pi/4]---> Frame 2
    double roll,pitch,yaw;
    roll = -PI/2;
    pitch = PI/3;
    yaw = PI/4;
 
    // 将欧拉角转换为旋转矩阵
    Eigen::Matrix3d roation_matrix;
    Eigen::Matrix3d roation_matrix1;
    Eigen::Matrix3d roation_matrix2;
    Eigen::Matrix3d roation_matrix3;
    
    Eigen::AngleAxisd rv_roll(roll, Eigen::Vector3d::UnitX());                
    Eigen::AngleAxisd rv_pitch(pitch, Eigen::Vector3d::UnitY());     
    Eigen::AngleAxisd rv_yaw(yaw, Eigen::Vector3d::UnitZ());   
    
    // 定轴，左乘
    roation_matrix1 = (rv_roll).toRotationMatrix();    
    roation_matrix2 = (rv_pitch).toRotationMatrix();
    roation_matrix3 = (rv_yaw).toRotationMatrix();
    roation_matrix = roation_matrix3 * roation_matrix2 * roation_matrix1;  // XYZ 左乘，即 Z*Y*X

    // 方式①
    // 提取 RPY (Roll, Pitch, Yaw) 角
    // 参数 (2, 1, 0) 指定了提取欧拉角的顺序，即旋转轴的顺序为 Z Y X
    Eigen::Vector3d pyr = roation_matrix.eulerAngles(2, 1, 0);  // Yaw, Pitch, Roll 顺序
    std::cout << "方式①" << std::endl;
    std::cout << "R:" << pyr.z() << " P:" << pyr.y() << " Y:" << pyr.x() << std::endl;

    //方式②
    tf::Matrix3x3 tf_matrix;

    // 逐元素复制数据
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            tf_matrix[i][j] = roation_matrix(i, j);
        }
    }
    double my_roll, my_pitch, my_yaw;
    tf_matrix.getRPY(my_roll, my_pitch, my_yaw);
    std::cout << "方式②" << std::endl;
    std::cout << "R:" << my_roll << " P:" << my_pitch << " Y:" << my_yaw << std::endl;
 
    std::cout << "***************************************" << std::endl;
    std::cout << "旋转矩阵R12: " << std::endl;
    std::cout << roation_matrix<< std::endl;
    std::cout << "***************************************" << std::endl;

 
    return 0;
}
 

        结果：

方式①
R:-1.5708 P:1.0472 Y:0.785398
方式②
R:-1.5708 P:1.0472 Y:0.785398
***************************************
旋转矩阵R12: 
   0.353553   -0.612372   -0.707107
   0.353553   -0.612372    0.707107
  -0.866025        -0.5 3.06162e-17
***************************************