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

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

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

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

#include
#include

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

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

方法一：用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

#include
#include

   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;

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

def quaternion_to_rotation_matrix(q):
    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)

#include
#include

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());

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

#include
#include

Eigen::Matrix3d rotation_matrix3;

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

公式：r o t a t i o n m a t r i x = R z ∗ R y ∗ R x rotation_matrix = R_z * R_y * R_x ro t a t i o n m ​a t r i x =R z ​∗R y ​∗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

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;

方法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

#include
#include

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

Eigen::Vector3d euler_angles_3 = R3.eulerAngles(2, 1, 0);

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])

#include
#include

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

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)

Original: https://blog.csdn.net/Will_Ye/article/details/127498034
Author: Will_Ye
Title: 四元数，欧拉角，旋转矩阵相互转换以及矩阵求逆合集（C++和python）