3D目标检测基础知识

如何描述3D空间中的一个物体

• 位置
  即xyz坐标

• 大小
  lwh长宽高，即3D 框的尺寸 (x_size, y_size, z_size)，按惯例定义为物体 3D 框在航向角 yaw 角度为 0 时沿着 x, y, z 轴三个方向的长度

• 姿态
  三种表达方式：欧拉角、旋转矩阵、四元数

  • 欧拉角
    分为pitch俯仰角、yaw航向角（偏航角）、roll翻滚角。车辆等地面上的物体，一般不会发生翻转和倾斜，
    因此一般只用考虑航向角。
    航向角的定义：选择一个轴作为重力轴，在垂直于重力轴的平面上选择一个参考方向，则参考方向的朝向角 yaw 为 0，朝向角沿着顺时针方向增大。如下图，z轴为重力轴，参考方向为y轴, 车辆前进方向与参考方向y轴的夹角则为yaw。需要注意是的，下图的坐标轴是基于车辆坐标系，即x轴向前，y轴向左，z轴向上。

    

  • 旋转矩阵
    一个坐标系中的坐标在另一个坐标系中表示的转换关系

  • 四元数
    (w,x,y,z)，参考姿态的不同表示

---

空间坐标变换

用角度变换关系(rotation)可以表示在参考坐标系下物体的姿态，除了角度变化之外，还有一种是参考系原点的相对位置关系，一般用平移矩阵(translation)来表示
关于四元数、旋转矩阵、平移矩阵、复合变换矩阵的定义及关系，convert_nuScenes.py 文件代码详解中的三、main 函数部分有详细介绍，便于连贯阅读，下面直接引用了上述内容

平移矩阵

已知空间中的一个点$P_0(x_0,y_0,z_0)$, 然后将$P_0$沿着X轴、Y轴、Z轴平移$t_x、t_y、t_z$的距离，得到点$P_1(x_1,y_1,z_1)$，那么此过程可以用平移矩阵表示为$P1=TranslationMatrix\cdot P0$, 即$P_0$左乘一个平移矩阵得到$P_1$
$$\left[\begin{array}{c}{\mathrm{x}}_1\\ {\mathrm{y}}_1\\ {\mathrm{z}}_1\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathrm{x}}_0+{\mathrm{t}}_{\mathrm{x}}\\ {\mathrm{y}}_0+{\mathrm{t}}_{\mathrm{y}}\\ {\mathrm{z}}_0+{\mathrm{t}}_{\mathrm{z}}\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{t}}_{\mathrm{x}}\\ 0& 1& 0& {\mathrm{t}}_{\mathrm{y}}\\ 0& 0& 1& {\mathrm{t}}_{\mathrm{z}}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{x}}_0\\ {\mathrm{y}}_0\\ {\mathrm{z}}_0\\ 1\end{array}\right]$$

其中平移矩阵定义为
$$TranslationMatrix=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{t}}_{\mathrm{x}}\\ 0& 1& 0& {\mathrm{t}}_{\mathrm{y}}\\ 0& 0& 1& {\mathrm{t}}_{\mathrm{z}}\\ 0& 0& 0& 1\end{array}\right]$$

旋转矩阵

四元素、旋转矩阵都是用来描述刚体的姿态信息，只不过表达形式不同，它们之间可以通过数学公式进行换算，而这里就需要把四元素转换成旋转矩阵，因为后面需要使用旋转矩阵进行相应的计算
已知四元数
$$Quaternion=(w,x,y,z)$$
则可以转换为如下旋转矩阵
$$RotationMatrix=\left[\begin{array}{ccc}{w}^2+x^2-y^2-z^2& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& w^2-x^2+y^2-z^2& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& w^2-x^2-y^2+z^2\end{array}\right]$$

已知空间中的一个点$P_0(x_0,y_0,z_0)$，然后将$P_0$绕着X轴、Y轴、Z轴旋转不同的角度时(假设旋转关系用上述四元组表示)，得到点$P_1(x_1,y_1,z_1)$， 那么此过程可以用转化成用旋转矩阵表示为$P1=\left[\begin{array}{cc}RotationMatrix& 0\\ 0& 1\end{array}\right]\cdot P0$，即$P_0$左乘一个旋转矩阵得到$P_1$，即
$$P1=\left[\begin{array}{c}{\mathrm{x}}_1\\ {\mathrm{y}}_1\\ {\mathrm{z}}_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{w}}^2+{\mathrm{x}}^2-{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{xy}-\mathrm{wz})& 2(\mathrm{xz}+\mathrm{wy})& 0\\ 2(\mathrm{xy}+\mathrm{wz})& {\mathrm{w}}^2-{\mathrm{x}}^2+{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{yz}-\mathrm{wx})& 0\\ 2(\mathrm{xz}-\mathrm{wy})& 2(\mathrm{yz}+\mathrm{wx})& {\mathrm{w}}^2-{\mathrm{x}}^2-{\mathrm{y}}^2+{\mathrm{z}}^2& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{x}}_0\\ {\mathrm{y}}_0\\ {\mathrm{z}}_0\\ 1\end{array}\right]$$

复合变换矩阵

已知空间中的一个点$P_0(x_0,y_0,z_0)$，然后将$P_0$绕着X轴、Y轴、Z轴旋转不同的角度时(假设旋转关系用上述四元组表示)，得到点$P_1(x_1,y_1,z_1)$，再将点$P_1$分别沿着X轴、Y轴、Z轴平移$t_x、t_y、t_z$的距离，得到点$P_2(x_2,y_2,z_2)$，则变换过程为
$$P2=\left[\begin{array}{c}{\mathrm{x}}_2\\ {\mathrm{y}}_2\\ {\mathrm{z}}_2\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{t}}_{\mathrm{x}}\\ 0& 1& 0& {\mathrm{t}}_{\mathrm{y}}\\ 0& 0& 1& {\mathrm{t}}_{\mathrm{z}}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{w}}^2+{\mathrm{x}}^2-{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{xy}-\mathrm{wz})& 2(\mathrm{xz}+\mathrm{wy})& 0\\ 2(\mathrm{xy}+\mathrm{wz})& {\mathrm{w}}^2-{\mathrm{x}}^2+{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{yz}-\mathrm{wx})& 0\\ 2(\mathrm{xz}-\mathrm{wy})& 2(\mathrm{yz}+\mathrm{wx})& {\mathrm{w}}^2-{\mathrm{x}}^2-{\mathrm{y}}^2+{\mathrm{z}}^2& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{x}}_0\\ {\mathrm{y}}_0\\ {\mathrm{z}}_0\\ 1\end{array}\right]$$
即$P_0$左乘一个旋转矩阵，再左乘一个平移矩阵，从而得到$P_2$，这个过程可以用复合变换矩阵表示为

$$TransformMatrix=TranslationMatrix\cdot RotationMatrix=\left[\begin{array}{cccc}{\mathrm{w}}^2+{\mathrm{x}}^2-{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{xy}-\mathrm{wz})& 2(\mathrm{xz}+\mathrm{wy})& {\mathrm{t}}_{\mathrm{x}}\\ 2(\mathrm{xy}+\mathrm{wz})& {\mathrm{w}}^2-{\mathrm{x}}^2+{\mathrm{y}}^2-{\mathrm{z}}^2& 2(\mathrm{yz}-\mathrm{wx})& {\mathrm{t}}_{\mathrm{y}}\\ 2(\mathrm{xz}-\mathrm{wy})& 2(\mathrm{yz}+\mathrm{wx})& {\mathrm{w}}^2-{\mathrm{x}}^2-{\mathrm{y}}^2+{\mathrm{z}}^2& {\mathrm{t}}_{\mathrm{z}}\\ 0& 0& 0& 1\end{array}\right]$$
即先旋转，再平移

---

坐标系

在自动驾驶的感知系统中，除了自车坐标系外，3D目标检测还涉及相机、激光雷达等传感器，每个传感器都有自己的坐标系。如下图

• lidar坐标系
  x轴向前，y轴向左，z轴向上。重力轴为z轴，参考方向为x轴正方向，航向角rz是lidar坐标系下目标的前进方向和x轴的夹角

• camera坐标系
  z轴向前，y轴向下，x轴向右。重力轴为y轴，参考方向为x轴正方向， 航向角ry是相机坐标下目标的前进方向和x轴的夹角

---

偏航角、观测角、目标方位角的关系

如下图，蓝色为ego vehicle，绿色为目标物体，相机坐标系的z轴向前.

• 方位角theta，定义为自车与目标物体连线偏离自车前进方向的角度

• 航向角rotation_y，即目标方向和相机X轴正方向的夹角（顺时针方向为正），描述的是目标在现实世界中的朝向。如图$\angle BOC$所示。rotaiton_y的取值范围$[-\pi ,\pi ]$，不随目标位置的变化而变化，

• 观测角alpha， 描述的是目标相对于相机视角的朝向，定义为以相机原点为中心，相机原点到物体中心的连线为半径，将目标旋绕重力轴旋转到目标前进方向与ego vechicle一样时所需的角度，如图$\angle BOD$所示。观测角alpha取值范围为$[-\pi ,\pi ]$，随目标位置变化而变化

  

Rotation_y和Alpha之间可以相互转换。因为$\angle \mathrm{AOC}=90^{\circ }-\text{ theta }$，所以有
$$\angle \mathrm{AOB}=\angle \mathrm{AOC}-\angle \mathrm{BOC}=90^{\circ }-\text{ theta }-\text{ rotaion\_y }$$
又因为$\angle \mathrm{AOB}+\angle \mathrm{BOD}=90^{\circ }$, 可得
$$\text{ alpha }=\angle \mathrm{BOD}=90^{\circ }-\angle \mathrm{AOB}=\text{ theta }+\text{ rotation\_y }$$
考虑到rotation_y和alpha都是逆时针方向为负，所以有
$$-\text{ alpha }=\text{ theta }-\text{ rotation\_y }$$
即
$$\text{ alpha }=\text{ rotation\_y }-\text{ theta }$$

---

数据集

NuScenes

点云数据原始格式

NuScenes数据中存在lidar点云和radar点云，其中lidar为稠密点云，具有三维坐标；而radar为稀疏点云，一般的FMCW毫米波雷达不存在高度信息（4D radar可以探测高度）。lidar点云数据的研究相对较多，这里不做过多介绍。

• radar点云格式
  radar的点云数据格式如下图所示，每一行为一个点，包含了点的坐标、速度、强度等信息。点云数据一般通过.pcd文件保存，包头等信息为明文，具体的点云数据为二进制数据

  

• 可视化效果

  从不同的角度观察同一个pcd文件, 普通的毫米波雷达无法测量高度信息，因此右图中Z轴取值一样

  

• 验证高度数据

  取值均为0

  

坐标转换

Nuscenes数据集分为全局坐标系、车身坐标系、camera坐标系、pixel坐标系、radar坐标系、lidar坐标系。除了全局坐标系，均为相对坐标系，标注真值基于全局坐标系。

基本原则

算法拟合的数据大多是基于出传感器坐标系，标注的真值需要从全局坐标系转换到各自的传感器坐标系，所有转换必须先通过传感器外参转到车身坐标系，再转换到传感器坐标系

如下，将全局坐标系下的标注box转化到传感器坐标系

 # Move box to ego vehicle coord system.
 box.translate(-np.array(pose_record['translation']))
 box.rotate(Quaternion(pose_record['rotation']).inverse)

 #  Move box to sensor coord system.
 box.translate(-np.array(cs_record['translation']))
 box.rotate(Quaternion(cs_record['rotation']).inverse)

两个坐标系之间如何转换

通过两个坐标系下的坐标转换矩阵（平移矩阵+旋转矩阵）来转换，相机需要额外通过内参转换到pixel坐标系

def transform_matrix(translation: np.ndarray = np.array([0, 0, 0]),
                    rotation: Quaternion = Quaternion([1, 0, 0, 0]),
                    inverse: bool = False) -> np.ndarray:
    """
    Convert pose to transformation matrix.
    :param translation: <np.float32: 3>. Translation in x, y, z.
    :param rotation: Rotation in quaternions (w ri rj rk).
    :param inverse: Whether to compute inverse transform matrix.
    :return: <np.float32: 4, 4>. Transformation matrix.
    """
    tm = np.eye(4)

    if inverse:

        rot_inv = rotation.rotation_matrix.T
        trans = np.transpose(-np.array(translation))
        tm[:3, :3] = rot_inv
        tm[:3, 3] = rot_inv.dot(trans)
    else:
        # rotation.rotation_matrix 表示将四元素 rotation 转换为旋转矩阵
        # 并将旋转矩阵赋值给对角矩阵 tm 的前三行前三列
        tm[:3, :3] = rotation.rotation_matrix
        # 将矩阵 tm 的前三行第四列赋值为 translation
        tm[:3, 3] = np.transpose(np.array(translation))
    # 最后返回一个 4×4 的复合变换矩阵
    return tm

以上返回的变换矩阵为复合变换矩阵, 包含位置(translation)和旋转信息(rotation)两部分, 关于四元数、旋转矩阵、平移矩阵、复合变换矩阵的定义及关系，convert_nuScenes.py 文件代码详解中的三、main 函数部分有详细介绍

多传感器融合时的坐标转换

不同传感器采集频率不同，不是同步触发，一个传感器的数据需要投影到全局坐标系下，经过全局坐标系再投影到另一个传感器下达到时间对齐。借助于全局坐标系（绝对坐标系）进行运动补偿，从而完成了不同传感器之间的时间对齐。

• radar外参：radar坐标系到ego(车身)坐标系的复合变换矩阵,
• camera外参: camera坐标系到ego坐标系的复合变换矩阵
• ego_pose: ego坐标系到全局坐标系的复合变换矩阵

最后的通过矩阵乘法将点云投影到当前帧的坐标系下，从而完成时间对齐

# Fuse four transformation matrices into one and perform transform.
trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])
velocity_trans_matrix = reduce(np.dot, [ref_from_car_rot, car_from_global_rot, global_from_car_rot, car_from_current_rot])
current_pc.transform(trans_matrix)

数据预处理

Kitti

---

REFERENCES

• 3D目标检测
• 点云数据
• 基于CenterTrack的3D目标检测源码解读
• 干货整理：欧拉角、旋转矩阵、四元数合辑
• 欧拉角pitch、yaw，roll的理解
• 旋转矩阵及左右乘的意义
• KITTI 3D目标检测数据集解析
• Nuscenes数据集中radar到image投影的源码解析
• NuScenes 3D目标检测数据集解析
• convert_nuScenes.py 文件代码详解