簡介

        在這篇文章中，我將分享將一個3×3旋轉矩陣轉換成尤拉角的程式碼，反之亦然。3D旋轉矩陣可以讓你的頭旋轉。 我知道這是一個壞的雙關語，但真相有時可能是非常小的！
        一個旋轉矩陣有三個自由度，數學家已經行使了他們的創造自由，以每個想象的方式來表示3D旋轉——或使用三個數字、或使用四個數字、或使用一個3×3矩陣。 還有很多不同的方式用三個數字表示一個旋轉或用四個數字的一些方法來表示旋轉。
        例如，3D中的旋轉可以表示為三個角度，可以將其表示在X、Y、Z三個軸上。但是，這三個角度也可以表示到Z，Y和X軸上（表示方法不同）。這些角度被稱為尤拉角或Tait–Bryan角。在原始尤拉角公式中，通過圍繞Z，X軸和再對Z軸（或者對於Y-X-Y或Z-Y-Z）的連續旋轉來描述旋轉。當旋轉被指定為圍繞三個不同的軸（例如X-Y-Z）的旋轉時，它們應該被稱為泰特—布賴恩（Tait-Bryan）角，但是流行的術語仍然是尤拉角，所以我們也將它們稱為尤拉角。有六種可以用Tait-Bryan角度描述旋轉的方法——X-Y-Z，X-Z-Y，Y-Z-X，Y-X-Z，Z-X-Y，Z-Y-X。現在你在想，選擇很簡單。 讓我們選擇X-Y-Z。 對 麼？ 答案是錯誤的。 行業標準是Z-Y-X，因為它對應於yaw偏航，pitch俯仰和roll滾轉。

        定義旋轉矩陣時還有其他的含糊之處。給定一個點（x，y，z），你可以把這個點想象成一個行向量[x，y，z]或一個列向量[x，y，z]T。如果使用行向量，則必須對3×3旋轉矩陣進行右乘；如果使用列向量表示，則用3×3旋轉矩陣左乘該列向量。這兩個旋轉矩陣是不一樣的（它們是彼此的轉置）。

        我的觀點是沒有標準的方法來將旋轉矩陣轉換成尤拉角。所以，我決定（幾乎）與MATLAB實現的rotm2euler.m一致。唯一的區別是他們返回的尤拉角z軸是第一個和x軸是最後一個（Z-Y-X）。我的程式碼先返回x（X-Y-Z）。

尤拉角->旋轉矩陣

        對於3D旋轉的最簡單的方法是以軸角形式思考。任何旋轉都可以由一個旋轉軸來定義，一個角度可以描述旋轉的量。比方說，你想旋轉一個點或一個參考框架繞x軸旋轉θx度。與該旋轉對應的旋轉矩陣由下式給出：

        θy和θz關於y和z軸的旋轉可以寫成：

        關於任意軸的旋轉R可以用關於Z，Y和最後X軸的連續旋轉來寫，使用下面顯示的矩陣乘法。

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

在OpenCV中將旋轉矩陣轉換為尤拉角

        將旋轉矩陣轉換成尤拉角是有點棘手的。該解決方案在大多數情況下不是唯一的。使用上一節中的程式碼，可以驗證與尤拉角[0.1920,2.3736,1.170]（或[11,136,64])(度)）和[−2.9496,0.7679，−2.0246]（或[−169,44,−116](度))實際上是相同的，儘管尤拉角看起來非常地不同。下面的程式碼顯示了給定旋轉矩陣的尤拉角的一種方法。下面程式碼的輸出應該與MATLAB的rotm2euler.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);
}