[开发工具]ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]

注意：

• 再学习本系列教程时，应该已经安装过ROS了并且需要有一些ROS的基本知识

ubuntu版本：20.04
ros版本：noetic

课程回顾

ROS1结合自动驾驶数据集Kitti开发教程(一)Kitti资料介绍和可视化
ROS1结合自动驾驶数据集Kitti开发教程(二)发布图片
ROS1结合自动驾驶数据集Kitti开发教程(三)发布点云数据
ROS1结合自动驾驶数据集Kitti开发教程(四)画出自己车子模型以及照相机视野
ROS1结合自动驾驶数据集Kitti开发教程(五)发布IMU数据
ROS1结合自动驾驶数据集Kitti开发教程(六)发布GPS数据
ROS1结合自动驾驶数据集Kitti开发教程(七)下载图像标注资料并读取显示
ROS1结合自动驾驶数据集Kitti开发教程(八)画出2D检测框

1.前期准备工作

打开Jupyter Notebook进入源码地址，并且创建新的源码文件，名为plot3D.ipynb
激光雷达数据的可视化开源网站
旋转矩阵基础知识

2.画出其中一个物体的3D检测框

至今，已经有先驱者开源了这个项目Visualizing lidar data，展示效果非常不错，通过笔者的教程，也能实现如下图所示的效果。

首先在项目页面下找到如下图的链接进，可以看到原作者已经提供了一些关键的代码，我们通过在其源代码的更改来实现第一步的3D检测框显示（以下程序从头至尾可以合成一段整的函数，直接放进Jupyter Notebook中去跑）：

2.1加载点云数据函数

这段程序和之前的程序一样，不在赘述了。

import numpy as np
import os
BASE_PATH = "/home/mckros/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync/"  
def read_point_cloud():
    point_cloud = np.fromfile(os.path.join(BASE_PATH, "velodyne_points/data/%010d.bin"%0), dtype=np.float32).reshape(-1,4)
    return point_cloud

2.2点云绘制函数

下面这段程序提供了一个点云绘制函数，和原作者程序有些许不同，这边是根据笔者需求来做了一些更改。
函数的主要用法参数介绍如下所示：
ax：matplotlib画板
points：read_point_cloud()读取的点云数据
title：画板标题
axes：[0,1,2]分别代表xyz三个轴
point_size：点云大小
xlim3d：X轴在3D显示下限制角度的范围
ylim3d：Y轴在3D显示下限制角度的范围
zlim3d：Z轴在3D显示下限制角度的范围

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def draw_point_cloud(ax, points, title, axes=[0, 1, 2], point_size=0.2, xlim3d=None, ylim3d=None, zlim3d=None):
    """
    Convenient method for drawing various point cloud projections as a part of frame statistics.
    """
    # 设置xyz三个轴的点云范围
    axes_limits = [
        [-20, 80], # X axis range
        [-20, 20], # Y axis range
        [-3, 5]    # Z axis range
    ]
    axes_str = ['X', 'Y', 'Z']
    # 禁止显示背后的网格
    ax.grid(False)
    # 创建散点图[1]:xyz数据集，[2]:点云的大小，[3]:点云的反射率数据,[4]:为灰度显示
    ax.scatter(*np.transpose(points[:, axes]), s=point_size, c=points[:, 3], cmap='gray')
    # 设置画板的标题
    ax.set_title(title)
    # 设置x轴标题
    ax.set_xlabel('{} axis'.format(axes_str[axes[0]]))
    # 设置y轴标题
    ax.set_ylabel('{} axis'.format(axes_str[axes[1]]))
    if len(axes) > 2:
        # 设置限制角度
        ax.set_xlim3d(*axes_limits[axes[0]])
        ax.set_ylim3d(*axes_limits[axes[1]])
        ax.set_zlim3d(*axes_limits[axes[2]])
        # 将背景颜色设置为RGBA格式，目前的参数以透明显示
        ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
        ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
        ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 0.0))
        # 设置z轴标题
        ax.set_zlabel('{} axis'.format(axes_str[axes[2]]))
    else:
        # 2D限制角度，只有xy轴
        ax.set_xlim(*axes_limits[axes[0]])
        ax.set_ylim(*axes_limits[axes[1]])
    # User specified limits
    if xlim3d!=None:
        ax.set_xlim3d(xlim3d)
    if ylim3d!=None:
        ax.set_ylim3d(ylim3d)
    if zlim3d!=None:
        ax.set_zlim3d(zlim3d)

2.3显示点云

在Jupyter Notebook中输入以下程序，可以显示最基础的3D点云，想要获得好看的点云显示需要通过自己调参优化哦。。

# 获取数据集中的点云数据
point_cloud = read_point_cloud()
# 绘制3D点云数据，创建一个大小为20*10的图形画板
fig = plt.figure(figsize=(20, 10))
# 在画板中添加1*1的网格的第一个子图，为3D图像
ax = fig.add_subplot(111, projection='3d')
# 改变绘制图像的视角，即相机的位置，elev为Z轴角度，azim为(x,y)角度
ax.view_init(60,130)
# 在画板中画出点云显示数据，point_cloud[::x]x值越大，显示的点越稀疏
draw_point_cloud(ax, point_cloud[::5], "velo_points")

效果如下所示：

想要获得2D点云也是非常的简单，代码如下所示：

# 同样是创建画布，只不过这个函数一步到位了
fig, ax = plt.subplots(figsize=(20,10))
# 绘制2D【x,y】轴方向的点云
draw_point_cloud(ax, point_cloud[::5], "velo_points", axes=[0, 1])

效果如下所示：

2.4获取label数据

我们需要获取label_02数据，其中bbox_left，bbox_top，bbox_right，bbox_bottom这四个参数是上节教程讲到的2D检测框的四个顶点，这次咱们要使用到的是dimensions_height，dimensions_width，dimensions_length，location_x，location_y，location_z，rotation_y这7个参数，用于计算3D检测框的8个顶点坐标。

因为这些代码在之前都分析过，今天就不在赘述了。

import pandas as pd 
LABEL_NAME = ["frame", "track id", "type", "truncated", "occluded", "alpha", "bbox_left", "bbox_top", "bbox_right", "bbox_bottom", "dimensions_height", "dimensions_width", "dimensions_length", "location_x", "location_y", "location_z", "rotation_y"] 
df = pd.read_csv(os.path.join(BASE_PATH, "label_02/0000.txt"), header=None, sep=" ")
df.columns = LABEL_NAME
df.loc[df.type.isin(['Van','Car','Truck']),'type'] = 'Car'
df = df[df.type.isin(['Car','Cyclist','Pedestrian'])]
df

输出结果如下所示：

2.5计算3D检测框的8个顶点坐标

在Kitti数据集中，dimensions_height，dimensions_width，dimensions_length，location_x，location_y，location_z，rotation_y这7个参数是以Cam2作为基准坐标系所标注的。通过以下笔者的分析，可以先计算出Cam2眼中的3DBox。

1. 首先根据Kitti提供的设备安装图，可以看到Cam2的坐标系，如下所示：
   

2. 由此，我们可以大概画出最理想的模型初步计算顶点坐标，也就是yaw=0，如下所示。
   

   可以看到按照Cam2的坐标系画出物体的上帝视角模型，因为知道了dimensions_height，dimensions_width，dimensions_length，所以也就轻松的画出了物体的长和宽，location_x，location_y，location_z代表着物体模型中心点的坐标，随之我们也能轻松的得到了8个顶点坐标，如下公式所示：

$$(x\pm \frac{w}{2},y,z\pm \frac{l}{2})$$

可以将其替换为相对于Location坐标系的差值，如下所示：
$$xcorners=(\frac{l}{2},\frac{l}{2},?\frac{l}{2},?\frac{l}{2},\frac{l}{2},\frac{l}{2},?\frac{l}{2},?\frac{l}{2})$$

$$ycorners=(0,0,0,0,?h,?h,?h,?h)$$

$$zcorners=(\frac{w}{2},?\frac{w}{2},?\frac{w}{2},\frac{w}{2},\frac{w}{2},?\frac{w}{2},?\frac{w}{2},\frac{w}{2})$$
但是这只是在yaw=0的情况下得到的坐标，一般正常情况下物体不会正常摆放，都会存在一个旋转角，在kitti数据集中也提供了rotation_y作为物体绕y轴的旋转角，类似下图所示：

这时，需要通过旋转矩阵将yaw=0时的坐标通过点乘转换为yaw!=0时的坐标，由于是绕着Y轴转动，所以旋转矩阵为：
$$?\begin{array}{ccc}cos(yaw)& 0& sin(yaw)\\ 0& 1& 0\\ sin(yaw)& 0& cos(yaw)\end{array}?\text{\hspace{1em}(2)}$$
代码如下所示：

def compute_3d_box_cam2(h, w, l, x, y, z, yaw):
    # 计算旋转矩阵
    R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
    # 8个顶点的xyz
    x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
    y_corners = [0,0,0,0,-h,-h,-h,-h] 
    z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
    # 旋转矩阵点乘(3，8)顶点矩阵
    corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
    # 加上location中心点，得出8个顶点旋转后的坐标
    corners_3d_cam2 += np.vstack([x,y,z])
    return corners_3d_cam2
# 计算cam2眼中的3DBox
corners_3d_cam2 = compute_3d_box_cam2(*df.loc[2,['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])

2.6绘制物体最初的样子

def draw_box(ax, vertices, axes=[0, 1, 2], color='black'):
    """
    Draws a bounding 3D box in a pyplot axis.
    
    Parameters
    ----------
    pyplot_axis : Pyplot axis to draw in.
    vertices    : Array 8 box vertices containing x, y, z coordinates.
    axes        : Axes to use. Defaults to `[0, 1, 2]`, e.g. x, y and z axes.
    color       : Drawing color. Defaults to `black`.
    """
    vertices = vertices[axes, :]
    connections = [
        [0, 1], [1, 2], [2, 3], [3, 0],  # Lower plane parallel to Z=0 plane
        [4, 5], [5, 6], [6, 7], [7, 4],  # Upper plane parallel to Z=0 plane
        [0, 4], [1, 5], [2, 6], [3, 7]  # Connections between upper and lower planes
    ]
    for connection in connections:
        ax.plot(*vertices[:, connection], c=color, lw=0.5)

fig = plt.figure(figsize=(20,10))
ax = fig.add_subplot(111, projection='3d')    
ax.view_init(40,150)
draw_box(ax,(corners_3d_cam2))

效果如下：

2.7cam2坐标系转velo坐标系

下载calibration转换程序：kitti_util.py
代码如下所示：

from kitti_util import *
# 读取calibration转换数据
calib = Calibration("/home/mckros/kitti/RawData/2011_09_26/", from_video=True)
# cam2转velo坐标系
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T).T

2.8绘制转换后的物体检测框

fig = plt.figure(figsize=(20,10))
ax = fig.add_subplot(111, projection = '3d')
ax.view_init(40,150)
draw_box(ax,corners_3d_velo)

效果如下所示：

2.9和点云结合在一起绘制检测框

# 和点云做结合
fig = plt.figure(figsize=(20,10))
ax = fig.add_subplot(111, projection = '3d')
ax.view_init(70,230)
draw_point_cloud(ax,point_cloud[::5],"velo pointcloud")
draw_box(ax,corners_3d_velo,color='red')

效果如下所示：

换个上帝视角

fig, ax = plt.subplots(figsize=(20,10))
draw_point_cloud(ax,point_cloud[::5],"velo pointcloud",axes=[0,1])
draw_box(ax,corners_3d_velo,color='red',axes=[0,1])

效果如下所示：

结语

本文也是基于笔者的学习和使用经验总结的，主观性较强，如果有哪些不对的地方或者不明白的地方，欢迎评论区留言交流~

为了能和读者进一步讨论问题，建立了一个微信群，方便给大家解答问题，也可以一起讨论问题。
加群链接
?Bye