有两种方式可以实现两个坐标系的转换。
车身坐标系下一个点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）先平移到车身坐标系。然后再旋转，注意这个时候的旋转矩阵是需要相机坐标系到车身坐标系的，直接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矩阵再求逆就可以得到。
