旋转矩阵与欧拉角的相互转换及代码

6 篇文章 2 订阅

这篇博客将会分享旋转矩阵和欧拉角的相互转换。

三维旋转矩阵有三个自由度,旋转能够使用多种方法表示(旋转矩阵,欧拉角,四元数,轴角,李群与李代数),比如一个3x3的矩阵,比如四元数,甚至可以将旋转表示为三个数字,即绕三个轴x,y,z的旋转角度。在原始的欧拉角公式中,旋转是通过围绕Z轴,Y轴,X轴分别旋转得到的。它分别对应于偏航,俯仰和横滚。

当定义一个旋转时,可能还会引起歧义,比如对于一个给定的点(x,y,z),可以把这个点视为行向量(x,y,z)或者列向量量 [ x , y , z ] T [x, y, z]^{T} [x,y,z]T,如果使用行向量,那么就必须后乘旋转矩阵(旋转矩阵在后),如果使用列向量,就要左乘旋转矩阵(旋转矩阵在前)。

在MATLAB中,rotm2euler.m能够实现旋转矩阵到欧拉角的转换。下文中将要叙述的转换代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是,它的旋转顺序是Z-Y-X,而下面的实现是X-Y-Z。

在计算将旋转矩阵转换成欧拉角之前,先介绍一下欧拉角转换为旋转矩阵的方法。

欧拉角转换为旋转矩阵

假如已知旋转角,绕着X-Y-Z三个轴的角度分别为 θ x θ y θ z \theta_{x} \quad \theta_{y} \quad \theta_{z} θxθyθz那么三个旋转矩阵可以表示如下:
在这里插入图片描述
如果旋转顺序为Z-Y-X的顺序,那么旋转矩阵可表示如下:
在这里插入图片描述
关于上面的旋转矩阵,以z轴旋转为例:
第一种物理意义,坐标系的旋转。 其应用场景有SLAM,机械臂运动学等。
在这里插入图片描述
如上图所示, P点不变,坐标系 O-XYZ旋转 α ,得到新的坐标系O-X’Y’Z’,在新坐标系下,P点的坐标变为P’ ,则有:
在这里插入图片描述
第二种物理意义,向量的旋转,其应用场景有机器人的姿态估计等。
向量旋转
如上图所示,坐标系 O-XYZ不变, P’点旋转α ,得到新的点P (我也想反过来,但用的教材上的图,没办法),则有:
在这里插入图片描述

对应于上述旋转顺序的C++代码如下:

C++
// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
     
    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
     
    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
        
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;  
    return R;
 
}
Python
# Calculates Rotation Matrix given euler angles.
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

在OpenCV中将旋转矩阵转换为欧拉角

将旋转矩阵转换为欧拉角就不是那么容易,不同的旋转顺序对应着不同的旋转角度。使用上面的代码,即使欧拉角看起来非常不同,您也可以验证与欧拉角[0.1920、2.3736、1.1170](或[[11,136,64]度)和[-2.9496,0.7679,-2.0246](或[-169,44,-116]度)相对应的旋转矩阵实际上是相同的。下面的代码显示了在给定旋转矩阵的情况下找到欧拉角的方法。以下代码的输出应与MATLAB的rotm2euler的输出完全匹配,但x和z的顺序会互换。

C++
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
    
    return  norm(I, shouldBeIdentity) < 1e-6;
    
}

// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{

    assert(isRotationMatrix(R));
    
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );

    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);   
}
Python
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6


# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
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])
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值