四元数，欧拉角，旋转矩阵相互转换以及矩阵求逆合集（C++和python）

0. 唠叨几句

项目过程中经常要用到四元数，欧拉角还有旋转矩阵，所以它们之间的相互转化代码就经常会被调用，整理一下，以后就不用东找西找了。

在C++的版本中，我一般都是用Eigen库里面提供的API来完成转换的，所以需要先安装好Eigen库，如果没装的，可以按照我另一篇博客去安装，点击这里

在python版本中，可以自己写公式来算，基本用的都是numpy，这个就比较容易安装，不多说了。
也可以直接调用scipy库，用pip install scipy安装

---

PS：
1）四元素，欧拉角和旋转矩阵之间的转换其实都有公式，用公式自己写个函数也是其中一种方法，我放在python版本处，C++调用现成的库会更方便
2）注意：所有的欧拉角都是按Z, Y, X的顺序旋转的

1. 四元数 -> 欧拉角

1.1 四元数 -> 欧拉角（C++）

#include <tf/tf.h>
#include <nav_msgs/Odometry.h>

double base_roll, base_pitch, base_yaw;
nav_msgs::Odometry odom_pose;

tf::Quaternion RQ2;  //quaternion to RPY
tf::quaternionMsgToTF(odom_msg.pose.pose.orientation,RQ2);
tf::Matrix3x3(RQ2).getRPY(base_roll,base_pitch,base_yaw);

1.2 四元数 -> 欧拉角（Python）

方法一：用tf库

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

方法二：根据公式自己写函数

def quart_to_rpy(x, y, z, w):
    roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
    pitch = math.asin(2 * (w * y - x * z))
    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
    return roll, pitch, yaw

2. 四元数 -> 旋转矩阵

2.1 四元数 -> 旋转矩阵（C++）

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

   Eigen::Quaterniond t_Q;
   t_Q.x() = odom_pose.pose.pose.orientation.x;
   t_Q.y() = odom_pose.pose.pose.orientation.y;
   t_Q.z() = odom_pose.pose.pose.orientation.z;
   t_Q.w() = odom_pose.pose.pose.orientation.w;

   t_Q.normalize();
   Eigen::Matrix3d R3;
   R3 = t_Q.matrix();
   cout << "R3: " << endl << R3 << endl;

2.2 四元数 -> 旋转矩阵（Python）

注意一下四元数的顺序就行，按xyzw来写

def quaternion_to_rotation_matrix(q):  # x, y ,z ,w
    rot_matrix = np.array(
        [[1.0 - 2 * (q[1] * q[1] + q[2] * q[2]), 2 * (q[0] * q[1] - q[3] * q[2]), 2 * (q[3] * q[1] + q[0] * q[2])],
         [2 * (q[0] * q[1] + q[3] * q[2]), 1.0 - 2 * (q[0] * q[0] + q[2] * q[2]), 2 * (q[1] * q[2] - q[3] * q[0])],
         [2 * (q[0] * q[2] - q[3] * q[1]), 2 * (q[1] * q[2] + q[3] * q[0]), 1.0 - 2 * (q[0] * q[0] + q[1] * q[1])]]
         )
    return rot_matrix

 Rq = [odom_pose.pose.pose.orientation.x, odom_pose.pose.pose.orientation.y, odom_pose.pose.pose.orientation.z, odom_pose.pose.pose.orientation.w]
 R3 = quaternion_to_rotation_matrix(Rq)

3. 欧拉角 - > 四元数

3.1 欧拉角 -> 四元数（C++）

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

Eigen::Quaterniond quaternion3;
double roll, pitch, yaw;

quaternion3 = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 
              Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 
              Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());

3.2 欧拉角 -> 四元数（Python）

def rpy2quaternion(roll, pitch, yaw):
    x = sin(pitch/2)sin(yaw/2)cos(roll/2)+cos(pitch/2)cos(yaw/2)sin(roll/2)
    y = sin(pitch/2)cos(yaw/2)cos(roll/2)+cos(pitch/2)sin(yaw/2)sin(roll/2)
    z = cos(pitch/2)sin(yaw/2)cos(roll/2)-sin(pitch/2)cos(yaw/2)sin(roll/2)
    w = cos(pitch/2)cos(yaw/2)cos(roll/2)-sin(pitch/2)sin(yaw/2)sin(roll/2)
    return x, y, z, w

4. 欧拉角 - > 旋转矩阵

4.1 欧拉角 -> 旋转矩阵（C++）

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

Eigen::Matrix3d rotation_matrix3;

rotation_matrix3 = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 
                   Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 
                   Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());

4.2 欧拉角 -> 旋转矩阵（Python）

公式：$rotatio{n}_{m}atrix=R_z\ast R_y\ast R_x$

import numpy as np
import math

def eul2rot(roll, pitch，yaw):

    R_z = np.array([[math.cos(yaw),    -math.sin(yaw),    0],
                    [math.sin(yaw),    math.cos(yaw),     0],
                    [0,                     0,                      1]
                    ])
                    
    R_y = np.array([[math.cos(pitch),    0,      math.sin(pitch)  ],
                    [0,                     1,      0                   ],
                    [-math.sin(pitch),   0,      math.cos(pitch)  ]
                    ])
                    
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(roll), -math.sin(roll) ],
                    [0,         math.sin(roll), math.cos(roll)  ]
                    ])

    R = np.dot(R_z, np.dot( R_y, R_x ))

    return R

5. 旋转矩阵 -> 四元数

5.1 旋转矩阵 -> 四元数（C++）

Eigen::Matrix3d R;

Eigen::Quaterniond q = Eigen::Quaterniond(R);  
q.normalize();  
cout << “Quaternion:” <<endl;  
cout << ”x = ” << q.x() <<endl;  
cout << ”y = ” << q.y() <<endl;  
cout << ”z = ” << q.z() <<endl;  
cout << ”w = ” << q.w() <<endl<<endl;  

5.2 旋转矩阵 -> 四元数（Python）

方法1： 调用scipy库

import numpy as np
import math
from scipy.spatial.transform import Rotation as R

r3 = R.from_matrix(Rm)
qua = r3.as_quat()

方法2： 直接写公式
对于旋转矩阵到四元数的公式，可以看看引用里的第三篇blog，写得很详细

import numpy as np
import math

def rmat2Quat(R):
    helper = np.array(np.abs([R[0][0]+R[1][1]+R[2][2],R[0][0]-R[1][1]-R[2][2],-R[0][0]+R[1][1]-R[2][2],-R[0][0]-R[1][1]+R[2][2]]))
    pos = np.argmax(helper)
    if(pos == 0):
        w = np.sqrt(R[0][0]+R[1][1]+R[2][2]+1)/2
        x = (R[1][2]-R[2][1])/4/w
        y = (R[2][0]-R[0][2])/4/w
        z = (R[0][1]-R[1][0])/4/w
    elif(pos == 1):
        x = np.sqrt(R[0][0]-R[1][1]-R[2][2]+1)/2
        w = (R[1][2]-R[2][1])/4/x
        y = (R[0][1]+R[1][0])/4/x
        z = (R[2][0]+R[0][2])/4/x
    elif(pos == 2):
        y = np.sqrt(-R[0][0]+R[1][1]-R[2][2]+1)/2
        w = (R[2][0]-R[0][2])/4/y
        x = (R[0][1]+R[1][0])/4/y
        z = (R[1][2]+R[2][1])/4/y
    elif(pos == 3):
        z = np.sqrt(-R[0][0]-R[1][1]+R[2][2]+1)/2
        w = (R[0][1]-R[1][0])/4/z
        x = (R[2][0]+R[0][2])/4/z
        y = (R[1][2]+R[2][1])/4/z
    return w,x,y,z

6. 旋转矩阵 -> 欧拉角

6.1 旋转矩阵 -> 欧拉角（C++）

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

Eigen::Matrix3d R3 = Eigen::Matrix3d::Identity();;
double roll, pitch, yaw;

//ZYX顺序，即先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,0表示X轴,1表示Y轴,2表示Z轴
Eigen::Vector3d euler_angles_3 = R3.eulerAngles(2, 1, 0); 

6.2 旋转矩阵 -> 欧拉角（Python）

def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))
    
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    
    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

7. 矩阵求逆

7.1 矩阵求逆（C++）

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

Eigen::Matrix3d trans_map2odom = Eigen::Matrix3d::Identity();
Eigen::Matrix3d trans_map2odom_inv = trans_map2odom.inverse();

7.2 矩阵求逆（Python）

求逆之前，最好先确认一下矩阵是否满稚，如果不满秩就直接报错了，不满秩的矩阵可以求伪逆，这个可以参考我的另一篇博客，点击这里

import numpy as np

trans_map2odom = np.mat([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], dtype = np.float)
np.linalg.inv(trans_map2odom)

Reference

1. 欧拉角，四元数，旋转矩阵相互转化（c++, python)
2. 四元数与欧拉角（RPY角）的相互转换
3. 从旋转矩阵到四元数
4. 四元数与旋转矩阵的转换