有兩種方式可以實現兩個坐標系的轉換。
車身坐標系下一個點p_car，需要轉換到相機坐標系下，旋轉矩陣R_car2Cam，平移矩陣T_car2Cam。點p_car在相機坐標系下記p_cam.
方法1：先旋轉再平移
p_cam = T_car2Cam * p_car + T_car2Cam
需要注意的是，這里的平移矩陣T_car2Cam是車身坐標系原點在相機坐標系下面的坐標！
用具體數值表示如下：
import numpy as nprot = np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])tran = np.array([0.106073, 1.411398, -1.971551])p_veh = np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 = rot.dot(p_veh) + tran.reshape(3,1)
print(p_cam_1)#[[-6.16055871]#[-6.28240156]#[ 1.27418073]]
方法2：先平移再旋轉
在方法1中， 是把p經過旋轉，轉到相機坐標系， 再加上平移向量，這個平移向量就是相機坐標系的，所以可以直接相加減。
同樣的，我們這里的方法2是需要先平移，就要求平移的向量是車身坐標系下的才能相加減。就是我們如果現在需要先平移，那么就要求這個平移向量是在車身坐標系下的才能直接相加減。
即需要知道相機坐標原點相對于車身坐標系。
如圖就是把左邊的相機坐標系原點（0,0,0）先平移到車身坐標系的原點(0,0,0)處，注意平移后的還是在相機坐標系下，現在tran = np.array([0.106073, 1.411398, -1.971551])是相機坐標系下，car坐標系原點的位置。
就是在相機坐標系下一個點Q（0.106073, 1.411398, -1.971551），我現在就要把我相機坐標系原點挪到這個Q點。就是Q點在新相機坐標系下是(0, 0, 0). 所以之前的點n轉到以Q點為原點的新相機坐標系需要經過n-（0.106073, 1.411398, -1.971551），
比如之前的原點(0,0,0)轉到新平移后的Q為原點相機坐標系下是：(0,0,0)-（0.106073, 1.411398, -1.971551）
相機坐標系原點平移到車身坐標系原點，這個時候還差一步旋轉。旋轉，注意這個時候的旋轉矩陣是需要相機坐標系到車身坐標系的，因為我們現在點都在相機坐標系下，只是平移了，需要相機到車身的旋轉矩陣，直接R_car2Cam取個逆就是。
Cam_0_to_car = inv(R_car2Cam) * (Cam_0 - T_car2Cam)
import numpy as npR_car2Cam = np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])T_car2Cam = np.array([0.106073, 1.411398, -1.971551])p_car = np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 = R_car2Cam.dot(p_car) + T_car2Cam.reshape(3,1)
print(p_cam_1)#旋轉矩陣的逆等于其轉置
Cam_0_to_car = (np.dot(R_car2Cam.T, (np.array([[0, 0, 0]]).T - T_car2Cam[..., np.newaxis])).T).reshape(3,1)
p_cam_2 = R_car2Cam.dot(p_car - Cam_0_to_car)
print(p_cam_2)
輸出：
[[-6.16055871][-6.28240156][ 1.27418073]]
[[-6.16055872][-6.28240152][ 1.27418082]]RT矩陣的逆
import numpy as npR_car2Cam = np.array([[ 4.6776832e-04, -9.9942386e-01, -3.3936482e-02],[ 3.1350527e-02, 3.3934463e-02, -9.9893224e-01],[ 9.9950832e-01, -5.9665786e-04, 3.1348340e-02]])T_car2Cam = np.array([0.106073, 1.411398, -1.971551])p_car = np.array([3, 6, 8]).reshape(3, 1)
p_cam_1 = R_car2Cam.dot(p_car) + T_car2Cam.reshape(3,1)
print(p_cam_1)Cam_0_to_car = (np.dot(R_car2Cam.T, (np.array([[0, 0, 0]]).T - T_car2Cam[..., np.newaxis])).T).reshape(3,1)
p_cam_2 = R_car2Cam.dot(p_car - Cam_0_to_car)
print(p_cam_2)rt = np.eye(4)
rt[:3, :3] = R_car2Cam
rt[:3, -1] = T_car2Cam
rt_1 = np.linalg.inv(rt)print("===>rt")
print(rt)
print("===>inv rt")
print(rt_1)print("==>>Cam_0_to_car")
print(Cam_0_to_car)[[-6.16055871][-6.28240156][ 1.27418073]]
[[-6.16055872][-6.28240152][ 1.27418082]]
===>rt
[[ 4.6776832e-04 -9.9942386e-01 -3.3936482e-02 1.0607300e-01][ 3.1350527e-02 3.3934463e-02 -9.9893224e-01 1.4113980e+00][ 9.9950832e-01 -5.9665786e-04 3.1348340e-02 -1.9715510e+00][ 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.0000000e+00]]
===>inv rt
[[ 4.67768326e-04 3.13505285e-02 9.99508364e-01 1.92628402e+00][-9.99423904e-01 3.39344610e-02 -5.96657759e-04 5.69405203e-02][-3.39364847e-02 -9.98932217e-01 3.13483392e-02 1.47529553e+00][ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
==>>Cam_0_to_car
[[1.92628394][0.05694051][1.47529556]]這里求相機坐標原點在車身坐標系下的坐標， 其實用RT矩陣再求逆就可以得到。
