devbox
代码创新者
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

3D目标检测数据集 KITTI（标签格式解析、3D框可视化、点云转图像、BEV鸟瞰图）

作者：代码创新者 | 2023-12-19 19:03:16

赞

踩

kitti

本文介绍在3D目标检测中，理解和使用KITTI 数据集，包括KITTI 的基本情况、下载数据集、标签格式解析、3D框可视化、点云转图像、画BEV鸟瞰图等，并配有实现代码。

目录

 1、KITTI数据集3D框可视化

2、KITTI 3D数据集

3、下载数据集

4、标签格式

5、标定参数解析

6、点云数据-->投影到图像

7、图像数据-->投影到点云

8、可视化图像2D结果、3D结果

9、点云3D结果-->图像BEV鸟瞰图结果（坐标系转换）

10、绘制BEV鸟瞰图

11、BEV鸟瞰图画2d框

12、完整工程代码

 1、KITTI数据集3D框可视化

2、KITTI 3D数据集

kitti 3D数据集的基本情况：

KITTI整个数据集是在德国卡尔斯鲁厄采集的，采集时长6小时。KITTI官网放出的数据大约占采集全部的25%，去除了测试集中相关的数据片段，按场景可以分为“道路”、“城市”、“住宅区”、“校园”和“行人”5类。

传感器配置：

传感器安装位置：


3、下载数据集

The KITTI Vision Benchmark Suite (cvlibs.net)

下载数据需要注册账号的，获取取百度网盘下载；文件的格式如下所示

图片格式：xxx.jpg

点云格式：xxx.bin（点云是以bin二进制的方式存储的）

标定参数：xxx.txt（一个文件中包括各个相机的内参、畸变校正矩阵、激光雷达坐标转到相机坐标的矩阵、IMU坐标转到激光雷达坐标的矩阵）

标签格式：xxx.txt（包含类别、截断情况、遮挡情况、观测角度、2D框左上角坐标、2D框右下角坐标、3D物体的尺寸-高宽长、3D物体的中心坐标-xyz、置信度）

4、标签格式

示例标签：Pedestrian 0.00 0 -0.20 712.40 143.00 810.73 307.92 1.89 0.48 1.20 1.84 1.47 8.41 0.01 

这时可以看看这个视频：

Nuscenes、KITTI等多个BEV开源数据集介绍

5、标定参数解析

然后看一下标定参数：

P0-P3:是各个相机的内参矩阵；3×4的相机投影矩阵，0~3分别对应左侧灰度相机、右侧灰度相机、左侧彩色相机、右侧彩色相机。

R0_rect: 是左相机的畸变矫正矩阵；3×3的旋转修正矩阵。

Tr_velo_to_cam:是激光雷达坐标系 转到 相机坐标系矩阵；3×4的激光坐标系到Cam 0坐标系的变换矩阵。

Tr_imu_to_velo: 是IMU坐标转到激光雷达坐标的矩阵；3×4的IMU坐标系到激光坐标系的变换矩阵。

6、点云数据-->投影到图像

当有了点云数据信息，如何投影到图像中呢？本质上是一个坐标系转换的问题，流程思路如下：

  1. 已知点云坐标（x，y，z），当前是处于激光雷达坐标系
  2. 激光雷达坐标系 转到 相机坐标系，需要用到标定参数中的Tr_velo_to_cam矩阵，此时得到相机坐标（x1，y1，z1）
  3. 相机坐标系进行畸变矫正，需要用到标定参数中的R0_rect矩阵，此时得到相机坐标（x2，y2，z2）
  4. 相机坐标系转为图像坐标系，需要用到标定参数中的P0矩阵，即相机内存矩阵，此时得到图像坐标（u，v）

看一下示例效果：

接口代码：

  1. '''
  2. 将点云数据投影到图像
  3. '''
  4. def show_lidar_on_image(pc_velo, img, calib, img_width, img_height):
  5.     ''' Project LiDAR points to image '''
  6.     imgfov_pc_velo, pts_2d, fov_inds = get_lidar_in_image_fov(pc_velo,
  7.         calib, 0, 0, img_width, img_height, True)
  8.     imgfov_pts_2d = pts_2d[fov_inds,:]
  9.     imgfov_pc_rect = calib.project_velo_to_rect(imgfov_pc_velo)
  10.  
  11.     import matplotlib.pyplot as plt
  12.     cmap = plt.cm.get_cmap('hsv', 256)
  13.     cmap = np.array([cmap(i) for i in range(256)])[:,:3]*255
  14.  
  15.     for i in range(imgfov_pts_2d.shape[0]):
  16.         depth = imgfov_pc_rect[i,2]
  17.         color = cmap[int(640.0/depth),:]
  18.         cv2.circle(img, (int(np.round(imgfov_pts_2d[i,0])),
  19.             int(np.round(imgfov_pts_2d[i,1]))),
  20.             2, color=tuple(color), thickness=-1)
  21.     Image.fromarray(img).save('save_output/lidar_on_image.png')
  22.     Image.fromarray(img).show() 
  23.     return img

核心代码：

  1. '''
  2. 将点云数据投影到相机坐标系
  3. '''
  4. def get_lidar_in_image_fov(pc_velo, calib, xmin, ymin, xmax, ymax,
  5.                            return_more=False, clip_distance=2.0):
  6.     ''' Filter lidar points, keep those in image FOV '''
  7.     pts_2d = calib.project_velo_to_image(pc_velo)
  8.     fov_inds = (pts_2d[:,0]<xmax) & (pts_2d[:,0]>=xmin) & \
  9.         (pts_2d[:,1]<ymax) & (pts_2d[:,1]>=ymin)
  10.     fov_inds = fov_inds & (pc_velo[:,0]>clip_distance)
  11.     imgfov_pc_velo = pc_velo[fov_inds,:]
  12.     if return_more:
  13.         return imgfov_pc_velo, pts_2d, fov_inds
  14.     else:
  15.         return imgfov_pc_velo

7、图像数据-->投影到点云

当有了图像RGB信息，如何投影到点云中呢？本质上是一个坐标系转换的问题，和上面的是逆过程，流程思路如下：

  1. 已知图像坐标（u，v），当前是处于图像坐标系
  2. 图像坐标系 转 相机坐标系，需要用到标定参数中的P0逆矩阵，即相机内存矩阵，得到相机坐标（x，y，z）
  3. 相机坐标系进行畸变矫正，需要用到标定参数中的R0_rect逆矩阵，得到相机坐标（x1，y1，z1）
  4. 矫正后相机坐标系 转 激光雷达坐标系，需要用到标定参数中的Tr_velo_to_cam逆矩阵，此时得到激光雷达坐标（x2，y2，z2）

8、可视化图像2D结果、3D结果

先看一下2D框的效果：

3D框的效果：

 接口代码：

  1. '''
  2. 在图像中画2D框、3D框
  3. '''
  4. def show_image_with_boxes(img, objects, calib, show3d=True):
  5.     img1 = np.copy(img) # for 2d bbox
  6.     img2 = np.copy(img) # for 3d bbox
  7.     for obj in objects:
  8.         if obj.type=='DontCare':continue
  9.         cv2.rectangle(img1, (int(obj.xmin),int(obj.ymin)), (int(obj.xmax),int(obj.ymax)), (0,255,0), 2) # 画2D框
  10.         box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P) # 获取图像3D框(8*2)、相机坐标系3D框(8*3)
  11.         img2 = utils.draw_projected_box3d(img2, box3d_pts_2d) # 在图像上画3D框
  12.     if show3d:
  13.         Image.fromarray(img2).save('save_output/image_with_3Dboxes.png')
  14.         Image.fromarray(img2).show()
  15.     else:
  16.         Image.fromarray(img1).save('save_output/image_with_2Dboxes.png')
  17.         Image.fromarray(img1).show()

核心代码：

  1. def compute_box_3d(obj, P):
  2.     '''
  3.     计算对象的3D边界框在图像平面上的投影
  4.     输入: obj代表一个物体标签信息,  P代表相机的投影矩阵-内参。
  5.     输出: 返回两个值, corners_3d表示3D边界框在 相机坐标系 的8个角点的坐标-3D坐标。
  6.                                      corners_2d表示3D边界框在 图像上 的8个角点的坐标-2D坐标。
  7.     '''
  8.     # 计算一个绕Y轴旋转的旋转矩阵R，用于将3D坐标从世界坐标系转换到相机坐标系。obj.ry是对象的偏航角
  9.     R = roty(obj.ry)    
  10.  
  11.     # 物体实际的长、宽、高
  12.     l = obj.l;
  13.     w = obj.w;
  14.     h = obj.h;
  15.     
  16.     # 存储了3D边界框的8个角点相对于对象中心的坐标。这些坐标定义了3D边界框的形状。
  17.     x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
  18.     y_corners = [0,0,0,0,-h,-h,-h,-h];
  19.     z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
  20.     
  21.     # 1、将3D边界框的角点坐标从对象坐标系转换到相机坐标系。它使用了旋转矩阵R
  22.     corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
  23.     # 3D边界框的坐标进行平移
  24.     corners_3d[0,:] = corners_3d[0,:] + obj.t[0];
  25.     corners_3d[1,:] = corners_3d[1,:] + obj.t[1];
  26.     corners_3d[2,:] = corners_3d[2,:] + obj.t[2];
  27.  
  28.     # 2、检查对象是否在相机前方，因为只有在相机前方的对象才会被绘制。
  29.     # 如果对象的Z坐标（深度）小于0.1，就意味着对象在相机后方，那么corners_2d将被设置为None，函数将返回None。
  30.     if np.any(corners_3d[2,:]<0.1):
  31.         corners_2d = None
  32.         return corners_2d, np.transpose(corners_3d)
  33.     
  34.     # 3、将相机坐标系下的3D边界框的角点，投影到图像平面上，得到它们在图像上的2D坐标。
  35.     corners_2d = project_to_image(np.transpose(corners_3d), P);
  36.     return corners_2d, np.transpose(corners_3d)
  37.  
  38.  
  39. def draw_projected_box3d(image, qs, color=(0,60,255), thickness=2):
  40.     '''
  41.     qs: 包含8个3D边界框角点坐标的数组, 形状为(8, 2)。图像坐标下的3D框, 8个顶点坐标。
  42.     '''
  43.     ''' Draw 3d bounding box in image
  44.         qs: (8,2) array of vertices for the 3d box in following order:
  45.             1 -------- 0
  46.            /|         /|
  47.           2 -------- 3 .
  48.           | |        | |
  49.           . 5 -------- 4
  50.           |/         |/
  51.           6 -------- 7
  52.     '''
  53.     qs = qs.astype(np.int32) # 将输入的顶点坐标转换为整数类型，以便在图像上绘制。
  54.  
  55.     # 这个循环迭代4次，每次处理一个边界框的一条边。
  56.     for k in range(0,4):
  57.        # Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html
  58.  
  59.        # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制边界框的前四条边。
  60.        i,j=k,(k+1)%4
  61.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  62.  
  63.         # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制边界框的后四条边，与前四条边平行
  64.        i,j=k+4,(k+1)%4 + 4
  65.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  66.  
  67.         # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制连接前四条边和后四条边的边界框的边。
  68.        i,j=k,k+4
  69.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  70.     return image

9、点云3D结果-->图像BEV鸟瞰图结果（坐标系转换）

思路流程：

  1. 读取点云数据，点云得存储格式是n*4，n是指当前文件点云的数量，4分别表示（x，y，z，intensity），即点云的空间三维坐标、反射强度
  2. 我们只需读取前两行即可，得到坐标点（x，y）
  3. 然后将坐标点（x，y），画散点图

BEV鸟瞰图效果如下：

10、绘制BEV鸟瞰图

BEV图像示例效果：

核心代码：

  1.  
  2. '''
  3. 可视化BEV鸟瞰图
  4. '''
  5. def show_lidar_topview(pc_velo, objects, calib):
  6.       # 1-设置鸟瞰图范围
  7.     side_range = (-30, 30)  # 左右距离
  8.     fwd_range = (0, 80)  # 后前距离
  9.     
  10.     x_points = pc_velo[:, 0]
  11.     y_points = pc_velo[:, 1]
  12.     z_points = pc_velo[:, 2]
  13.     
  14.     # 2-获得区域内的点
  15.     f_filt = np.logical_and(x_points > fwd_range[0], x_points < fwd_range[1])
  16.     s_filt = np.logical_and(y_points > side_range[0], y_points < side_range[1])
  17.     filter = np.logical_and(f_filt, s_filt)
  18.     indices = np.argwhere(filter).flatten() 
  19.     x_points = x_points[indices]
  20.     y_points = y_points[indices]
  21.     z_points = z_points[indices]
  22.     
  23.     # 定义了鸟瞰图中每个像素代表的距离
  24.     res = 0.1   
  25.     # 3-1将点云坐标系 转到 BEV坐标系
  26.     x_img = (-y_points / res).astype(np.int32)
  27.     y_img = (-x_points / res).astype(np.int32)
  28.     # 3-2调整坐标原点
  29.     x_img -= int(np.floor(side_range[0]) / res)
  30.     y_img += int(np.floor(fwd_range[1]) / res)
  31.     print(x_img.min(), x_img.max(), y_img.min(), y_img.max()) 
  32.     
  33.     # 4-填充像素值, 将点云数据的高度信息（Z坐标）映射到像素值
  34.     height_range = (-3, 1.0)
  35.     pixel_value = np.clip(a=z_points, a_max=height_range[1], a_min=height_range[0])
  36.      
  37.  
  38.     def scale_to_255(a, min, max, dtype=np.uint8):
  39.         return ((a - min) / float(max - min) * 255).astype(dtype)
  40.     
  41.     pixel_value = scale_to_255(pixel_value, height_range[0], height_range[1])
  42.     
  43.     # 创建图像数组
  44.     x_max = 1 + int((side_range[1] - side_range[0]) / res)
  45.     y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
  46.     im = np.zeros([y_max, x_max], dtype=np.uint8)
  47.     im[y_img, x_img] = pixel_value
  48.     
  49.     im2 = Image.fromarray(im)
  50.     im2.save('save_output/BEV.png')
  51.     im2.show()

11、BEV鸟瞰图画2d框

在BEV视图中画框，可视化结果：

接口代码：

  1. '''
  2. 将点云数据3D框投影到BEV
  3. '''
  4. def show_lidar_topview_with_boxes(img, objects, calib):
  5.     def bbox3d(obj):
  6.         box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P) # 获取3D框-图像、3D框-相机坐标系
  7.         box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) # 将相机坐标系的框 转到 激光雷达坐标系
  8.         return box3d_pts_3d_velo # 返回nx3的点
  9.  
  10.     boxes3d = [bbox3d(obj) for obj in objects if obj.type == "Car"]
  11.     gt = np.array(boxes3d)
  12.     im2 = utils.draw_box3d_label_on_bev(img, gt, scores=None, thickness=1) # 获取激光雷达坐标系的3D点，选择x, y两维，画到BEV平面坐标系上
  13.     im2 = Image.fromarray(im2)
  14.     im2.save('save_output/BEV with boxes.png')
  15.     im2.show()

核心代码：

  1. # 设置BEV鸟瞰图参数
  2. side_range = (-30, 30)  # 左右距离
  3. fwd_range = (0, 80)  # 后前距离
  4. res = 0.1  # 分辨率0.05m
  5.  
  6. def compute_box_3d(obj, P):
  7.     '''
  8.     计算对象的3D边界框在图像平面上的投影
  9.     输入: obj代表一个物体标签信息,  P代表相机的投影矩阵-内参。
  10.     输出: 返回两个值, corners_3d表示3D边界框在 相机坐标系 的8个角点的坐标-3D坐标。
  11.                                      corners_2d表示3D边界框在 图像上 的8个角点的坐标-2D坐标。
  12.     '''
  13.     # 计算一个绕Y轴旋转的旋转矩阵R，用于将3D坐标从世界坐标系转换到相机坐标系。obj.ry是对象的偏航角
  14.     R = roty(obj.ry)    
  15.  
  16.     # 物体实际的长、宽、高
  17.     l = obj.l;
  18.     w = obj.w;
  19.     h = obj.h;
  20.     
  21.     # 存储了3D边界框的8个角点相对于对象中心的坐标。这些坐标定义了3D边界框的形状。
  22.     x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
  23.     y_corners = [0,0,0,0,-h,-h,-h,-h];
  24.     z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
  25.     
  26.     # 1、将3D边界框的角点坐标从对象坐标系转换到相机坐标系。它使用了旋转矩阵R
  27.     corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
  28.     # 3D边界框的坐标进行平移
  29.     corners_3d[0,:] = corners_3d[0,:] + obj.t[0];
  30.     corners_3d[1,:] = corners_3d[1,:] + obj.t[1];
  31.     corners_3d[2,:] = corners_3d[2,:] + obj.t[2];
  32.  
  33.     # 2、检查对象是否在相机前方，因为只有在相机前方的对象才会被绘制。
  34.     # 如果对象的Z坐标（深度）小于0.1，就意味着对象在相机后方，那么corners_2d将被设置为None，函数将返回None。
  35.     if np.any(corners_3d[2,:]<0.1):
  36.         corners_2d = None
  37.         return corners_2d, np.transpose(corners_3d)
  38.     
  39.     # 3、将相机坐标系下的3D边界框的角点，投影到图像平面上，得到它们在图像上的2D坐标。
  40.     corners_2d = project_to_image(np.transpose(corners_3d), P);
  41.     return corners_2d, np.transpose(corners_3d)
  42.  
  43.  
  44.

12、完整工程代码

工程目录：

kitti_vis_main.py（主代码入口）

  1.  
  2. from __future__ import print_function
  3.  
  4. import os
  5. import sys
  6. import cv2
  7. import os.path
  8. from PIL import Image
  9. BASE_DIR = os.path.dirname(os.path.abspath(__file__))
  10. ROOT_DIR = os.path.dirname(BASE_DIR)
  11. sys.path.append(BASE_DIR)
  12. sys.path.append(os.path.join(ROOT_DIR, 'mayavi'))
  13. from kitti_object import *
  14.  
  15.  
  16. def visualization():
  17.     import mayavi.mlab as mlab
  18.     dataset = kitti_object(os.path.join(ROOT_DIR, 'Kitti_3D_Vis/dataset/object'))   # linux 路径
  19.     data_idx = 10               # 选择第几张图像
  20.  
  21.     # 1-加载标签数据
  22.     objects = dataset.get_label_objects(data_idx)
  23.     print("There are %d objects.", len(objects))
  24.  
  25.     # 2-加载图像
  26.     img = dataset.get_image(data_idx)
  27.     img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
  28.     img_height, img_width, img_channel = img.shape
  29.  
  30.     # 3-加载点云数据
  31.     pc_velo = dataset.get_lidar(data_idx)[:,0:3] # (x, y, z)
  32.  
  33.     # 4-加载标定参数
  34.     calib = dataset.get_calibration(data_idx)
  35.  
  36.     # 5-可视化原始图像
  37.     print(' ------------ show raw image -------- ')
  38.     Image.fromarray(img).show()
  39.     
  40.     # 6-在图像中画2D框
  41.     print(' ------------ show image with 2D bounding box -------- ')
  42.     show_image_with_boxes(img, objects, calib, False)
  43.  
  44.     # 7-在图像中画3D框
  45.     print(' ------------ show image with 3D bounding box ------- ')
  46.     show_image_with_boxes(img, objects, calib, True)
  47.     
  48.     # 8-将点云数据投影到图像
  49.     print(' ----------- LiDAR points projected to image plane -- ')
  50.     show_lidar_on_image(pc_velo, img, calib, img_width, img_height)
  51.  
  52.     # 9-画BEV图
  53.     print('------------------ BEV of LiDAR points -----------------------------')
  54.     show_lidar_topview(pc_velo, objects, calib)
  55.  
  56.      # 10-在BEV图中画2D框
  57.     print('--------------- BEV of LiDAR points with bobes ---------------------')
  58.     img1 = cv2.imread('save_output/BEV.png')     
  59.     img = cv2.cvtColor(img1, cv2.COLOR_BGR2RGB)
  60.     show_lidar_topview_with_boxes(img1, objects, calib)
  61.     
  62.     
  63. if __name__=='__main__':
  64.     visualization()

kitti_util.py

  1. from __future__ import print_function
  2.  
  3. import numpy as np
  4. import cv2
  5. from PIL import Image
  6. import os
  7.  
  8. # 设置BEV鸟瞰图参数
  9. side_range = (-30, 30)  # 左右距离
  10. fwd_range = (0, 80)  # 后前距离
  11. res = 0.1  # 分辨率0.05m
  12.  
  13. def compute_box_3d(obj, P):
  14.     '''
  15.     计算对象的3D边界框在图像平面上的投影
  16.     输入: obj代表一个物体标签信息,  P代表相机的投影矩阵-内参。
  17.     输出: 返回两个值, corners_3d表示3D边界框在 相机坐标系 的8个角点的坐标-3D坐标。
  18.                                      corners_2d表示3D边界框在 图像上 的8个角点的坐标-2D坐标。
  19.     '''
  20.     # 计算一个绕Y轴旋转的旋转矩阵R，用于将3D坐标从世界坐标系转换到相机坐标系。obj.ry是对象的偏航角
  21.     R = roty(obj.ry)    
  22.  
  23.     # 物体实际的长、宽、高
  24.     l = obj.l;
  25.     w = obj.w;
  26.     h = obj.h;
  27.     
  28.     # 存储了3D边界框的8个角点相对于对象中心的坐标。这些坐标定义了3D边界框的形状。
  29.     x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
  30.     y_corners = [0,0,0,0,-h,-h,-h,-h];
  31.     z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
  32.     
  33.     # 1、将3D边界框的角点坐标从对象坐标系转换到相机坐标系。它使用了旋转矩阵R
  34.     corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
  35.     # 3D边界框的坐标进行平移
  36.     corners_3d[0,:] = corners_3d[0,:] + obj.t[0];
  37.     corners_3d[1,:] = corners_3d[1,:] + obj.t[1];
  38.     corners_3d[2,:] = corners_3d[2,:] + obj.t[2];
  39.  
  40.     # 2、检查对象是否在相机前方，因为只有在相机前方的对象才会被绘制。
  41.     # 如果对象的Z坐标（深度）小于0.1，就意味着对象在相机后方，那么corners_2d将被设置为None，函数将返回None。
  42.     if np.any(corners_3d[2,:]<0.1):
  43.         corners_2d = None
  44.         return corners_2d, np.transpose(corners_3d)
  45.     
  46.     # 3、将相机坐标系下的3D边界框的角点，投影到图像平面上，得到它们在图像上的2D坐标。
  47.     corners_2d = project_to_image(np.transpose(corners_3d), P);
  48.     return corners_2d, np.transpose(corners_3d)
  49.  
  50.  
  51. def project_to_image(pts_3d, P):
  52.     '''
  53.     将相机坐标系下的3D边界框的角点, 投影到图像平面上, 得到它们在图像上的2D坐标
  54.     输入: pts_3d是一个nx3的矩阵, 包含了待投影的3D坐标点(每行一个点), P是相机的投影矩阵, 通常是一个3x4的矩阵。
  55.     输出: 返回一个nx2的矩阵, 包含了投影到图像平面上的2D坐标点。
  57.       P(3x4) dot pts_3d_extended(4xn) = projected_pts_2d(3xn)  => normalize projected_pts_2d(2xn)
  58.       <=> pts_3d_extended(nx4) dot P'(4x3) = projected_pts_2d(nx3)   => normalize projected_pts_2d(nx2)
  59.     '''
  60.     n = pts_3d.shape[0] # 获取3D点的数量
  61.     pts_3d_extend = np.hstack((pts_3d, np.ones((n,1)))) # 将每个3D点的坐标扩展为齐次坐标形式（4D），通过在每个点的末尾添加1，创建了一个nx4的矩阵。
  62.     pts_2d = np.dot(pts_3d_extend, np.transpose(P)) # 将扩展的3D坐标点矩阵与投影矩阵P相乘，得到一个nx3的矩阵，其中每一行包含了3D点在图像平面上的投影坐标。每个点的坐标表示为[x, y, z]。
  63.     pts_2d[:,0] /= pts_2d[:,2] # 将投影坐标中的x坐标除以z坐标，从而获得2D图像上的x坐标。
  64.     pts_2d[:,1] /= pts_2d[:,2] # 将投影坐标中的y坐标除以z坐标，从而获得2D图像上的y坐标。
  65.     return pts_2d[:,0:2] # 返回一个nx2的矩阵,其中包含了每个3D点在2D图像上的坐标。
  66.  
  67.  
  68.  
  69. def draw_projected_box3d(image, qs, color=(0,60,255), thickness=2):
  70.     '''
  71.     qs: 包含8个3D边界框角点坐标的数组, 形状为(8, 2)。图像坐标下的3D框, 8个顶点坐标。
  72.     '''
  73.     ''' Draw 3d bounding box in image
  74.         qs: (8,2) array of vertices for the 3d box in following order:
  75.             1 -------- 0
  76.            /|         /|
  77.           2 -------- 3 .
  78.           | |        | |
  79.           . 5 -------- 4
  80.           |/         |/
  81.           6 -------- 7
  82.     '''
  83.     qs = qs.astype(np.int32) # 将输入的顶点坐标转换为整数类型，以便在图像上绘制。
  84.  
  85.     # 这个循环迭代4次，每次处理一个边界框的一条边。
  86.     for k in range(0,4):
  87.        # Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html
  88.  
  89.        # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制边界框的前四条边。
  90.        i,j=k,(k+1)%4
  91.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  92.  
  93.         # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制边界框的后四条边，与前四条边平行
  94.        i,j=k+4,(k+1)%4 + 4
  95.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  96.  
  97.         # 定义了要绘制的边的起始点和结束点的索引。在这个循环中，它用于绘制连接前四条边和后四条边的边界框的边。
  98.        i,j=k,k+4
  99.        cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness)
  100.     return image
  101.  
  102. def draw_box3d_label_on_bev(image, boxes3d, thickness=1, scores=None):
  103.     # if scores is not None and scores.shape[0] >0:
  104.     img = image.copy() 
  105.     num = len(boxes3d)
  106.     for n in range(num):
  107.         b = boxes3d[n]
  108.         x0 = b[0, 0]
  109.         y0 = b[0, 1]
  110.         x1 = b[1, 0]
  111.         y1 = b[1, 1]
  112.         x2 = b[2, 0]
  113.         y2 = b[2, 1]
  114.         x3 = b[3, 0]
  115.         y3 = b[3, 1]
  116.         if (x0<30 and x1<30 and x2<30 and x3<30):
  117.             u0, v0 = lidar_to_top_coords(x0, y0)
  118.             u1, v1 = lidar_to_top_coords(x1, y1)
  119.             u2, v2 = lidar_to_top_coords(x2, y2)
  120.             u3, v3 = lidar_to_top_coords(x3, y3)
  121.             color = (0, 255, 0) # green
  122.             cv2.line(img, (u0, v0), (u1, v1), color, thickness, cv2.LINE_AA)
  123.             cv2.line(img, (u1, v1), (u2, v2), color, thickness, cv2.LINE_AA)
  124.             cv2.line(img, (u2, v2), (u3, v3), color, thickness, cv2.LINE_AA)
  125.             cv2.line(img, (u3, v3), (u0, v0), color, thickness, cv2.LINE_AA)
  126.         elif (x0<50 and x1<50 and x2<50 and x3<50):
  127.             color = (255, 0, 0) # red
  128.             u0, v0 = lidar_to_top_coords(x0, y0)
  129.             u1, v1 = lidar_to_top_coords(x1, y1)
  130.             u2, v2 = lidar_to_top_coords(x2, y2)
  131.             u3, v3 = lidar_to_top_coords(x3, y3)
  132.             cv2.line(img, (u0, v0), (u1, v1), color, thickness, cv2.LINE_AA)
  133.             cv2.line(img, (u1, v1), (u2, v2), color, thickness, cv2.LINE_AA)
  134.             cv2.line(img, (u2, v2), (u3, v3), color, thickness, cv2.LINE_AA)
  135.             cv2.line(img, (u3, v3), (u0, v0), color, thickness, cv2.LINE_AA)
  136.         else:
  137.             color = (0, 0, 255) # blue
  138.             u0, v0 = lidar_to_top_coords(x0, y0)
  139.             u1, v1 = lidar_to_top_coords(x1, y1)
  140.             u2, v2 = lidar_to_top_coords(x2, y2)
  141.             u3, v3 = lidar_to_top_coords(x3, y3)
  142.             cv2.line(img, (u0, v0), (u1, v1), color, thickness, cv2.LINE_AA)
  143.             cv2.line(img, (u1, v1), (u2, v2), color, thickness, cv2.LINE_AA)
  144.             cv2.line(img, (u2, v2), (u3, v3), color, thickness, cv2.LINE_AA)
  145.             cv2.line(img, (u3, v3), (u0, v0), color, thickness, cv2.LINE_AA)       
  146.  
  147.     return img
  148.  
  149. def draw_box3d_predict_on_bev(image, boxes3d, thickness=1, scores=None):
  150.      # if scores is not None and scores.shape[0] >0:
  151.     img = image.copy() 
  152.     num = len(boxes3d)
  153.     for n in range(num):
  154.         b = boxes3d[n]
  155.         x0 = b[0, 0]
  156.         y0 = b[0, 1]
  157.         x1 = b[1, 0]
  158.         y1 = b[1, 1]
  159.         x2 = b[2, 0]
  160.         y2 = b[2, 1]
  161.         x3 = b[3, 0]
  162.         y3 = b[3, 1]
  163.         color = (255, 255, 255) # white
  164.         u0, v0 = lidar_to_top_coords(x0, y0)
  165.         u1, v1 = lidar_to_top_coords(x1, y1)
  166.         u2, v2 = lidar_to_top_coords(x2, y2)
  167.         u3, v3 = lidar_to_top_coords(x3, y3)
  168.         cv2.line(img, (u0, v0), (u1, v1), color, thickness, cv2.LINE_AA)
  169.         cv2.line(img, (u1, v1), (u2, v2), color, thickness, cv2.LINE_AA)
  170.         cv2.line(img, (u2, v2), (u3, v3), color, thickness, cv2.LINE_AA)
  171.         cv2.line(img, (u3, v3), (u0, v0), color, thickness, cv2.LINE_AA)
  172.     return img
  173.  
  174. def lidar_to_top_coords(x, y, z=None):
  175.     if 0:
  176.         return x, y
  177.     else:
  178.         # print("TOP_X_MAX-TOP_X_MIN:",TOP_X_MAX,TOP_X_MIN)
  179.         xx = (-y / res).astype(np.int32)
  180.         yy = (-x / res).astype(np.int32)
  181.         # 调整坐标原点
  182.         xx -= int(np.floor(side_range[0]) / res)
  183.         yy += int(np.floor(fwd_range[1]) / res)
  184.         return xx, yy
  185.  
  186.  
  187. # 解析标签数据
  188. class Object3d(object):
  189.     ''' 3d object label '''
  190.     def __init__(self, label_file_line):
  191.         data = label_file_line.split(' ')
  192.         data[1:] = [float(x) for x in data[1:]]
  193.  
  194.         # extract label, truncation, occlusion
  195.         self.type = data[0] # 'Car', 'Pedestrian', ...
  196.         self.truncation = data[1] # truncated pixel ratio [0..1]
  197.         self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown
  198.         self.alpha = data[3] # object observation angle [-pi..pi]
  199.  
  200.         # extract 2d bounding box in 0-based coordinates
  201.         self.xmin = data[4] # left
  202.         self.ymin = data[5] # top
  203.         self.xmax = data[6] # right
  204.         self.ymax = data[7] # bottom
  205.         self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax])
  206.         
  207.         # extract 3d bounding box information
  208.         self.h = data[8] # box height
  209.         self.w = data[9] # box width
  210.         self.l = data[10] # box length (in meters)
  211.         self.t = (data[11],data[12],data[13]) # location (x,y,z) in camera coord.
  212.         self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
  213.  
  214.     def print_object(self):
  215.         print('Type, truncation, occlusion, alpha: %s, %d, %d, %f' % \
  216.             (self.type, self.truncation, self.occlusion, self.alpha))
  217.         print('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f' % \
  218.             (self.xmin, self.ymin, self.xmax, self.ymax))
  219.         print('3d bbox h,w,l: %f, %f, %f' % \
  220.             (self.h, self.w, self.l))
  221.         print('3d bbox location, ry: (%f, %f, %f), %f' % \
  222.             (self.t[0],self.t[1],self.t[2],self.ry))
  223.  
  224.  
  225. class Calibration(object):
  226.     ''' Calibration matrices and utils
  227.         3d XYZ in <label>.txt are in rect camera coord.
  228.         2d box xy are in image2 coord
  229.         Points in <lidar>.bin are in Velodyne coord.
  231.         y_image2 = P^2_rect * x_rect
  232.         y_image2 = P^2_rect * R0_rect * Tr_velo_to_cam * x_velo
  233.         x_ref = Tr_velo_to_cam * x_velo
  234.         x_rect = R0_rect * x_ref
  236.         P^2_rect = [f^2_u,  0,      c^2_u,  -f^2_u b^2_x;
  237.                     0,      f^2_v,  c^2_v,  -f^2_v b^2_y;
  238.                     0,      0,      1,      0]
  239.                  = K * [1|t]
  241.         image2 coord:
  242.          ----> x-axis (u)
  243.         |
  244.         |
  245.         v y-axis (v)
  247.         velodyne coord:
  248.         front x, left y, up z
  250.         rect/ref camera coord:
  251.         right x, down y, front z
  253.         Ref (KITTI paper): http://www.cvlibs.net/publications/Geiger2013IJRR.pdf
  255.         TODO(rqi): do matrix multiplication only once for each projection.
  256.     '''
  257.     def __init__(self, calib_filepath, from_video=False):
  258.         if from_video:
  259.             calibs = self.read_calib_from_video(calib_filepath)
  260.         else:
  261.             calibs = self.read_calib_file(calib_filepath)
  262.         # Projection matrix from rect camera coord to image2 coord
  263.         self.P = calibs['P2'] 
  264.         self.P = np.reshape(self.P, [3,4])
  265.         # Rigid transform from Velodyne coord to reference camera coord
  266.         self.V2C = calibs['Tr_velo_to_cam']
  267.         self.V2C = np.reshape(self.V2C, [3,4])
  268.         self.C2V = inverse_rigid_trans(self.V2C)
  269.         # Rotation from reference camera coord to rect camera coord
  270.         self.R0 = calibs['R0_rect']
  271.         self.R0 = np.reshape(self.R0,[3,3])
  272.  
  273.         # Camera intrinsics and extrinsics
  274.         self.c_u = self.P[0,2]
  275.         self.c_v = self.P[1,2]
  276.         self.f_u = self.P[0,0]
  277.         self.f_v = self.P[1,1]
  278.         self.b_x = self.P[0,3]/(-self.f_u) # relative 
  279.         self.b_y = self.P[1,3]/(-self.f_v)
  280.  
  281.     def read_calib_file(self, filepath):
  282.         ''' Read in a calibration file and parse into a dictionary.'''
  283.         data = {}
  284.         with open(filepath, 'r') as f:
  285.             for line in f.readlines():
  286.                 line = line.rstrip()
  287.                 if len(line)==0: continue
  288.                 key, value = line.split(':', 1)
  289.                 # The only non-float values in these files are dates, which
  290.                 # we don't care about anyway
  291.                 try:
  292.                     data[key] = np.array([float(x) for x in value.split()])
  293.                 except ValueError:
  294.                     pass
  295.  
  296.         return data
  297.     
  298.     def read_calib_from_video(self, calib_root_dir):
  299.         ''' Read calibration for camera 2 from video calib files.
  300.             there are calib_cam_to_cam and calib_velo_to_cam under the calib_root_dir
  301.         '''
  302.         data = {}
  303.         cam2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_cam_to_cam.txt'))
  304.         velo2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_velo_to_cam.txt'))
  305.         Tr_velo_to_cam = np.zeros((3,4))
  306.         Tr_velo_to_cam[0:3,0:3] = np.reshape(velo2cam['R'], [3,3])
  307.         Tr_velo_to_cam[:,3] = velo2cam['T']
  308.         data['Tr_velo_to_cam'] = np.reshape(Tr_velo_to_cam, [12])
  309.         data['R0_rect'] = cam2cam['R_rect_00']
  310.         data['P2'] = cam2cam['P_rect_02']
  311.         return data
  312.  
  313.     def cart2hom(self, pts_3d):
  314.         ''' Input: nx3 points in Cartesian
  315.             Oupput: nx4 points in Homogeneous by pending 1
  316.         '''
  317.         n = pts_3d.shape[0]
  318.         pts_3d_hom = np.hstack((pts_3d, np.ones((n,1))))
  319.         return pts_3d_hom
  320.  
  321.     # =========================== 
  322.     # ------- 3d to 3d ---------- 
  323.     # =========================== 
  324.     def project_velo_to_ref(self, pts_3d_velo):
  325.         pts_3d_velo = self.cart2hom(pts_3d_velo) # nx4
  326.         return np.dot(pts_3d_velo, np.transpose(self.V2C))
  327.  
  328.     def project_ref_to_velo(self, pts_3d_ref):
  329.         pts_3d_ref = self.cart2hom(pts_3d_ref) # nx4
  330.         return np.dot(pts_3d_ref, np.transpose(self.C2V))
  331.  
  332.     def project_rect_to_ref(self, pts_3d_rect):
  333.         ''' Input and Output are nx3 points '''
  334.         return np.transpose(np.dot(np.linalg.inv(self.R0), np.transpose(pts_3d_rect)))
  335.     
  336.     def project_ref_to_rect(self, pts_3d_ref):
  337.         ''' Input and Output are nx3 points '''
  338.         return np.transpose(np.dot(self.R0, np.transpose(pts_3d_ref)))
  339.  
  340.     def project_rect_to_velo(self, pts_3d_rect):
  341.         ''' Input: nx3 points in rect camera coord.
  342.             Output: nx3 points in velodyne coord.
  343.         ''' 
  344.         pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
  345.         return self.project_ref_to_velo(pts_3d_ref)
  346.  
  347.     def project_velo_to_rect(self, pts_3d_velo):
  348.         pts_3d_ref = self.project_velo_to_ref(pts_3d_velo)
  349.         return self.project_ref_to_rect(pts_3d_ref)
  350.     
  351.     def corners3d_to_img_boxes(self, corners3d):
  352.         """
  353.         :param corners3d: (N, 8, 3) corners in rect coordinate
  354.         :return: boxes: (None, 4) [x1, y1, x2, y2] in rgb coordinate
  355.         :return: boxes_corner: (None, 8) [xi, yi] in rgb coordinate
  356.         """
  357.         sample_num = corners3d.shape[0]
  358.         corners3d_hom = np.concatenate((corners3d, np.ones((sample_num, 8, 1))), axis=2)  # (N, 8, 4)
  359.  
  360.         img_pts = np.matmul(corners3d_hom, self.P.T)  # (N, 8, 3)
  361.  
  362.         x, y = img_pts[:, :, 0] / img_pts[:, :, 2], img_pts[:, :, 1] / img_pts[:, :, 2]
  363.         x1, y1 = np.min(x, axis=1), np.min(y, axis=1)
  364.         x2, y2 = np.max(x, axis=1), np.max(y, axis=1)
  365.  
  366.         boxes = np.concatenate((x1.reshape(-1, 1), y1.reshape(-1, 1), x2.reshape(-1, 1), y2.reshape(-1, 1)), axis=1)
  367.         boxes_corner = np.concatenate((x.reshape(-1, 8, 1), y.reshape(-1, 8, 1)), axis=2)
  368.  
  369.         return boxes, boxes_corner
  370.  
  371.  
  372.     # =========================== 
  373.     # ------- 3d to 2d ---------- 
  374.     # =========================== 
  375.     def project_rect_to_image(self, pts_3d_rect):
  376.         ''' Input: nx3 points in rect camera coord.
  377.             Output: nx2 points in image2 coord.
  378.         '''
  379.         pts_3d_rect = self.cart2hom(pts_3d_rect)
  380.         pts_2d = np.dot(pts_3d_rect, np.transpose(self.P)) # nx3
  381.         pts_2d[:,0] /= pts_2d[:,2]
  382.         pts_2d[:,1] /= pts_2d[:,2]
  383.         return pts_2d[:,0:2]
  384.     
  385.     def project_velo_to_image(self, pts_3d_velo):
  386.         ''' Input: nx3 points in velodyne coord.
  387.             Output: nx2 points in image2 coord.
  388.         '''
  389.         pts_3d_rect = self.project_velo_to_rect(pts_3d_velo)
  390.         return self.project_rect_to_image(pts_3d_rect)
  391.  
  392.     # =========================== 
  393.     # ------- 2d to 3d ---------- 
  394.     # =========================== 
  395.     def project_image_to_rect(self, uv_depth):
  396.         ''' Input: nx3 first two channels are uv, 3rd channel
  397.                    is depth in rect camera coord.
  398.             Output: nx3 points in rect camera coord.
  399.         '''
  400.         n = uv_depth.shape[0]
  401.         x = ((uv_depth[:,0]-self.c_u)*uv_depth[:,2])/self.f_u + self.b_x
  402.         y = ((uv_depth[:,1]-self.c_v)*uv_depth[:,2])/self.f_v + self.b_y
  403.         pts_3d_rect = np.zeros((n,3))
  404.         pts_3d_rect[:,0] = x
  405.         pts_3d_rect[:,1] = y
  406.         pts_3d_rect[:,2] = uv_depth[:,2]
  407.         return pts_3d_rect
  408.  
  409.     def project_image_to_velo(self, uv_depth):
  410.         pts_3d_rect = self.project_image_to_rect(uv_depth)
  411.         return self.project_rect_to_velo(pts_3d_rect)
  412.  
  413.  
  414. def rotx(t):
  415.     ''' 3D Rotation about the x-axis. '''
  416.     c = np.cos(t)
  417.     s = np.sin(t)
  418.     return np.array([[1,  0,  0],
  419.                      [0,  c, -s],
  420.                      [0,  s,  c]])
  421.  
  422.  
  423. def roty(t):
  424.     ''' Rotation about the y-axis. '''
  425.     c = np.cos(t)
  426.     s = np.sin(t)
  427.     return np.array([[c,  0,  s],
  428.                      [0,  1,  0],
  429.                      [-s, 0,  c]])
  430.  
  431.  
  432. def rotz(t):
  433.     ''' Rotation about the z-axis. '''
  434.     c = np.cos(t)
  435.     s = np.sin(t)
  436.     return np.array([[c, -s,  0],
  437.                      [s,  c,  0],
  438.                      [0,  0,  1]])
  439.  
  440.  
  441. def transform_from_rot_trans(R, t):
  442.     ''' Transforation matrix from rotation matrix and translation vector. '''
  443.     R = R.reshape(3, 3)
  444.     t = t.reshape(3, 1)
  445.     return np.vstack((np.hstack([R, t]), [0, 0, 0, 1]))
  446.  
  447.  
  448. def inverse_rigid_trans(Tr):
  449.     ''' Inverse a rigid body transform matrix (3x4 as [R|t])
  450.         [R'|-R't; 0|1]
  451.     '''
  452.     inv_Tr = np.zeros_like(Tr) # 3x4
  453.     inv_Tr[0:3,0:3] = np.transpose(Tr[0:3,0:3])
  454.     inv_Tr[0:3,3] = np.dot(-np.transpose(Tr[0:3,0:3]), Tr[0:3,3])
  455.     return inv_Tr
  456.  
  457. def read_label(label_filename):
  458.     lines = [line.rstrip() for line in open(label_filename)]
  459.     objects = [Object3d(line) for line in lines]
  460.     return objects
  461.  
  462. def load_image(img_filename):
  463.     return cv2.imread(img_filename)
  464.  
  465. def load_velo_scan(velo_filename):
  466.     scan = np.fromfile(velo_filename, dtype=np.float32)
  467.     scan = scan.reshape((-1, 4))
  468.     return scan
  469.

kitti_object.py

  1.  
  2.  
  3. from __future__ import print_function
  4.  
  5. import os
  6. import sys
  7. import cv2
  8. import numpy as np
  9. from PIL import Image
  10. import matplotlib.pyplot as plt
  11. BASE_DIR = os.path.dirname(os.path.abspath(__file__))
  12. ROOT_DIR = os.path.dirname(BASE_DIR)
  13. sys.path.append(os.path.join(ROOT_DIR, 'mayavi'))
  14. import kitti_util as utils
  15.  
  16.  
  17. '''
  18. 在图像中画2D框、3D框
  19. '''
  20. def show_image_with_boxes(img, objects, calib, show3d=True):
  21.     img1 = np.copy(img) # for 2d bbox
  22.     img2 = np.copy(img) # for 3d bbox
  23.     for obj in objects:
  24.         if obj.type=='DontCare':continue
  25.         cv2.rectangle(img1, (int(obj.xmin),int(obj.ymin)), (int(obj.xmax),int(obj.ymax)), (0,255,0), 2) # 画2D框
  26.         box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P) # 获取图像3D框(8*2)、相机坐标系3D框(8*3)
  27.         img2 = utils.draw_projected_box3d(img2, box3d_pts_2d) # 在图像上画3D框
  28.     if show3d:
  29.         Image.fromarray(img2).save('save_output/image_with_3Dboxes.png')
  30.         Image.fromarray(img2).show()
  31.     else:
  32.         Image.fromarray(img1).save('save_output/image_with_2Dboxes.png')
  33.         Image.fromarray(img1).show()
  34.  
  35.  
  36. '''
  37. 可视化BEV鸟瞰图
  38. '''
  39. def show_lidar_topview(pc_velo, objects, calib):
  40.       # 1-设置鸟瞰图范围
  41.     side_range = (-30, 30)  # 左右距离
  42.     fwd_range = (0, 80)  # 后前距离
  43.     
  44.     x_points = pc_velo[:, 0]
  45.     y_points = pc_velo[:, 1]
  46.     z_points = pc_velo[:, 2]
  47.     
  48.     # 2-获得区域内的点
  49.     f_filt = np.logical_and(x_points > fwd_range[0], x_points < fwd_range[1])
  50.     s_filt = np.logical_and(y_points > side_range[0], y_points < side_range[1])
  51.     filter = np.logical_and(f_filt, s_filt)
  52.     indices = np.argwhere(filter).flatten() 
  53.     x_points = x_points[indices]
  54.     y_points = y_points[indices]
  55.     z_points = z_points[indices]
  56.     
  57.     # 定义了鸟瞰图中每个像素代表的距离
  58.     res = 0.1   
  59.     # 3-1将点云坐标系 转到 BEV坐标系
  60.     x_img = (-y_points / res).astype(np.int32)
  61.     y_img = (-x_points / res).astype(np.int32)
  62.     # 3-2调整坐标原点
  63.     x_img -= int(np.floor(side_range[0]) / res)
  64.     y_img += int(np.floor(fwd_range[1]) / res)
  65.     print(x_img.min(), x_img.max(), y_img.min(), y_img.max()) 
  66.     
  67.     # 4-填充像素值, 将点云数据的高度信息（Z坐标）映射到像素值
  68.     height_range = (-3, 1.0)
  69.     pixel_value = np.clip(a=z_points, a_max=height_range[1], a_min=height_range[0])
  70.      
  71.  
  72.     def scale_to_255(a, min, max, dtype=np.uint8):
  73.         return ((a - min) / float(max - min) * 255).astype(dtype)
  74.     
  75.     pixel_value = scale_to_255(pixel_value, height_range[0], height_range[1])
  76.     
  77.     # 创建图像数组
  78.     x_max = 1 + int((side_range[1] - side_range[0]) / res)
  79.     y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
  80.     im = np.zeros([y_max, x_max], dtype=np.uint8)
  81.     im[y_img, x_img] = pixel_value
  82.     
  83.     im2 = Image.fromarray(im)
  84.     im2.save('save_output/BEV.png')
  85.     im2.show()
  86.  
  87.  
  88. '''
  89. 将点云数据3D框投影到BEV
  90. '''
  91. def show_lidar_topview_with_boxes(img, objects, calib):
  92.     def bbox3d(obj):
  93.         box3d_pts_2d, box3d_pts_3d = utils.compute_box_3d(obj, calib.P) # 获取3D框-图像、3D框-相机坐标系
  94.         box3d_pts_3d_velo = calib.project_rect_to_velo(box3d_pts_3d) # 将相机坐标系的框 转到 激光雷达坐标系
  95.         return box3d_pts_3d_velo # 返回nx3的点
  96.  
  97.     boxes3d = [bbox3d(obj) for obj in objects if obj.type == "Car"]
  98.     gt = np.array(boxes3d)
  99.     im2 = utils.draw_box3d_label_on_bev(img, gt, scores=None, thickness=1) # 获取激光雷达坐标系的3D点，选择x, y两维，画到BEV平面坐标系上
  100.     im2 = Image.fromarray(im2)
  101.     im2.save('save_output/BEV with boxes.png')
  102.     im2.show()
  103.  
  104.  
  105. '''
  106. 将点云数据投影到图像
  107. '''
  108. def show_lidar_on_image(pc_velo, img, calib, img_width, img_height):
  109.     ''' Project LiDAR points to image '''
  110.     imgfov_pc_velo, pts_2d, fov_inds = get_lidar_in_image_fov(pc_velo,
  111.         calib, 0, 0, img_width, img_height, True)
  112.     imgfov_pts_2d = pts_2d[fov_inds,:]
  113.     imgfov_pc_rect = calib.project_velo_to_rect(imgfov_pc_velo)
  114.  
  115.     import matplotlib.pyplot as plt
  116.     cmap = plt.cm.get_cmap('hsv', 256)
  117.     cmap = np.array([cmap(i) for i in range(256)])[:,:3]*255
  118.  
  119.     for i in range(imgfov_pts_2d.shape[0]):
  120.         depth = imgfov_pc_rect[i,2]
  121.         color = cmap[int(640.0/depth),:]
  122.         cv2.circle(img, (int(np.round(imgfov_pts_2d[i,0])),
  123.             int(np.round(imgfov_pts_2d[i,1]))),
  124.             2, color=tuple(color), thickness=-1)
  125.     Image.fromarray(img).save('save_output/lidar_on_image.png')
  126.     Image.fromarray(img).show() 
  127.     return img
  128.  
  129.  
  130. '''
  131. 将点云数据投影到相机坐标系
  132. '''
  133. def get_lidar_in_image_fov(pc_velo, calib, xmin, ymin, xmax, ymax,
  134.                            return_more=False, clip_distance=2.0):
  135.     ''' Filter lidar points, keep those in image FOV '''
  136.     pts_2d = calib.project_velo_to_image(pc_velo)
  137.     fov_inds = (pts_2d[:,0]<xmax) & (pts_2d[:,0]>=xmin) & \
  138.         (pts_2d[:,1]<ymax) & (pts_2d[:,1]>=ymin)
  139.     fov_inds = fov_inds & (pc_velo[:,0]>clip_distance)
  140.     imgfov_pc_velo = pc_velo[fov_inds,:]
  141.     if return_more:
  142.         return imgfov_pc_velo, pts_2d, fov_inds
  143.     else:
  144.         return imgfov_pc_velo
  145.     
  146.  
  147. '''
  148. 解析标签
  149. '''
  150. class kitti_object(object):
  151.     '''Load and parse object data into a usable format.'''
  152.     
  153.     def __init__(self, root_dir, split='training'):
  154.         '''root_dir contains training and testing folders'''
  155.         self.root_dir = root_dir
  156.         self.split = split
  157.         self.split_dir = os.path.join(root_dir, split)
  158.  
  159.         if split == 'training':
  160.             self.num_samples = 7481
  161.         elif split == 'testing':
  162.             self.num_samples = 7518
  163.         else:
  164.             print('Unknown split: %s' % (split))
  165.             exit(-1)
  166.  
  167.         self.image_dir = os.path.join(self.split_dir, 'image_2')
  168.         self.calib_dir = os.path.join(self.split_dir, 'calib')
  169.         self.lidar_dir = os.path.join(self.split_dir, 'velodyne')
  170.         self.label_dir = os.path.join(self.split_dir, 'label_2')
  171.  
  172.     def __len__(self):
  173.         return self.num_samples
  174.  
  175.     def get_image(self, idx):
  176.         assert(idx<self.num_samples) 
  177.         img_filename = os.path.join(self.image_dir, '%06d.png'%(idx))
  178.         return utils.load_image(img_filename)
  179.  
  180.     def get_lidar(self, idx): 
  181.         assert(idx<self.num_samples) 
  182.         lidar_filename = os.path.join(self.lidar_dir, '%06d.bin'%(idx))
  183.         return utils.load_velo_scan(lidar_filename)
  184.  
  185.     def get_calibration(self, idx):
  186.         assert(idx<self.num_samples) 
  187.         calib_filename = os.path.join(self.calib_dir, '%06d.txt'%(idx))
  188.         return utils.Calibration(calib_filename)
  189.  
  190.     def get_label_objects(self, idx):
  191.         assert(idx<self.num_samples and self.split=='training') 
  192.         label_filename = os.path.join(self.label_dir, '%06d.txt'%(idx))
  193.         return utils.read_label(label_filename)
  194.     
  195.     def get_depth_map(self, idx):
  196.         pass
  197.  
  198.     def get_top_down(self, idx):
  199.         pass

运行程序后kitti_vis_main.py后，回保存5张结果图片

后面还会介绍Nuscenes、Waymo等3D数据集。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/article/detail/33034
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号