其他细节：

• 所以雷达到图像，涉及两次坐标系变化，雷达->相机->图像，所以就有了内外参。 所以需要两次乘法，顺序不能错！！！，因为y = cam2img @lidar2cam@x_lidar。如果仅仅2个坐标系的变化，1个外参即可。

• 外参数就是一个旋转+平移，两个可以合并为1个4元数，将矩阵乘加变成矩阵乘。RX+T->[4x4]X

• 构造4元数，变换矩阵用单位阵，坐标矩阵用全为1的矩阵；

• 图像坐标系最终结果需要归一化，最后的得到的结果，[3]不用管，[:3]需要/z

• 反变化求逆矩阵即可y=Bx， x=B^-1y。

• 当坐标维度是（Nx4），单次变化可以用两次转置坐标，y1 = (lidar2img @ x_4d.T).T，也可以一次转置外参x_4d @ lidar2img.T。因为（A × B）T = BT × AT。

• 一般车辆的坐标系以雷达为中心，雷达就是ego。bev是雷达向下平移到地面（也就是雷达到高度）

• 总结坐标变化公式1：PB = A2B @ PA; ====> PA = A2B-1@ PB; =====> B2A= A2B-1

• 总结坐标变化公式2: PC = B2C @ A2B @ PA =A2C @ PA==========> A2C = B2C @ A2B

例子代码

import numpy as np
np.random.seed(0)
#x是雷达坐标系，转换到像素（图片）坐标系。
x = np.random.randint(0, 10, (8, 3))

#此时需要lidar2cam变换参数矩阵y=B*X^T
#旋转矩阵
cam2lidar_rotate = np.random.rand(3, 3)
#平移矩阵
cam2lidar_translation = np.random.rand(1, 3)
#构造外参矩阵，单位矩阵，4元数，矩阵乘法，
cam2lidar = np.eye(4)
cam2lidar[:3, :3] = cam2lidar_rotate
cam2lidar[:3, 3] = cam2lidar_translation
#矩阵逆运算。y=B*X => X= B^-1* y,所以直接求逆即可。
lidar2cam = np.linalg.inv(cam2lidar)

#内参
cam2img = np.random.rand(3, 3)
#内参构造4元数，为了矩阵乘法
K = np.eye(4)
K[:3,:3] = cam2img
lidar2img = K @ lidar2cam ###lidar2cam @ K顺序不能反！！！

#x也要构造4元，np.ones()
x_4d = np.ones((x.shape[0], 4))
x_4d[:, :3] = x
#此处的两个转制是因为原来是8x3需要变成3x8,然后再转回去。

y1 = (lidar2img @ x_4d.T).T
#转置运算等价一下（A × B）T = BT × AT。
y2 = x_4d @ lidar2img.T

print(y1, y2, y1==y2)
final_y = y1/y1[:, 2]

https://mp.weixin.qq.com/s/tTRCjZBRZcnb59nX3FRR8w
https://zhuanlan.zhihu.com/p/597554089
各种坐标系
https://developer.baidu.com/article/detail.html?id=290582