欧拉角-旋转矩阵相互转换

转自：https://blog.csdn.net/u012525096/article/details/78890463

theta  描述应该是欧拉角

欧拉角->旋转矩阵

        对于3D旋转的最简单的方法是以轴角形式思考。任何旋转都可以由一个旋转轴来定义，一个角度可以描述旋转的量。比方说，你想旋转一个点或一个参考框架绕x轴旋转θxθx度。与该旋转对应的旋转矩阵由下式给出： 

Rx=⎡⎣⎢1000cos(θx)sin(θx)0−sin(θx)(cosθx)⎤⎦⎥Rx=[1000cos⁡(θx)−sin⁡(θx)0sin⁡(θx)(cos⁡θx)]

        θyθy和θzθz关于y和z轴的旋转可以写成： 

Ry=⎡⎣⎢cos(θy)0−sin(θy)010sin(θy)0(cosθy)⎤⎦⎥Ry=[cos⁡(θy)0sin⁡(θy)010−sin⁡(θy)0(cos⁡θy)]

Rz=⎡⎣⎢cos(θz)sin(θz)0−sin(θz)(cosθz)0001⎤⎦⎥Rz=[cos⁡(θz)−sin⁡(θz)0sin⁡(θz)(cos⁡θz)0001]

        关于任意轴的旋转RR可以用关于Z，Y和最后X轴的连续旋转来写，使用下面显示的矩阵乘法。 

R=RzRyRxR=RzRyRx

        在这个公式中， θxθx ， θyθy 和 θzθz 是欧拉角。给定这三个角度，首先找到 RxRx ， RyRy 和 RzRz ，然后将它们相乘得到 RR ，就可以很容易地找到旋转矩阵。

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;
}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29

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

        将旋转矩阵转换成欧拉角是有点棘手的。该解决方案在大多数情况下不是唯一的。使用上一节中的代码，可以验证与欧拉角[0.1920,2.3736,1.170][0.1920,2.3736,1.170]（或[11,136,64][11,136,64])(度)）和[−2.9496,0.7679，−2.0246][−2.9496,0.7679，−2.0246]（或[−169,44,−116][−169,44,−116](度))实际上是相同的，尽管欧拉角看起来非常地不同。下面的代码显示了给定旋转矩阵的欧拉角的一种方法。下面代码的输出应该与MATLAB的rotm2euler.mrotm2euler.m的输出完全匹配，但是x和z的顺序是交换的(Z-Y-X)。

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