（七）可视化KITTI标注的3D BoundingBox

一、KITTI 标注3DBouding Box文件格式

        在以下网址下载已经标注好的文件，里面有20个场景的标注文件。寻找与自己对应场景的文件。比如我对应的为文档里的0004.txt 。
　　可以从https://blog.csdn.net/qq_29931083/article/details/106460698 这里下载
　　
        通过Tracking里的README可以看到文本记录的格式https://blog.csdn.net/qq_29931083/article/details/106504415

---

Values      Name      Description

1      frame      Frame within the sequence where the object appearers
1     track id      Unique tracking id of this object within this sequence
1      type      Describes the type of object: ‘Car’, ‘Van’, ‘Truck’, ‘Pedestrian’, ‘Person_sitting’, ‘Cyclist’, ‘Tram’, ‘Misc’ or ‘DontCare’
1      truncated      Integer (0,1,2) indicating the level of truncation.Note that this is in contrast to the object detection benchmark where truncation is a float in [0,1].
1     occluded     Integer (0,1,2,3) indicating occlusion state:
          0 = fully visible, 1 = partly occluded
          2 = largely occluded, 3 = unknown
1     alpha      Observation angle of object, ranging [-pi…pi]
4      bbox     2D bounding box of object in the image (0-based index):
            contains left, top, right, bottom pixel coordinates
3    dimensions    3D object dimensions: height, width, length (in meters)
3    location     3D object location x,y,z in camera coordinates (in meters)
1    rotation_y     Rotation ry around Y-axis in camera coordinates [-pi…pi]
1     score     Only for results: Float, indicating confidence in detection, needed for p/r curves, higher is better.

---

        使用jupyter notebook进行调试，将文件导入查看文件内容：

import pandas as pd

Columns_name = ['frame','track_id','type','truncated','occluded','alpha','bbox_lift','bbox_top','bbox_right','bbox_bottom','height','width','length','pos_x','pos_y','pos_z','rot_y']
df = pd.read_csv('/home/liqi/dev/catkin_ws/src/KITTI_tutorials/2011_09_26_drive_0014_sync/training/label_02/0004.txt',header = None,sep = ' ')

df.columns = Columns_name

df.head()

        其中包括了每一帧的帧序号frame；每一个物体特有的id track_id；3D 检测框记录的数据：height，width，bbox_right，length，pos_x，pos_y，pos_z，rot_y；

• 其中’height’,‘width’,'length’为3D Bouding Box 分别的高 宽 长；
• 其中’pos_x’,‘pos_y’,'pos_z’代表3D Bouding Box 下底面中心点的坐标；
• 其中’rot_y’代表3D Bouding Box 绕着yaw轴旋转角度，可以计算旋转矩阵R。yaw即笛卡尔坐标系下绕y轴旋转的角度。具体公式如下：
• 注：此使用的yaw=rot_y 而不是IMU中的yaw
  
  3D旋转矩阵的推倒可以通过此处查看https://blog.csdn.net/mightbxg/article/details/79363699

二、3D Bouding Box坐标求取

3DBoundingBox的资料参考此处：
https://github.com/navoshta/KITTI-Dataset/blob/master/kitti-dataset.ipynb

---

        The reference point for the 3D bounding box for each object is centered on the
bottom face of the box. The corners of bounding box are computed as follows with
respect to the reference point and in the object coordinate system:
x_corners = [l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]^T
y_corners = [0, 0, 0, 0, -h, -h, -h, -h ]^T
z_corners = [w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2 ]^T
with l=length, h=height, and w=width.

---

        在KITTI的数据集的Tracking文件中，其中与激光雷达即3D检测相关的参数有七个，使用这七个参数通过以上公式可以计算出3DBoundingBox的8个顶点的x，y，z。再根据rot_y进行旋转即可得到3D Bounding Box在点云数据上的具体位置；但需要注意的是，这是在Camera2 坐标系下的数据，因此需要进行坐标转换，将其转换到Voledyne Lider坐标系下。

        将3D Bouding Box 的8个顶点坐标与旋转矩阵相乘，即可得到cam2坐标系下，旋转变换后的点坐标，子函数代码如下：

# cam2坐标下的 3D box
def compute_3D_box_cam2(h,w,l,x,y,z,yaw):
'''
Return:3Xn in cam2 coordinate
'''
# 建立旋转矩阵R
R = np.array([[np.cos(yaw),