四元数、变换矩阵、欧拉角转换关系

四元数to变换矩阵

   	Eigen::Quaterniond quaternion(w, x, y, z);
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix = quaternion.matrix();
    transformation.rotate(rotation_matrix);
    transformation.pretranslate(Eigen::Vector3d(tx, ty, tz));
    cout << transformation.matrix() << endl;

变换矩阵to四元数

transfor为已知的变换矩阵，从icp配准得到的就是

//获取变换矩阵，求四元数q和位移t_vector
    Eigen::Matrix4f transfor;
    transfor = icp.getFinalTransformation();
    Eigen::Matrix3f r_matrix = Eigen::Matrix3f::Identity();
    for(int i=0;i<3;i++)
    {
	for(int j=0;j<3;j++)
	{
	    r_matrix(i,j) = transfor(i,j);
	}
    }
    Eigen::Vector3f t_vector(transfor(0,3),transfor(1,3),transfor(2,3));
    Eigen::Quaternionf q;
    q = Eigen::Quaternionf ( r_matrix );

欧拉角转换to旋转矩阵

参考：https://zhuanlan.zhihu.com/p/144032401
https://blog.csdn.net/shyjhyp11/article/details/111701127
Eigen库:

    Eigen::Vector3d eulerAngle(1.0, 2.0, 3.0);//欧拉角
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitZ()));
    Eigen::Matrix3d rotation_matrix1; //旋转矩阵
    rotation_matrix1 = yawAngle*pitchAngle*rollAngle;
    //旋转矩阵to变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_matrix1);
    T.pretranslate(Eigen::Vector3d(11, 3, 4));
    cout << T.matrix() <<endl;

自己写：

Eigen::Matrix3d eulerAnglesToRotationMatrix(Eigen::Vector3d &theta)
{
    Eigen::Matrix3d R_x;    // 计算旋转矩阵的X分量
    R_x <<
            1,              0,               0,
            0,  cos(theta[0]),  -sin(theta[0]),
            0,  sin(theta[0]),   cos(theta[0]);

    Eigen::Matrix3d R_y;    // 计算旋转矩阵的Y分量
    R_y <<
            cos(theta[1]),   0, sin(theta[1]),
            0,   1,             0,
            -sin(theta[1]),  0, cos(theta[1]);

    Eigen::Matrix3d R_z;    // 计算旋转矩阵的Z分量
    R_z <<
            cos(theta[2]), -sin(theta[2]), 0,
            sin(theta[2]),  cos(theta[2]), 0,
            0,              0,             1;
    Eigen::Matrix3d R = R_z * R_y * R_x;
    return R;
}

旋转矩阵to欧拉角

eigen：

    Eigen::Vector3d eulerAngle1 = rotation_matrix1.eulerAngles(2,1,0); // ZYX顺序，yaw,pitch,roll
    cout << "roll_1 pitch_1 yaw_1 = " << eulerAngle1[2] << " " << eulerAngle1[1] 
         << " " << eulerAngle1[0] << endl << endl;

自己写：

Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(R));
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}

python版

https://blog.csdn.net/shyjhyp11/article/details/111701127

import numpy as np
import math
from scipy.spatial.transform import Rotation as R
 
 
Rq=[-0.71934025092983234, 1.876085535681999e-06, 3.274841213980097e-08, 0.69465790385533299]
 
# 四元数到旋转矩阵
r = R.from_quat(Rq)
Rm = r.as_matrix()
# 0:array([ 1.00000000e+00, -2.74458557e-06,  2.55936079e-06])
# 1:array([-2.65358979e-06, -3.49007932e-02,  9.99390782e-01])
# 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
 
# 符号相反的四元数, 仍表示同一个旋转
Rq1= [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299]
# 四元数到旋转矩阵
r1 = R.from_quat(Rq1)
Rm1 = r1.as_matrix()
# 0:array([ 1.00000000e+00, -2.74458557e-06,  2.55936079e-06])
# 1:array([-2.65358979e-06, -3.49007932e-02,  9.99390782e-01])
# 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
 
# 四元数到欧拉角
euler0 = r.as_euler('xyz', degrees=True)
# ([-9.20000743e+01,  1.52039496e-04, -1.52039496e-04])
euler3 = r.as_euler('xzy', degrees=True)
#([-9.20000743e+01, -1.52039496e-04,  1.52039496e-04])
euler1 = r.as_euler('zxy', degrees=True)
#([-179.99564367,  -87.99992566,  179.99579836])
euler2 = r.as_euler('zyx', degrees=True)
#([ 1.57253169e-04,  1.46640571e-04, -9.20000743e+01])
 
euler4 = r.as_euler('yxz', degrees=True)
#([179.99564367, -87.99992566, 179.99549428])
 
euler5 = r.as_euler('yzx', degrees=True)
#([ 1.46640571e-04,  1.57253169e-04, -9.20000743e+01])
 
 
# 旋转矩阵到四元数
r3 = R.from_matrix(Rm)
qua = r3.as_quat()
#[0.7193402509298323, -1.8760855356819988e-06, -3.2748412139801076e-08, -0.694657903855333] #与原始相反,但等价
 
# 旋转矩阵到欧拉角
euler_1 = r3.as_euler('zxy', degrees=True)
#([-179.99564367,  -87.99992566,  179.99579836])
 
# 欧拉角到旋转矩阵
r4 = R.from_euler('zxy', [-179.99564367,  -87.99992566,  179.99579836], degrees=True)
rm = r4.as_matrix()
# 0:array([ 1.00000000e+00, -2.74452529e-06,  2.55936075e-06])
# 1:array([-2.65358765e-06, -3.49007933e-02,  9.99390782e-01])
# 2:array([-2.65352955e-06, -9.99390782e-01, -3.49007933e-02])
 
# 欧拉角到四元数
qua1 = r4.as_quat()
#([-7.19340251e-01,  1.87606384e-06,  3.27274889e-08,  6.94657904e-01])
 
 
#----测试--------------------------------------------------------------------
theta=[-116,    0. , -105]
r6 = R.from_euler('xyz', theta, degrees=True)
rm = r6.as_matrix()
# 0:array([-0.25881905, -0.42343401,  0.86816838])
# 1:array([-0.96592583,  0.1134588 , -0.23262502])
# 2:array([ 0.        , -0.89879405, -0.43837115])
 
qua3 = r6.as_quat()
#array([-0.52720286,  0.68706415, -0.39667667,  0.30438071])
 
print(qua3)