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,展示效果非常不错,通过笔者的教程,也能实现如下图所示的效果。
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第1张图片

首先在项目页面下找到如下图的链接进,可以看到原作者已经提供了一些关键的代码,我们通过在其源代码的更改来实现第一步的3D检测框显示(以下程序从头至尾可以合成一段整的函数,直接放进Jupyter Notebook中去跑):
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第2张图片

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")

效果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第3张图片

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

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

效果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第4张图片

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

输出结果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第5张图片

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的坐标系,如下所示:
    ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第6张图片

  2. 由此,我们可以大概画出最理想的模型初步计算顶点坐标,也就是yaw=0,如下所示。
    ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第7张图片

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

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

可以将其替换为相对于Location坐标系的差值,如下所示:
x c o r n e r s = ( l 2 , l 2 , − l 2 , − l 2 , l 2 , l 2 , − l 2 , − l 2 ) 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}) xcorners=(2l,2l,2l,2l,2l,2l,2l,2l)

y c o r n e r s = ( 0 , 0 , 0 , 0 , − h , − h , − h , − h ) ycorners = (0,0,0,0,-h,-h,-h,-h) ycorners=(0,0,0,0,h,h,h,h)

z c o r n e r s = ( w 2 , − w 2 , − w 2 , w 2 , w 2 , − w 2 , − w 2 , w 2 ) 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}) zcorners=(2w,2w,2w,2w,2w,2w,2w,2w)
但是这只是在yaw=0的情况下得到的坐标,一般正常情况下物体不会正常摆放,都会存在一个旋转角,在kitti数据集中也提供了rotation_y作为物体绕y轴的旋转角,类似下图所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第8张图片

这时,需要通过旋转矩阵将yaw=0时的坐标通过点乘转换为yaw!=0时的坐标,由于是绕着Y轴转动,所以旋转矩阵为:
[ c o s ( y a w ) 0 s i n ( y a w ) 0 1 0 s i n ( y a w ) 0 c o s ( y a w ) ] (2) \left[ \begin{matrix} cos(yaw) & 0 & sin(yaw) \\ 0 & 1 & 0 \\ sin(yaw) & 0 & cos(yaw) \end{matrix} \right]\tag{2} cos(yaw)0sin(yaw)010sin(yaw)0cos(yaw)(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))

效果如下:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第9张图片

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)

效果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第10张图片

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')

效果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第11张图片

换个上帝视角

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])

效果如下所示:
ROS1结合自动驾驶数据集Kitti开发教程(九)画出3D检测框[1]_第12张图片

结语

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

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

你可能感兴趣的:(▶自动驾驶专栏,▶机器人操作系统ROS)