【SLAM】欧拉角，四元数，旋转矩阵相互转化（c++, python)

转载连接：知乎-欧拉角，四元数，旋转矩阵相互转化（c++, python)

---

四元数->欧拉角

C++

#include <tf/tf.h>
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);

double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换

PYTHON

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

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

欧拉角->四元数

C++

#include <tf/tf.h>
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
#create ros msg
tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)

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

欧拉角->旋转矩阵

C++

static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived> &ypr)
    {
        typedef typename Derived::Scalar Scalar_t;

        Scalar_t y = ypr(0) / 180.0 * M_PI;
        Scalar_t p = ypr(1) / 180.0 * M_PI;
        Scalar_t r = ypr(2) / 180.0 * M_PI;

        Eigen::Matrix<Scalar_t, 3, 3> Rz;
        Rz << cos(y), -sin(y), 0,
            sin(y), cos(y), 0,
            0, 0, 1;

        Eigen::Matrix<Scalar_t, 3, 3> Ry;
        Ry << cos(p), 0., sin(p),
            0., 1., 0.,
            -sin(p), 0., cos(p);

        Eigen::Matrix<Scalar_t, 3, 3> Rx;
        Rx << 1., 0., 0.,
            0., cos(r), -sin(r),
            0., sin(r), cos(r);

        return Rz * Ry * Rx;
    }

PYTHON

def eulerAnglesToRotationMatrix(theta) :    

    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
                                        
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ]
                    
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                                     
    R = np.dot(R_z, np.dot( R_y, R_x ))
    return R

旋转矩阵->欧拉角

旋转顺序：ZYX
C++

static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
    {
        Eigen::Vector3d n = R.col(0);
        Eigen::Vector3d o = R.col(1);
        Eigen::Vector3d a = R.col(2);

        Eigen::Vector3d ypr(3);
        double y = atan2(n(1), n(0));
        double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
        double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
        ypr(0) = y;
        ypr(1) = p;
        ypr(2) = r;

        return ypr / M_PI * 180.0;
    }

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

旋转矩阵->四元数

C++

Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
{  
    Eigen::Quaterniond q = Eigen::Quaterniond(R);  
    q.normalize();  
    cout << “RotationMatrix2Quaterniond result is:” <<endl;  
    cout << ”x = ” << q.x() <<endl;  
    cout << ”y = ” << q.y() <<endl;  
    cout << ”z = ” << q.z() <<endl;  
    cout << ”w = ” << q.w() <<endl<<endl;  
    return q;  
} 

PYTHON

function q = vgg_quat_from_rotation_matrix( R )
% vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix 
%            q = vgg_quat_from_rotation_matrix(R)

q = [   (1 + R(1,1) + R(2,2) + R(3,3))
    (1 + R(1,1) - R(2,2) - R(3,3))
    (1 - R(1,1) + R(2,2) - R(3,3))
    (1 - R(1,1) - R(2,2) + R(3,3)) ];

if ~issym(q)
  % Pivot to avoid division by small numbers
  [b I] = max(abs(q));
else
  % For symbolic quats, just make sure we're nonzero
  for k=1:4
    if q(k) ~= 0
      I = k;
      break
    end
  end
end

q(I) = sqrt(q(I)) / 2 ;

if I == 1 
    q(2) = (R(3,2) - R(2,3)) / (4*q(I));
    q(3) = (R(1,3) - R(3,1)) / (4*q(I));
    q(4) = (R(2,1) - R(1,2)) / (4*q(I));
elseif I==2
    q(1) = (R(3,2) - R(2,3)) / (4*q(I));
    q(3) = (R(2,1) + R(1,2)) / (4*q(I));
    q(4) = (R(1,3) + R(3,1)) / (4*q(I));
elseif I==3
    q(1) = (R(1,3) - R(3,1)) / (4*q(I));
    q(2) = (R(2,1) + R(1,2)) / (4*q(I));
    q(4) = (R(3,2) + R(2,3)) / (4*q(I));
elseif I==4
    q(1) = (R(2,1) - R(1,2)) / (4*q(I));
    q(2) = (R(1,3) + R(3,1)) / (4*q(I));
    q(3) = (R(3,2) + R(2,3)) / (4*q(I));
end

四元数->旋转矩阵

C++

Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;  
  
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();  
    cout << “Quaternion2RotationMatrix result is:” <<endl;  
    cout << ”R = ” << endl << R << endl<< endl;  
    return R;  
}  

PYTHON

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])]],
        dtype=q.dtype)
    return rot_matrix

也可以用tf 的或者scipy的转换

# tf
from tf.listener import xyzw_to_mat44

class oritation:
    def __init__(self):
        self.x = 0
        self.y = 0
        self.z = 0
        self.w = 0

ori = oritation()
ori.x = rot[0]
ori.y = rot[1]
ori.z = rot[2]
ori.w = rot[3]
mat44 = xyzw_to_mat44(ori) # 转换的结果为4 × 4 矩阵

# scipy
from scipy.spatial.transform import Rotation as R

# (x, y, z, w) format
r = R.from_quat([-0.716556549511624,-0.6971278819736084, -0.010016582945017661,  0.02142651612120239])
r.as_matrix()
print('rotation:\n',r.as_matrix())
rotation_matrix = r.as_matrix()
print(rotation_matrix)

其他

tf::createQuaternionMsgFromYaw(double y);
//只通过y即绕z的旋转角度计算四元数，用于平面小车。返回四元数