使用kitti数据集实现自动驾驶——发布照片、点云、IMU、GPS、显示2D和3D侦测框

语言: CN / TW / HK

持续创作,加速成长!这是我参与「掘金日新计划 · 6 月更文挑战」的第13天,点击查看活动详情

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

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

视频

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

1、准备工作

1.1数据集下载

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

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

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219110748221.png" alt="image-20211219110748221" style="zoom:50%;" />

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

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OnhRasMG-1639920546758)(C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219111558235.png)]

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

  • 创建功能包

c cd ~/catkin_ws/src catkin_create_pkg kitti_turtorial rospy

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

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219204219255.png" alt="image-20211219204219255" style="zoom:67%;" />

2、详细步骤

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

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

   

2.1 发布照片

```python

创建一个摄像头的发布者

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

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jcSwXiK1-1639920546763)(C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203514146.png)]

2.2 发布点云

```python

创建一个点云的发布者

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

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203024911.png" alt="image-20211219203024911" style="zoom:67%;" />

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

```python

创建一个汽车的发布者

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

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203102041.png" alt="image-20211219203102041" style="zoom:67%;" />

2.4 发布IMU

```python

创建一个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)

```

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203127402.png" alt="image-20211219203127402" style="zoom:67%;" />

2.5 发布GPS

```python

创建一个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)

```

2.6 在rviz上显示2D侦测框

```python

读取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")) ```

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VlxeydG9-1639920546766)(C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203301610.png)]

2.7 在rviz上显示3D 侦测框

```python #读取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)

```

<img src="C:\Users\WSJ\AppData\Roaming\Typora\typora-user-images\image-20211219203355050.png" alt="image-20211219203355050" style="zoom:67%;" />

3、代码合集

代码托管在Gitee上,自行下载:代码

   

咻咻咻咻~~duang\~~点个赞呗