三維座標系旋轉——旋轉矩陣到旋轉角之間的換算

ciky奇發表於2019-04-23

在做單目三維位姿估計（即估計目標物相對相機的姿態或相機相對目標物的姿態）時會用到solvepnp函式，

函式原型為：

cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]) → retval, rvec, tvec

引數解釋

objectPoints：世界座標系中的3D點座標，單位mm
imagePoints：影像座標系中點的座標，單位畫素
cameraMatrix：相機內參矩陣
distCoeffs：畸變係數
rvec：旋轉矩陣
tvec：平移矩陣
useExtrinsicGuess：是否輸出平移矩陣和旋轉矩陣，預設為false
flags：SOLVEPNP _ITERATIVE、SOLVEPNP _P3P、SOLVEPNP _EPNP、SOLVEPNP _DLS、SOLVEPNP _UPNP

內參矩陣和畸變係數都是要通過標定得到的，這個不細講，opencv官方提供了有標定例子（或者參考我的這篇文章：用matlab標定獲取相機內參矩陣和畸變係數）。函式輸出的是旋轉矩陣rvec和tvec。

本文就來說說得到了這個旋轉矩陣rvec後，如何得知目標物實際的角度呢~

旋轉矩陣是一個3×3的正交矩陣，有3個自由度。處理旋轉矩陣的問題時，通常採用旋轉矩陣的方式來描述，也可以用旋轉向量來表示，兩者之間可以通過羅德里格斯（Rodrigues）變換來進行轉換。

旋轉矩陣和旋轉向量間的轉換請參考：旋轉矩陣和旋轉向量

其中，旋轉向量的長度（模）表示繞軸逆時針旋轉的角度（弧度）。

norm為求向量的模。

程式碼如下：

theta = np.linalg.norm(rvec)
r = rvec / theta
R_ = np.array([[0, -r[2][0], r[1][0]],
               [r[2][0], 0, -r[0][0]],
               [-r[1][0], r[0][0], 0]])
R = np.cos(theta) * np.eye(3) + (1 - np.cos(theta)) * r * r.T + np.sin(theta) * R_
print('旋轉矩陣')
print(R)

反變換也可以很容易的通過如下公式實現：

空間中三維座標變換一般由三種方式實現，第一種是旋轉矩陣和旋轉向量；第二種是尤拉角；第三種是四元數。下面介紹旋轉矩陣(旋轉向量)與尤拉角實現三維空間座標變換的方法以及兩者之間的關係。

旋轉矩陣

對於一個三維空間的點 P(x,y,z)P(x,y,z)，要將其繞 zz 軸旋轉 θθ 角度是可以很簡單地用旋轉矩陣來表示的

尤拉角

此處得到結論：自旋轉的“先轉的放前面”

定角（Fixed angles）

圍繞固定的座標系轉動。固定座標系的原點，座標系再圍繞已經固定的軸轉動，全程原座標系不動。

注意！移動位置的順序可以調換，但是旋轉的順序不能調換，結果不一樣。

以X-Y-Z型為例子：即先圍繞X軸進行轉動γ°，然後圍繞Y軸進行轉動β°，最後圍繞Z軸進行轉動α°。注意逆時針為正方向。

X-Y-Z型公式：

重點：先轉的軸的 $\large R$ 放後面運算，如下

程式碼：

def isRotationMatrix(R):
    Rt = np.transpose(R)   #旋轉矩陣R的轉置
    shouldBeIdentity = np.dot(Rt, R)   #R的轉置矩陣乘以R
    I = np.identity(3, dtype=R.dtype)           # 3階單位矩陣
    n = np.linalg.norm(I - shouldBeIdentity)   #np.linalg.norm預設求二範數
    return n < 1e-6                            # 目的是判斷矩陣R是否正交矩陣（旋轉矩陣按道理須為正交矩陣，如此其返回值理論為0）


def rotationMatrixToAngles(R):
    assert (isRotationMatrix(R))   #判斷是否是旋轉矩陣（用到正交矩陣特性）

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])  #矩陣元素下標都從0開始（對應公式中是sqrt(r11*r11+r21*r21)），sy=sqrt(cosβ*cosβ)

    singular = sy < 1e-6   # 判斷β是否為正負90°

    if not singular:   #β不是正負90°
        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:              #β是正負90°
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)   #當z=0時，此公式也OK，上面圖片中的公式也是OK的
        z = 0

    return np.array([x, y, z])

備註：np.linalg.norm(求範數)

舉例：

由角度推旋轉矩陣

由旋轉矩陣推角度

尤拉角（Euler angles）

“自旋轉”，圍繞當下（自己）的座標系某軸轉動，就是每次旋轉，都固定被圍繞的某一軸，另兩軸動。

每次旋轉，整個座標系都會改變位置。

以Z-Y-Z型為例的公式：

重點：先轉的軸的 $\large R$ 放前面運算，如下

舉例：

矩陣轉角度:

注意：自旋轉的“先轉的放前面”

尤拉角轉旋轉矩陣

尤拉角通過將剛體繞過原點的軸(i,j,k)旋轉θ，分解成三步，如下圖（藍色是起始座標系，而紅色的是旋轉之後的座標系）
這裡寫圖片描述
如果將每一個角度用旋轉矩陣表示如下：

所以，容易得到，尤拉角轉旋轉矩陣如下：

程式碼：

/**
尤拉角計算對應的旋轉矩陣
**/
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // 計算旋轉矩陣的X分量
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
 
 
    // 計算旋轉矩陣的Y分量
    Mat R_y = (Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
 
 
    // 計算旋轉矩陣的Z分量
    Mat R_z = (Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
 
 
    // 合併 
    Mat R = R_z * R_y * R_x;
 
 
    return R;
}

旋轉矩陣轉尤拉角

將旋轉矩陣表示如下：
這裡寫圖片描述
則可以如下表示尤拉角：

程式碼：

/**
 * 功能： 1. 檢查是否是旋轉矩陣
**/
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;    
}

/**
 * 功能： 1. 通過給定的旋轉矩陣計算對應的尤拉角
**/
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);   
}

旋轉向量轉尤拉角（經過四元數）程式碼如下：

# 從旋轉向量轉換為尤拉角
def get_euler_angle(rotation_vector):
    # calculate rotation angles
    theta = cv2.norm(rotation_vector, cv2.NORM_L2)
    
    # transformed to quaterniond
    w = math.cos(theta / 2)
    x = math.sin(theta / 2)*rotation_vector[0][0] / theta
    y = math.sin(theta / 2)*rotation_vector[1][0] / theta
    z = math.sin(theta / 2)*rotation_vector[2][0] / theta
    
    ysqr = y * y
    # pitch (x-axis rotation)
    t0 = 2.0 * (w * x + y * z)
    t1 = 1.0 - 2.0 * (x * x + ysqr)
    print('t0:{}, t1:{}'.format(t0, t1))
    pitch = math.atan2(t0, t1)
    
    # yaw (y-axis rotation)
    t2 = 2.0 * (w * y - z * x)
    if t2 > 1.0:
        t2 = 1.0
    if t2 < -1.0:
        t2 = -1.0
    yaw = math.asin(t2)
    
    # roll (z-axis rotation)
    t3 = 2.0 * (w * z + x * y)
    t4 = 1.0 - 2.0 * (ysqr + z * z)
    roll = math.atan2(t3, t4)
    
    print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll))
    
	# 單位轉換：將弧度轉換為度
    Y = int((pitch/math.pi)*180)
    X = int((yaw/math.pi)*180)
    Z = int((roll/math.pi)*180)
    
    return 0, Y, X, Z

在3D 空間中，表示物體的旋轉可以由三個尤拉角來表示：
pitch圍繞X軸旋轉，叫俯仰角。
yaw圍繞Y軸旋轉，叫偏航角。
roll圍繞Z軸旋轉，叫翻滾角。
這三個角的順序對旋轉結果有影響。