使用kitti数据集实现自动驾驶——发布照片、点云、IMU、GPS、显示2D和3D侦测框_怎么在kitti数据集图片上显示边界框

作者：秃头小苏
编辑：3D视觉开发者社区

 

本次内容主要是使用kitti数据集来可视化kitti车上一些传感器（相机、激光雷达、IMU）采集的资料以及对行人和车辆进行检测并在图像中画出行人和车辆的2D框、在点云中画出行人和车辆的3D框。

首先先看看最终实现的效果：

看了上面的效果视频，是不是充满好奇了呢，下面让我们一步步的来学习

1、准备工作

1.1数据集下载

在开始之前，先做一些准备工作，即从kitti上下载相关数据：kitty官网

如图所示：下载途中箭头所指的两个文件【注：需要先进行注册】

除了下载这两个文件，后面还需要下载汽车模型文件和标注文件，这里直接贴出下载地址：数据下载

 

1.2 创建工作空间并建立一些文件

• 创建功能包

cd ~/catkin_ws/src
catkin_create_pkg kitti_turtorial rospy 
1
2

• 在刚创建的功能包下的src文件夹中创建以下python文件

  

 

2、详细步骤

说明：该部分只是自己的学习笔记，故只会贴出每一步比较核心的代码，要想看懂整个流程，建议完整的观看相关视频：视频

当然最后我也会贴出所有文件的源码供大家学习

 
 

2.1 发布照片

#创建一个摄像头的发布者
cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10)
#读取摄像机数据
image = read_camera(os.path.join(DAtA_PATH, 'image_02/data/%010d.png'%frame))
#发布数据
publish_camera(cam_pub,bridge,image,boxes_2d,types)
1
2
3
4
5
6

 

2.2 发布点云

#创建一个点云的发布者
pcl_pub = rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)
#读取点云数据
point_cloud = read_point_cloud(os.path.join(DAtA_PATH,'velodyne_points/data/%010d.bin'%frame))
#发布数据    
publish_point_cloud(pcl_pub,point_cloud)
1
2
3
4
5
6

 

2.3 画出自己车子以及照相机视野

#创建一个汽车的发布者
ego_pub = rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
#发布ego_car数据
publish_ego_car(ego_pub)

##绘制车子的照相机视野
marker.id = 0
marker.action = marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP

marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
    
marker.points = []
marker.points.append(Point(10,-10,0))
marker.points.append(Point(0,0,0))
marker.points.append(Point(10,10,0))

marker_array.markers.append(marker)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

 

2.4 发布IMU

#创建一个IMU发布者
imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10)

#发布imu数据
publish_imu(imu_pub,imu_data)

##IMU发布函数相关设置
def publish_imu(imu_pub,imu_data):
    imu = Imu()
    imu.header.frame_id = FRAME_ID
    imu.header.stamp = rospy.Time.now()
    
    #设置旋转量
    q = tf.transformations.quaternion_from_euler(float(imu_data.roll),float(imu_data.pitch),float(imu_data.yaw));
    imu.orientation.x = q[0]
    imu.orientation.y = q[1]
    imu.orientation.z = q[2]
    imu.orientation.w = q[3]
    
    #设置线性加速度
    imu.linear_acceleration.x = imu_data.af
    imu.linear_acceleration.y = imu_data.al
    imu.linear_acceleration.z = imu_data.au
    
    #设置角加速度
    imu.angular_velocity.x = imu_data.wf
    imu.angular_velocity.y = imu_data.wl
    imu.angular_velocity.z = imu_data.wu
    
    imu_pub.publish(imu)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

 

2.5 发布GPS

#创建一个GPS发布者
gps_pub = rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)
#发布GPS数据
publish_gps(gps_pub,imu_data)
 
##GPS发布函数相关设置
def publish_gps(gps_pub,imu_data):
    gps = NavSatFix()
    gps.header.frame_id = FRAME_ID
    gps.header.stamp = rospy.Time.now()
    
    #gps经度、纬度、海拔高度
    gps.latitude = imu_data.lat
    gps.longitude = imu_data.lon
    gps.altitude = imu_data.alt
    
    gps_pub.publish(gps) 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

 

2.6 在rviz上显示2D侦测框

#读取2D检测框数据
boxes_2d = np.array(df_tracking_frame[['bbox_left', 'bbox_top', 'bbox_right', 'bbox_bottom']])
types = np.array(df_tracking_frame['type'])

## 2D框相关设置
for typ,box in zip(types,boxes):
    top_left = int(box[0]),int(box[1])
    bottom_right = int(box[2]),int(box[3])
    cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2)
cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))
1
2
3
4
5
6
7
8
9
10

 

2.7 在rviz上显示3D 侦测框

 #读取3D检测框数据
boxes_3d = np.array(df_tracking_frame[['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']]      corners_3d_velos = []
for box_3d in boxes_3d:
    corners_3d_cam2 = compute_3d_box_cam2(*box_3d)
    corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)
    corners_3d_velos += [corners_3d_velo]
  
##3D框发布函数相关设置
def publish_3dbox(box3d_pub,corners_3d_velos,types):
    marker_array = MarkerArray()
    for i, corners_3d_velo in enumerate(corners_3d_velos):
        # 3d box
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()
        
        marker.id = i
        marker.action = Marker.ADD
        marker.lifetime = rospy.Duration(LIFETIME)
        marker.type = Marker.LINE_LIST
        
        
        b, g, r = DETECTION_COLOR_DICT[types[i]]
    
        marker.color.r = r/255.0
        marker.color.g = g/255.0
        marker.color.b = b/255.0
        marker.color.a = 1.0
        
        marker.scale.x = 0.1
        
        marker.points = []
        for l in LINES:
            p1 = corners_3d_velo[l[0]]
            marker.points.append(Point(p1[0], p1[1], p1[2]))
            p2 = corners_3d_velo[l[1]]
            marker.points.append(Point(p2[0], p2[1], p2[2]))
        marker_array.markers.append(marker)
        
    box3d_pub.publish(marker_array)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

3、代码合集

代码托管在Gitee上，自行下载：代码

---

版权声明：本文为奥比中光3D视觉开发者社区特约作者授权原创发布，未经授权不得转载，本文仅做学术分享，版权归原作者所有，若涉及侵权内容请联系删文。

3D视觉开发者社区是由奥比中光给所有开发者打造的分享与交流平台，旨在将3D视觉技术开放给开发者。平台为开发者提供3D视觉领域免费课程、奥比中光独家资源与专业技术支持。

加入【3D视觉开发者社区】学习行业前沿知识，赋能开发者技能提升！
加入【3D视觉AI开放平台】体验AI算法能力，助力开发者视觉算法落地！

往期推荐：

1、奥比中光&英伟达第三届3D视觉创新应用竞赛圆满落幕！
2、 速来！2023第三届3D视觉创新应用竞赛决赛即将开启！
3、DeepMIM：MIM中引入深度监督方法​
4、SPM: 一种即插即用的形状先验模块!