ROS之四元数、欧拉角以及旋转矩阵之间的相互转换（tf库）

在平时使用ROS时，经常需要用到各种位姿。例如机器人的pose、里程计的数据、机械臂的末端位姿等。大部分时候主要以使用四元数为主，但是四元数本身不是很直观，比如在需要对角度进行加减运算的时候就不太方便直接操作四元数而是会转换成欧拉角的形式，而当我们进行位姿间转换的时候，一般需要进行旋转矩阵进行乘法运算，这时候需要将四元数转换成旋转矩阵。在ROS中，可以使用tf库对这三者进行简单的转换。这里简单记录一下这三者之间的相互转换方式：

1、四元数转欧拉角

C++：

C++下可以直接使用TF库进行两者间的转换：

tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //进行转换
cout<<"current rpy:"<<roll<<", "<<pitch<<", "<<yaw<<endl;

python：

python中可以使用下述方式进行转换：

import tf
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])

2、欧拉角转四元数

C++：

geometry_msgs::Quaternion q;
q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
turngoal.pose.pose.orientation.x = q.x;
turngoal.pose.pose.orientation.y = q.y;
turngoal.pose.pose.orientation.z = q.z;
turngoal.pose.pose.orientation.w = q.w;

python：
python欧拉角转四元数没找到可以调用的包，但是可以根据公式自己换算：

q = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
print(q)

3、四元数转旋转矩阵

C++：

tf::Quaternion qw1={tool.pose.orientation.x,tool.pose.orientation.y,tool.pose.orientation.z,tool.pose.orientation.w};
tf::Matrix3x3 matrixw1;
matrixw1.setRotation(qw1);/*通过四元数计算得到旋转矩阵*/

python：

import tf
q=[x,y,z,w]
matrix = tf.transformations.quaternion_matrix(q)

4、旋转矩阵转四元数

C++：

tf::Matrix3x3 matrixw2;
tf::Quaternion qAC;
matrixw2.getRotation(qAC);
arm_pose.pose.orientation.x = qAC.getX();
arm_pose.pose.orientation.y = qAC.getY();
arm_pose.pose.orientation.z = qAC.getZ();
arm_pose.pose.orientation.w = qAC.getW();

python：

q2=tf.transformations.quaternion_from_matrix(matrix)

5、欧拉角转旋转矩阵

C++：

tf::Matrix3x3 matrix;
matrix.setRPY(roll,pitch,yaw);

python:

matrix2 = tf.transformations.euler_matrix(roll,pitch,yaw)

6、旋转矩阵转欧拉角

C++：

matrixw.getEulerYPR(yaw,pitch,roll);

python：

(roll2,pitch2,yaw2) = tf.transformations.euler_from_matrix(matrix2)

以下简单给出一版测试代码以及测试结果：
C++：

#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "geometry_msgs/PoseStamped.h"
using namespace std;
class tf_test
{
private:
    /* data */
public:
    tf_test(/* args */);
    void test();
    ~tf_test();
};

tf_test::tf_test(/* args */)
{
    ros::NodeHandle n;
}

void tf_test::test()
{
    tf::Quaternion quat;
    geometry_msgs::PoseStamped robot_pose;
    tf::Matrix3x3 matrixw1;
    robot_pose.pose.orientation.w = 0.852364;
    robot_pose.pose.orientation.x = -0.001;
    robot_pose.pose.orientation.y = -0.49621;
    robot_pose.pose.orientation.z = 0.16507;
    cout<<"初始值: q.x:"<<robot_pose.pose.orientation.x<<",q.y:"<<robot_pose.pose.orientation.y<<",q.z:"<<robot_pose.pose.orientation.z<<",q.w"<<robot_pose.pose.orientation.w<<endl;
    cout<<"四元数转欧拉角："<<endl;
    tf::quaternionMsgToTF(robot_pose.pose.orientation, quat);
    double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //进行转换
    cout<<"结果:"<<roll<<", "<<pitch<<", "<<yaw<<endl;
    geometry_msgs::Quaternion q;
    q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
    cout<<"欧拉角转四元数："<<endl;
    cout<<"结果:"<<q.x<<", "<<q.y<<", "<<q.z<<", "<<q.w<<endl;
    cout<<"四元数转旋转矩阵："<<endl;
    tf::Quaternion qw1={robot_pose.pose.orientation.x,robot_pose.pose.orientation.y,robot_pose.pose.orientation.z,robot_pose.pose.orientation.w};    
    matrixw1.setRotation(qw1);/*通过四元数计算得到旋转矩阵*/
    cout<<"结果:"<<matrixw1[0][0]<<" "<<matrixw1[0][1]<<" "<<matrixw1[0][2]<<" "<<endl;
    cout<<"     "<<matrixw1[1][0]<<" "<<matrixw1[1][1]<<" "<<matrixw1[1][2]<<" "<<endl;
    cout<<"     "<<matrixw1[2][0]<<" "<<matrixw1[2][1]<<" "<<matrixw1[2][2]<<" "<<endl;
    ros::Duration(0.2).sleep();
    cout<<"旋转矩阵转四元数："<<endl;
    tf::Matrix3x3 matrixw2;
    tf::Quaternion qAC;
    matrixw2[0][0] = 0.453054;
    matrixw2[0][1] = -0.280408;
    matrixw2[0][2] = -0.846235;
    matrixw2[1][0] = 0.282392;
    matrixw2[1][1] = 0.945502;
    matrixw2[1][2] = -0.162114;
    matrixw2[2][0] = 0.845575;
    matrixw2[2][1] = -0.165524;
    matrixw2[2][2] = 0.507548;
    matrixw2.getRotation(qAC);
    cout<<"结果:"<<qAC.getX()<<", "<<qAC.getY()<<", "<<qAC.getZ()<<", "<<qAC.getW()<<endl;
    ros::Duration(0.2).sleep();
    cout<<"欧拉角转旋转矩阵："<<endl;
    
    double roll1,pitch1,yaw1;
    roll1 = -0.315249;
    pitch1 = -1.00764;
    yaw1 = 0.557382;
    
    tf::Matrix3x3 matrix;
    
    matrix.setRPY(roll1,pitch1,yaw1);
    
    cout<<"结果:"<<matrix[0][0]<<" "<<matrix[0][1]<<" "<<matrix[0][2]<<" "<<endl;
    cout<<"     "<<matrix[1][0]<<" "<<matrix[1][1]<<" "<<matrix[1][2]<<" "<<endl;
    cout<<"     "<<matrix[2][0]<<" "<<matrix[2][1]<<" "<<matrix[2][2]<<" "<<endl;
    
    cout<<"旋转矩阵转欧拉角："<<endl;
    double roll2,pitch2,yaw2;
    matrix.getEulerYPR(yaw2,pitch2,roll2);
    cout<<"结果:"<<roll2<<", "<<pitch2<<", "<<yaw2<<endl;
    
}
tf_test::~tf_test()
{
}


int main(int argc, char **argv)
{
    ros::init(argc,argv,"tf_test");
    tf_test tf_test;
    tf_test.test();
    ros::spin();
    return 0;
}

python：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import tf


if __name__ == '__main__':
    rospy.init_node('fake_test')
    while not rospy.is_shutdown():
        rospy.sleep(0.1)
        print("四元数转欧拉角：")
        q=[-0.001,-0.496,0.165,0.852]
        print(q)
        (r, p, y) = tf.transformations.euler_from_quaternion(q)
        print(r,p,y)
        rospy.sleep(0.5)
        print("欧拉角转四元数：")
        roll = -0.31525282759303797
        pitch = -1.0076434199908184
        yaw = 0.5573862967289317
        q2 = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
        print(q2)
        rospy.sleep(0.5)
        print("四元数转旋转矩阵：")
        matrix = tf.transformations.quaternion_matrix(q)
        print(matrix)
        rospy.sleep(0.5)
        print("旋转矩阵转四元数：")
        q3 = tf.transformations.quaternion_from_matrix(matrix)
        print(q3)
        rospy.sleep(0.5)
        print("欧拉角转旋转矩阵：")
        matrix2 = tf.transformations.euler_matrix(roll,pitch,yaw)
        print(matrix2)
        rospy.sleep(0.5)
        print("旋转矩阵到欧拉角：")
        (roll2,pitch2,yaw2) = tf.transformations.euler_from_matrix(matrix2)
        print(roll2,pitch2,yaw2)
        rospy.sleep(0.5)