自动驾驶两个传感器之间的坐标系转换

有两种方式可以实现两个坐标系的转换。
车身坐标系下一个点p_car，需要转换到相机坐标系下，旋转矩阵R_car2Cam，平移矩阵T_car2Cam。点p_car在相机坐标系下记p_cam.

方法1：先旋转再平移

p_cam = T_car2Cam * p_car + T_car2Cam
需要注意的是，这里的平移矩阵T_car2Cam是车身坐标系原点在相机坐标系下面的坐标！
用具体数值表示如下：

import numpy as np

rot = 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 np

R_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 np

R_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矩阵再求逆就可以得到。