不同坐标系下3D bbox通过中心点坐标和长宽高求8个角点问题

这种问题可分解为三步:

1.根据长(l)宽(w)高(h)计算出一个和正向运动方向重合的3D bbox的8个角点坐标向量:

x_corners = [-l/2,-l/2,l/2,l/2,-l/2,-l/2,l/2,l/2]
y_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
#z_corners = [0,0,0,0,h,h,h,h]   #for kitti3d dataset
z_corners = [-h/2,-h/2,-h/2,-h/2,h/2,h/2,h/2,h/2]  #for our lidar-coordination-based dataset

2.根据3D bbox的朝向角(rotation angle)构建旋转矩阵与8个角点的坐标相称以将8个点旋转到实际位置，不同3D坐标系下，无论是相机坐标系还是雷达坐标系还是其他3d坐标系下，旋转问题处理起来其实是相同的，关键是看绕哪个轴旋转以及朝向角的正/负方向，也就是说无论坐标系定义其 x,y,z实际指向哪里，我们都只需看绕哪个轴旋转以及旋转角度的值(已含正/负方向)即可定义出对应的旋转矩阵(旋转矩阵本质是由正交向量组成，通过与矩阵相乘来对图像做旋转或缩放这样的线性变换以及SVD之类原理的解释可参考有关书籍资料，这里不多说)：

# Rotation around x-axis
def rotx(r):
    c = np.cos(r)
    s = np.sin(r)
    return np.array([[1,  0,  0],
                     [0,  c, -s],
                     [0,  s,  c]])

# Rotation around y-axis
def roty(r):
    c = np.cos(r)
    s = np.sin(r)
    return np.array([[c,  0,  s],
                     [0,  1,  0],
                     [-s, 0,  c]])

# Rotation around z-axis
def  rotz(r):
    c = np.cos(r)
    s = np.sin(r)
    return np.array([[c, -s,  0],
                     [s,  c,  0],
                     [0,  0,  1]])

绕哪个轴旋转时，这个轴方向的坐标是无需旋转的只需保持原样即可，一般为了保持(x,y,z)三个维度在运算上的统一，给旋转矩阵的行列维度都增加对围绕旋转的轴这个方向的坐标的乘以0(不参与运算)或1(复制)处理。上面的r是3D bbox的实际朝向角，lidar坐标系下，r一般定义为与lidar坐标系下的x轴的夹角且以逆时针旋转为正向，相机坐标系下，r一般定义为与相机坐标系下的x轴的夹角且以顺时针方向旋转为正向，所以lidar坐标系下，旋转坐标一般调用rotz(r)，而相机坐标系下一般调用roty(r)。为何旋转子矩阵是正负cos(r)和正负sin(r)的组合组成的，了解以特征向量组成的矩阵的作用的不难理解，或者画一下图也比较容易明白。

3.对3D bbox旋转后的各角点的坐标(此时3D bbox各个角点的坐标仍是相对3D bbox中心点的相对坐标)加上中心点的坐标得到相应坐标系下的绝对坐标。

完整代码如下(以lidar坐标系统下3D bbox的中心在其几何中心为例):

def center2corner_3d(x,y,z,w,l,h,r):
    x_corners = [-l/2,-l/2,l/2,l/2,-l/2,-l/2,l/2,l/2]
    y_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
    #z_corners = [0,0,0,0,h,h,h,h]   #for kitti3d dataset
    z_corners = [-h/2,-h/2,-h/2,-h/2,h/2,h/2,h/2,h/2]  #for our lidar-coordination-based dataset
    R = rotz(r)
    corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners])) + np.vstack([x,y,z])
    return np.transpose(corners_3d,(0,2,1))