罗德里格斯求解实验——加深理解

使用罗德里格斯公式求R

问题：坐标系绕（0.697978, 0.444596, 0.561392）轴旋转40°。考察坐标变换。

In [1]: import numpy as np

In [2]: R0 = np.array([[0.880021, 0.433456, -0.194108], [-0.288255, 0.812298, 0.507045], [0.377454, -0.390258, 0.839778]]) #旋转之后的坐标系R矩阵
Out[2]:
array([[ 0.880021,  0.433456, -0.194108],
       [-0.288255,  0.812298,  0.507045],
       [ 0.377454, -0.390258,  0.839778]])

In [3]: a = np.array([[0.697978], [0.444596], [0.561392]])    # 旋转轴世界坐标
Out[3]:
array([[0.697978],
       [0.444596],
       [0.561392]])

In [4]: A = np.array([[0, -0.561392, 0.444596], [0.561392, 0, -0.697978], [-0.444596, 0.697978, 0]])    # a对应的反对称矩阵
Out[4]:
array([[ 0.      , -0.561392,  0.444596],
       [ 0.561392,  0.      , -0.697978],
       [-0.444596,  0.697978,  0.      ]])

In [5]: theta = np.pi * 40/180    # 旋转角

In [6]: I = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])    # 单位矩阵
Out[6]:
array([[1, 0, 0],
       [0, 1, 0],
       [0, 0, 1]])

In [7]: R1 = np.cos(theta)*I + (1-np.cos(theta))*np.dot(a, a.T) + np.sin(theta)*A    # 使用罗德里格斯公式求R
Out[7]:
array([[ 0.88002134, -0.28825515,  0.37745377],
       [ 0.4334565 ,  0.81228941, -0.39025803],
       [-0.19410783,  0.50704519,  0.83977811]])

In [8]: R1.T-R0
Out[8]:
array([[ 3.41123811e-07,  4.95359059e-07,  1.73422696e-07],
       [-1.48195232e-07, -8.59060440e-06,  1.94764389e-07],
       [-2.26344911e-07, -2.57031936e-08,  1.05155526e-07]])

    使用\({\boldsymbol a} = {\boldsymbol R} {\boldsymbol a'} + {\boldsymbol t}\)式，在世界坐标系中的向量\({\boldsymbol a}\)(世界坐标系下的坐标值)，到了相机坐标系中变成了\({\boldsymbol a}'\)(相机坐标系下的坐标值). 其中R是相机坐标系的基组成的矩阵\([{\boldsymbol e}_1'\;{\boldsymbol e}_2'\;{\boldsymbol e}_3']\)，\({\boldsymbol e}_1', {\boldsymbol e}_2', {\boldsymbol e}_3'\)表示为世界坐标系下的坐标(这是因为世界坐标系的标准正交基组成的矩阵是单位矩阵). 如上所述，我把这里面的世界坐标系看作是变换后的坐标系，把相机坐标系看作是原坐标系.

     通过计算结果我发现，我使用罗德里格斯公式算出来的R是\({\boldsymbol a} = {\boldsymbol R} {\boldsymbol a'} + {\boldsymbol t}\)中R的逆矩阵（转置矩阵）.    

    

    结论：使用罗德里格斯公式时的R应该是原坐标系的基组成的矩阵的转置点乘变换后的坐标系的基组成的矩阵.