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

最新推荐文章于 2024-05-14 14:40:55 发布

weixin_34044446

最新推荐文章于 2024-05-14 14:40:55 发布

阅读量3k

点赞数 2

文章标签： python 旋转矩阵

本文链接：https://blog.csdn.net/weixin_34044446/article/details/113489741

版权

四元数->欧拉角

$equation?tex=roll%2C+pitch%2C+yaw+%E5%88%86%E5%88%AB%E4%B8%BA+%5Calpha%2C+%5Cbeta%2C+%5Cgamma%EF%BC%8C%E5%88%99%E6%9C%89$

$equation?tex=+%5Cbegin%7Bcases%7D+%5Calpha%3Datan2%282%28wx%2Byz%29%2C1-2%28xx%2Byy%29%29%5C%5C+%5Cbeta%3Dasin%282%28wy-xz%29%29%5C%5C+%5Cgamma%3Datan2%282%28wz%2Bxy%29%2C1-2%28zz%2Byy%29%29+%5Cend%7Bcases%7D$

C++

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

double roll, pitch, yaw;//定义存储rpy的容器
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])

欧拉角->四元数

$equation?tex=roll%2C+pitch%2C+yaw+%E5%88%86%E5%88%AB%E4%B8%BA+%5Calpha%2C+%5Cbeta%2C+%5Cgamma%EF%BC%8C%E5%88%99%E6%9C%89$

$equation?tex=x%3Dsin%28%5Cbeta%2F2%29sin%28%5Cgamma%2F2%29cos%28%5Calpha%2F2%29%2Bcos%28%5Cbeta%2F2%29cos%28%5Cgamma%2F2%29sin%28%5Calpha%2F2%29%5C%5C+y%3Dsin%28%5Cbeta%2F2%29cos%28%5Cgamma%2F2%29cos%28%5Calpha%2F2%29%2Bcos%28%5Cbeta%2F2%29sin%28%5Cgamma%2F2%29sin%28%5Calpha%2F2%29%5C%5C+z%3Dcos%28%5Cbeta%2F2%29sin%28%5Cgamma%2F2%29cos%28%5Calpha%2F2%29-sin%28%5Cbeta%2F2%29cos%28%5Cgamma%2F2%29sin%28%5Calpha%2F2%29%5C%5C+w%3Dcos%28%5Cbeta%2F2%29cos%28%5Cgamma%2F2%29cos%28%5Calpha%2F2%29-sin%28%5Cbeta%2F2%29sin%28%5Cgamma%2F2%29sin%28%5Calpha%2F2%29%5C%5C$

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

欧拉角->旋转矩阵

$equation?tex=R%3DR_z%28%5Cphi%29R_y%28%5Ctheta%29R_x%28%5Cpsi%29%3D%5Cbegin%7Bbmatrix%7D+cos%5Ctheta+cos%5Cphi+%26+sin%5Cpsi+sin%5Ctheta+cos%5Cphi-cos%5Cpsi+sin%5Cphi+%26+cos%5Cpsi+sin%5Ctheta+cos%5Cphi%2Bsin%5Cpsi+sin%5Cphi%5C%5C+cos%5Ctheta+sin%5Cphi+%26+sin%5Cpsi+sin%5Ctheta+sin%5Cphi%2Bcos%5Cpsi+cos%5Cphi++%26+cos%5Cpsi+sin%5Ctheta+sin%5Cphi+-+sin%5Cpsi+cos%5Cphi%5C%5C+-sin%5Ctheta+%26+sin%5Cpsi+cos%5Ctheta+%26+cos%5Cpsi+cos%5Ctheta+%5Cend%7Bbmatrix%7D$

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

旋转矩阵->欧拉角

$equation?tex=R%3D%5Cbegin%7Bbmatrix%7D+r_%7B11%7D+%26+r_%7B12%7D+%26+r_%7B13%7D+%5C%5C+r_%7B21%7D+%26+r_%7B22%7D+%26+r_%7B23%7D+%5C%5C+r_%7B31%7D+%26+r_%7B32%7D+%26+r_%7B33%7D%5Cend%7Bbmatrix%7D$ 则欧拉角为

$equation?tex=+%5Cbegin%7Bcases%7D+%5Ctheta_x%3Datan2%28r_%7B32%7D%2C+r_%7B33%7D%29+%5C%5C+%5Ctheta_y%3Datan2%28-r_%7B31%7D%2C+%5Csqrt%7Br_%7B32%7D%5E2%2Br_%7B33%7D%5E2%7D%29+%5C%5C+%5Ctheta_z%3Datan2%28r_%7B21%7D%2C+r_%7B11%7D%29+%5Cend%7Bcases%7D$

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

旋转矩阵->四元数

$equation?tex=+%5Cbegin%7Bcases%7D+w%3D%5Cfrac%7B%5Csqrt+%7Btr%28R%29%2B1%7D%7D2+%5C%5C+x%3D%5Cfrac+%7Br_%7B32%7D-r_%7B23%7D%7D+%7B4w%7D+%5C%5C+y%3D%5Cfrac+%7Br_%7B12%7D-r_%7B31%7D%7D+%7B4w%7D+%5C%5C+z%3D%5Cfrac+%7Br_%7B21%7D-r_%7B12%7D%7D+%7B4w%7D+%5Cend%7Bcases%7D$

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

四元数->旋转矩阵

四元数

$equation?tex=q%3D%5Cbegin%7Bbmatrix%7D+q_0+%26+q_1+%26+q_2+%26+q_3+%5Cend%7Bbmatrix%7D$

$equation?tex=R%3D%5Cbegin%7Bbmatrix%7D+1-2q_2%5E2-2q_3%5E2+%26+2q_1q_2%2B2q_0q_3+%26+2q_1q_3-2q_0q_2%5C%5C+2q_1q_2-2q_0q_3+%26+1-2q_1%5E2-2q_3%5E2+%26+2q_2q_3%2B2q_0q_1+%5C%5C+2q_1q_3%2B2q_0q_2+%26+2q_2q_3-2q_0q_1+%26+1-2q_1%5E2-2q_2%5E2%5Cend%7Bbmatrix%7D$

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的旋转角度计算四元数，用于平面小车。返回四元数

weixin_34044446

关注

2
点赞
踩
28

收藏

觉得还不错? 一键收藏
0
评论
python 旋转矩阵_欧拉角，四元数，旋转矩阵相互转化（c++, python)

四元数->欧拉角 C++#include <tf/tf.h>tf::Quaternion quat;tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);double roll, pitch, yaw;//定义存储rpy的容器tf::Matrix3x3(quat).getRPY(roll, pitch, yaw...
复制链接

扫一扫