Waymo dataset+mmdet3d的坐标系问题

mmdet3d 在处理 waymo dataset的时候，3D/2D gt box, point cloud等数据进行了非常多的坐标系转换。本身waymo的坐标系也有不少。
写这篇文章的motivation主要是，自己在处理3D point投影到2D image的过程中产生了两个问题：

1. 枚举ego centric 3D点投到5个相机的时候，发现覆盖范围是歪的，front camera没有正对前方；别人的方法如CMKD，BEV range都是正的
2. 使用同样的投影矩阵，将3D gt box的center投到相机时，pixel坐标和waymo给的projected laser label中的坐标不一致。

最后1是因为u,v弄反了，2是因为waym的projected 2d box，是最小的，能够框住所有lidar points in 3D box 的矩形，所以3D 和 2D gt box的center没有对应关系。

现在总结一下waymo dataset的坐标系，以及mmdet3d是如何转换waymo坐标系的。其中最为重要的，还是gt labels前后的坐标变化。

waymo dataset coordinate system

首先得说一下waymo dataset的结构。以training split为例，有700多个tfrecord文件，每个文件对应一个scene，是长度为20s左右的视频，大概有200帧，每帧有lidar和camera的数据，还有gt labels。
一帧对应一个Frame的protobuf类，具体的描述在waymo dataset项目的dataset.proto里。

global frame（coordinate）

对于一个scene，我们首先有一个预设的全局坐标系，global frame坐标系在ego运动之前就设定好了，z轴背对地心，x轴向东，y轴向北。车在global frame里移动，位姿由Frame.pose给出，是一个4x4的转移矩阵T，即齐次坐标下的旋转加位移[R,t]。

vehicle frame（coordinate）

车坐标系，ego-centric的坐标系，原点固定于车的某个点上，x轴向前，y轴向左，z轴向上。可以说，车的位姿，和这个坐标系是绑定的。当要计算不同frame间点的相对关系，我们可以利用Frame.pose，把点都转到global frame下进行计算。注意到，这里的T是vehicle转global的矩阵，用法是$p{t}_{global}=T\times p{t}_{vehicle},p{t}_{vehicle}=[x,y,z,1{]}^T$。

sensor frame

vehicle上装了许多sensor，每个sensor的位姿，对于每个scene来说是固定的。类似于frame.pose，每个sensor的位姿，或者说“外参”，都定义在vehicle坐标系下，同样有T=[R,t]。其中位移t在不同帧之间似乎是一致的（仅观察了部分top lidar的t），R有轻微不同。top lidar的转移矩阵（外参）记录在frames[0].context.laser_calibrations[0].extrinsic.transform中，意义为top lidar转移到vehicle坐标系的转移矩阵，其他的sensor类似。
每个sensor的坐标系，其x,y,z轴如下图所示，sensor的位姿定义了其坐标系。

top lidar和front camera的坐标轴朝向基本和vehicle一致，只是有些位移，那么可以猜测其外参矩阵[R,t]中的R，大概为对角阵，而不同帧之间的外参应该基本相同（常理来说sensor的位置固定）。
比如validation set的scene_id=50（0开始计）的top lidar和front cam的外参如下

lidar2vehicle = np.array(frames[0].context.laser_calibrations[0].extrinsic.transform).reshape(4,4)
print(lidar2frame)
[[ 0.99991058  0.01128794  0.0071704   4.07      ]
 [-0.01139378  0.99982415  0.01489488  0.        ]
 [-0.00700101 -0.01497524