技術標籤：Opencv線性代數matlabpython計算機視覺

閱讀原文

旋轉矩陣轉尤拉角

• 概述
• • 尤拉角
  • 旋轉矩陣
• 將尤拉角轉為旋轉矩陣
• 將旋轉矩陣轉為尤拉角
• 總結

概述

本篇部落格，主要給出如何將3x3的旋轉矩陣轉換成尤拉角的討論和程式碼。
什麼是旋轉矩陣和尤拉角呢？

尤拉角

我們應該知道，對於一個三維空間，我們可以建立一個三維座標軸，而這個三維座標軸由X軸，Y軸和Z軸組成。當三維物體進行旋轉時，可以看成該物體依次繞著這三個軸進行某種角度的旋轉，這些角度被稱為尤拉角。值得注意的是，在行業中，尤拉角饒軸旋轉的循序是Z-Y-X，因為這樣對應著yaw-pitch-roll。

yaw：偏航角，指物體繞著Z軸旋轉

pitch：俯仰角，指物體繞著Y軸旋轉
roll：滾轉角，指物體繞著X軸旋轉

尤拉角看起來比較簡單，但是它的一個重大缺點是會出現萬向鎖問題：在俯仰角為 ± 9 0 ° \pm90^° ±90°時，第一次旋轉與第三次旋轉將使用同一個軸，這使得系統丟失了一個自由度。

旋轉矩陣

旋轉矩陣的幾何意義也很明確，把一個三維空間點(x,y,z)看為一個三維向量[x,y,z]，然後將該向量與一個矩陣相乘，得到一個變換後的三維向量。值得注意的一點是，如果你將三維空間點看成一個行向量，那旋轉矩陣是在乘號後邊，即VM。但是如果使用列向量表示，那旋轉矩陣是在乘號前邊，即M

將尤拉角轉為旋轉矩陣

考慮三維旋轉最簡單的方式是軸角形式。因為任何旋轉都可以由旋轉軸和描述旋轉量的角度來定義。
由上述的討論可知，每個軸都有其對應的旋轉矩陣。下面分別給出X軸，Y軸和Z軸的旋轉矩陣。



其中， θ x , θ y , θ z \theta_x,\theta_y,\theta_z θx​,θy​,θz​對應為每個軸的旋轉角度，即尤拉角。
為了得到最後的旋轉矩陣 R R R，我們需要依次將物體在這些矩陣上進行連續運算，因此，最後的 R R R可以表示為如下的矩陣乘法。

從尤拉角轉換為旋轉矩陣的函式片段如下：

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

將旋轉矩陣轉為尤拉角

將旋轉矩陣轉換為尤拉角有點困難，因為在大多數情況下，解決方案不是唯一的。下面程式碼的輸出完全與Matlab的rotm2euler函式一致。

首先，先對旋轉矩陣進行檢查

# 檢查一個旋轉矩陣是否有效
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

完整的轉換程式碼

# 這部分的程式碼輸出與Matlab裡邊的rotm2euler一致
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([z, y, x])

總結

三維旋轉是一個十分常見的需求，這些程式碼都可以直接使用，非常的nice。如果對完整程式碼有需求的小夥伴，可以去原網站下載，嫌麻煩，也可以下載我新增上註釋後的版本