【从kitti开始自动驾驶】--7.1 Jupyter-notebook实现3d侦测框显示

本节将采用在jupyter notebook上的方式执行对单点云数据进行打标(画出三维侦测框i)效果工作的情况.

tracking资料存放在/kitti_folder/tracking中
jupyter工程存放在/test3_autodrive_ws/src/jupyter_prj下的3D_tracking.ipynb
数据文件存放在/home/qinsir/kitti_folder/tracking/data_tracking_label_2/training/label_02
下,可以看到是txt文件,选择0000
标定程序的代码在test3_autodrive_ws/src/demo1_kitti_pub_photo/scripts/calibration.py文件夹下

1. 预准备

1.1 绘图参考资料

对于已有数据在点云中绘制3D框已经有很多优秀的案例,这里选择在浏览器中键入visualize kitti lidar 选择"Alex Staravoitau’bolg"如下进行参考,进入点击查看plot3D的绘制的github源码
如下:

阅读资料后发现,我们只需要取绘制点云地图和绘制3D框的函数即可.

1.2 绘图参考程序

1. copy代码中的绘制点云draw_point_cloud函数和绘制检测框函数draw_box.
2. 更改部分参数,变量,直接将原程序中的变量定义更改为该函数中的临时变量来适应我们的代码

1.3 相机->雷达坐标变换

点云地图以velodyne为坐标中心,而计算出来的tracking框以camera为原点,必然存在旋转和平移的关系.

• 以线性代数知识,需要T和R矩阵即可,
• 这里的矫正文件已经被我们下载(类似与calib_velo_to_cam)
• 网上变换开源的代码有很多,我们选择frustum pointnet的github源码,另存为我们同路径下的calibtation.py文档

2. 程序流程

2.1 绘制点云

2.1.1 包导入

这里只介绍本节需要用到的,新的包:

• 绘图包
• 3D坐标系
• data_utils文件路径

...
import matplotlib.pyplot as plt  
from mpl_toolkits.mplot3d import Axes3D 
import sys
import os
sys.path.append('../demo1_kitti_pub_photo/scripts')
from data_utils import *

2.1.2 绘制点云函数

已经更改过的draw_point_cloud函数

• 坐标轴范围(velodyne为原点)
  axes_limits = [
  [-20, 80], # X axis range
  [-20, 20], # Y axis range
  [-3, 10] # Z axis range

axes_limits = [
            [-20, 80], # X axis range
            [-20, 20], # Y axis range
            [-3, 10]   # Z axis range

---

• 三坐标系命名
• 禁用栅格

axes_str = ['X', 'Y', 'Z']
        ax.grid(False)  #不要画出栅格

2.1.3 读取点云

从本地TXT文件读取点云数据比较简单

DATA_PATH = '/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync'
points = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%0)) #点云data的 path

2.1.4 3D显示点云

这里调用之前的函数查看的是3D的点云:

• 画布大小和定义
• 分割:1行1列取第一个画布
• 观看角度
• 画出点云,每5个点绘制,较快

fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
draw_point_cloud(ax, points[::5])   #每5个点画一个

2.1.5 观看点云的2D平面投影

函数还为我们提供了2D显示接口:

• axes代表在xoy平面

fig, ax = plt.subplots(figsize=(20, 10))
draw_point_cloud(ax, points[::5], axes=[0, 1])  #只在xoy平面上,俯视图

2.2 绘制Tracking-3D框

2.2.1 绘制长方体框

本函数无需改动,可直接使用:

def draw_box(pyplot_axis, 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:
        pyplot_axis.plot(*vertices[:, connection], c=color, lw=0.5)

---

输入tracking中的数据点,得到一个8个点,分别是点长方体的8个点,并绘制:

• 得到8X3的坐标阵列
• 绘制矩形
• 要注意这里是相机坐标系下得到的,需要转到velodyne下才会显示非常方正的长方体

corners_3d_cam2 = compute_3d_box_cam2(*df_tracking.loc[2, ['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']])

#3是x,y,z,8是一共长方体的8个定点定位
#绘制
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
#得到corners_3d_cam2是在照相机坐标系下得到的,所以会出现如下所示的略微奇怪的长方体
#为了可视化,应该再转为velodyne坐标系下
draw_box(ax, corners_3d_cam2)  

显示如下:

2.2.2 坐标系转换

从2.2.1我们可以看到,坐标系需要经过从cam->velodyne的过程才可以正常显示:

calib = Calibration('/home/qinsir/kitti_folder/2011_09_26', from_video=True)
corners_3d_velo = calib.project_ref_to_velo(corners_3d_cam2.T).T        

corners_3d_velo

• 矫正程序直接用刚才拷贝的Calibration函数
• 矫正数据直接用我们的calib文件解压后的3个文件,分别是velo, cam, IMU三个坐标的相互转换
• 得到准确的相机投影到velo上的坐标
• 转置:为了让之后的draw认识,3X8->8X3,每一列是一个点的坐标

---

2.3 绘制并查看效果

2.3.1 点云并tracking的3D查看:

#绘制
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
#同一坐标系下绘制出即可
draw_point_cloud(ax, points[::5])
draw_box(ax, corners_3d_velo, color='r')

• 把2.2.2的内容引入
• 设置颜色,其他就没什么说的了

效果展示:

---

2.3.2 点云并tracking的2D查看:

这里以俯视图为例:

fig, ax = plt.subplots(figsize=(20, 10))
draw_point_cloud(ax, points[::5], axes=[0, 1])
draw_box(ax, corners_3d_velo, axes=[0, 1], color='r')

• 只需要将axes参数添加,并且同时绘制于一张画布上即可,效果如下:

3. 3D_tracking的python源码

源码已上传至我的博客,3D_tracking.ipynb

不先麻烦,源码如下:

import pandas as pd   #读取csv等
import matplotlib.pyplot as plt  #画出图形资料包
from mpl_toolkits.mplot3d import Axes3D #画3D的轴的资料包
import numpy as np
import sys
import os
sys.path.append('../demo1_kitti_pub_photo/scripts')

from data_utils import *

def draw_point_cloud(ax, title, axes=[0, 1, 2],  point_size=0.1,xlim3d=None, ylim3d=None, zlim3d=None):
        """
        Convenient method for drawing various point cloud projections as a part of frame statistics.
        """
        #范围限制
        axes_limits = [
            [-20, 80], # X axis range
            [-20, 20], # Y axis range
            [-3, 10]   # Z axis range
                    ]
        axes_str = ['X', 'Y', 'Z']
        ax.grid(False)  #不要画出栅格
        
        ax.scatter(*np.transpose(points[:, axes]), s=point_size, c=points[:, 3], cmap='gray')
        ax.set_title(title)
        ax.set_xlabel('{} axis'.format(axes_str[axes[0]]))
        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]])
            ax.set_zlabel('{} axis'.format(axes_str[axes[2]]))
        else:
            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)

DATA_PATH = '/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync'
points = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%0)) #点云data的 path


fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
draw_point_cloud(ax, points[::5])   #每5个点画一个


fig, ax = plt.subplots(figsize=(20, 10))
draw_point_cloud(ax, points[::5], axes=[0, 1])  #只在xoy平面上,俯视图

df_tracking = read_tracking('/home/qinsir/kitti_folder/tracking/data_tracking_label_2/training/label_02/0000.txt')
df_tracking.head()
# 3D侦测框需要后边height, width, len, pos_x, y, z(相机坐标系里的坐标), rot_y(从上往下看旋转的角度)

# 根据长宽高XYZ和旋转角坐标定位并画出3D框
def compute_3d_box_cam2(h, w, l, x, y, z, yaw):
    '''
    return : 3xn in cam2 coordinate
    '''
    
    R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
    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]
    
    corners_3d_cam2 = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))
    corners_3d_cam2 += np.vstack([x, y, z])
    
    return corners_3d_cam2

def draw_box(pyplot_axis, 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:
        pyplot_axis.plot(*vertices[:, connection], c=color, lw=0.5)

#先预览一个框, 得到一个3D侦测框
corners_3d_cam2 = compute_3d_box_cam2(*df_tracking.loc[2, ['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']])

#3是x,y,z,8是一共长方体的8个定点定位
corners_3d_cam2.shape


#绘制
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
#得到corners_3d_cam2是在照相机坐标系下得到的,所以会出现如下所示的略微奇怪的长方体
#为了可视化,应该再转为velodyne坐标系下
draw_box(ax, corners_3d_cam2)   


from calibration import *

calib = Calibration('/home/qinsir/kitti_folder/2011_09_26', from_video=True)
#经过矫正到velodyne的函数,入口参数为从cam到velodyne
#需要8X3,转置一下
corners_3d_velo = calib.project_ref_to_velo(corners_3d_cam2.T).T        

corners_3d_velo
#转换完之后,每一列就是它的一个的点的坐标

#绘制
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
#已经转为velodyne坐标系下,可以正常显示
draw_box(ax, corners_3d_velo)   


#绘制
fig = plt.figure(figsize=(20, 10))
ax = fig.add_subplot(111,projection='3d')
ax.view_init(40, 150)   #观看角度
#同一坐标系下绘制出即可
draw_point_cloud(ax, points[::5])
draw_box(ax, corners_3d_velo, color='r')

fig, ax = plt.subplots(figsize=(20, 10))
draw_point_cloud(ax, points[::5], axes=[0, 1])
draw_box(ax, corners_3d_velo, axes=[0, 1], color='r')