注意：
- 再学习本系列教程时，应该已经安装过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。
-
首先根据Kitti提供的设备安装图，可以看到
Cam2的坐标系，如下所示：
-
由此，我们可以大概画出最理想的模型初步计算顶点坐标，也就是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轴转动，所以旋转矩阵为：
\left[
\begin{matrix}
cos(yaw) & 0 & sin(yaw) \
0 & 1 & 0 \
sin(yaw) & 0 & cos(yaw)
\end{matrix} \right]\tag{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
评论（1）
您还未登录，请登录后发表或查看评论