持續創作，加速成長！這是我參與「掘金日新計劃 · 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\~~點個讚唄

使用kitti數據集實現自動駕駛——發佈照片、點雲、IMU、GPS、顯示2D和3D偵測框

1、準備工作

1.1數據集下載

1.2 創建工作空間並建立一些文件

2、詳細步驟

2.1 發佈照片

創建一個攝像頭的發佈者

讀取攝像機數據

發佈數據

2.2 發佈點雲

創建一個點雲的發佈者

讀取點雲數據

發佈數據

2.3 畫出自己車子以及照相機視野

創建一個汽車的發佈者

發佈ego_car數據

繪製車子的照相機視野

2.4 發佈IMU

創建一個IMU發佈者

發佈imu數據

IMU發佈函數相關設置

2.5 發佈GPS

創建一個GPS發佈者

發佈GPS數據

GPS發佈函數相關設置

2.6 在rviz上顯示2D偵測框

讀取2D檢測框數據

2D框相關設置

2.7 在rviz上顯示3D 偵測框

3D框發佈函數相關設置

3、代碼合集