kitti之ros可視化_學習筆記--第8課：tracking資料下載下傳及2d偵測框繪制

環境：ubuntu16.04，ros-kinetic，python2，vscode，opencv，rviz

概要：這節課筆記，新增展示的是，介紹的是下載下傳繪制2号相機所需要的tracking資料，并使用tracking資料，先給圖檔中一個貨車繪制一個淺藍色的2d偵測框，最後統一給每張圖檔根據物體類型不同繪制不同顔色2d偵測框。

資料準備及預處理可參考部落格，

https://blog.csdn.net/qq_45701501/article/details/116447770

0、tracking資料下載下傳和一些注意問題

1）tracking資料簡單作用：加載人工加注資料用以提供給機器學習，如自動駕駛的物體偵測訓練

1）tracking資料下載下傳：

http://www.cvlibs.net/datasets/kitti/eval_tracking.php

資料名稱：

Download training labels of tracking data set (9 MB)

填寫郵箱，系統發送郵件，官方會發送下載下傳連結到你郵箱，這裡是個人之前擷取下載下傳連結 ：

https://s3.eu-central-1.amazonaws.com/avg-kitti/data_tracking_label_2.zip

資料儲存位置參考：壓縮包-解壓-放到kitti資料集的RawData同級目錄中，如：

training就是下載下傳壓縮包，解壓後的檔案夾。

2）這個tracking資料隻是包含2号相機，如果前面小夥伴選了其他相機的，注意把相機設定為2号，才能使用這個資料，具體說明可以看連結：

https://github.com/pratikac/kitti/blob/master/readme.tracking.txt
           

3）使用jupyter notebook工具

安裝指令：

sudo pip install jupyter notebook

使用時，終端輸入：

jupyter notebook

在網頁中生成一個界面，進入所在包src檔案中，按鈕建立一個

python2.0

檔案。

注意，網頁使用讀取資料時候，終端一定要求保持打開狀态。填寫路徑時，最好采用的

pwd

指令擷取

1、源碼及解析

1）jupyter notebook預測試2d偵測框：

以下代碼，均直接放到jupyter notebook裡面，點選run即可。注意，不同子產品，放到不同的欄中。

1-1）單張圖檔某一個 車子的2d偵測框繪制

import pandas as pd
import numpy as np
#資料機關
COLUMN_NAMES=['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
#讀取tracking資料并處理
df=pd.read_csv('/home/ylh/data/kitti/training/label_02/0000.txt',header=None,sep=' ')
df.columns=COLUMN_NAMES#給讀取的資料添加機關
df.head()

#查找原始資料'Truck','Van','Tram'類型汽車，都更改類型為'Car'，得到表1
df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'
#對表1,查找'Car','Pedestrian','Cyclist'三種類型物體，得到表2
df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]

#表2,對2号資料，注意不是id為2，進行讀取所示類型的資料，并以陣列形式展示
#df.loc[2,['bbox_left','bbox_top','bbox_right','bbox_bottom']]
#将陣列顯示的資料，儲存到box的numpy數組裡面
box=np.array(df.loc[2,['bbox_left','bbox_top','bbox_right','bbox_bottom']])

import cv2

#讀取圖檔
image = cv2.imread('/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync/image_02/data/0000000000.png')

#這裡展示的給圖檔中一個貨車繪制淺藍色的框框
#由于像素隻有整數，是以需要對box數組元素進行int類型轉換;根據坐标值，繪制左上角點
top_left=int(box[0]),int(box[1])
#根據坐标值，繪制右下角的點
bottom_right=int(box[2]),int(box[3])

#指定圖檔，根據左上角點和右下角點繪制框框，(255,255,0)表示框框的顔色,2表示框線的寬度
cv2.rectangle(image,top_left,bottom_right,(255,255,0),2)
cv2.imshow('image',image)#顯示圖檔
cv2.waitKey(0)#設定圖檔顯示，避免一顯示就消失情況
cv2.destroyAllWindows()#最後銷毀該圖檔視窗
           

效果：

1-2）單張圖檔多個物體的2d偵測框繪制

import pandas as pd
import numpy as np
COLUMN_NAMES=['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
df=pd.read_csv('/home/ylh/data/kitti/training/label_02/0000.txt',header=None,sep=' ')
df.columns=COLUMN_NAMES
df.head()

df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'#将這三種車子，統一定義為Car
df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]#隻是擷取資料集中類型為指定的資料，注意car為重定義類型

#box=np.array(df.loc[2,['bbox_left','bbox_top','bbox_right','bbox_bottom']])#把2号資料四個邊坐标儲存到numpy陣列裡面

import cv2

image = cv2.imread('/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync/image_02/data/0000000000.png')#讀取圖檔

boxes = np.array(df[df.frame==0][['bbox_left','bbox_top','bbox_right','bbox_bottom']])#擷取第0幀圖檔中的box們對應的四邊坐标

for box in boxes:#給每個box繪制圖線
    top_left=int(box[0]),int(box[1])#box的左上角點，像素為整數，是以需要轉換int類型
    bottom_right=int(box[2]),int(box[3])#box的右下角點
    cv2.rectangle(image,top_left,bottom_right,(255,255,0),2)#繪制框框，依次指定圖檔、左上角點、右下角點、顔色、線粗細
    
cv2.imshow('image',image)#顯示框框
cv2.waitKey(0)#避免框框一顯示就消失，也就是一直顯示
cv2.destroyAllWindows()#結束就銷毀所有視窗
           

效果：

1-3) 單張圖檔多個物體的2d偵測框根據類型而異顔色框框繪制

DETECTION_COLOR_DICT = {'Car':(255,255,0),'Pedestrian':(0,226,255),'Cyclist':(141,40,255)}#顔色字典
import pandas as pd
import numpy as np
COLUMN_NAMES=['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']
df=pd.read_csv('/home/ylh/data/kitti/training/label_02/0000.txt',header=None,sep=' ')
df.columns=COLUMN_NAMES
df.head()

df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'#将這三種車子，統一定義為Car
df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]#隻是擷取資料集中類型為指定的資料，注意car為重定義類型

#box=np.array(df.loc[2,['bbox_left','bbox_top','bbox_right','bbox_bottom']])#把2号資料四個邊坐标儲存到numpy陣列裡面

import cv2

image = cv2.imread('/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync/image_02/data/0000000000.png')#讀取圖檔

boxes = np.array(df[df.frame==0][['bbox_left','bbox_top','bbox_right','bbox_bottom']])#擷取第0幀圖檔中的box們對應的四邊坐标
types=np.array(df[df.frame==0]['type'])#讀取圖檔中的種類類型并儲存到tpyes數組中

#zip(types,boxes)[0]#兩個數組的第一筆資料合并在一起
#for box in boxes:#給每個box繪制圖線

for typ,box in zip(types,boxes):#給對應類型每個box繪制對應顔色圖線
    top_left=int(box[0]),int(box[1])#box的左上角點，像素為整數，是以需要轉換int類型
    bottom_right=int(box[2]),int(box[3])#box的右下角點
    cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2)#繪制框框，依次指定圖檔、左上角點、右下角點、根據類型不同給的顔色（bgr）、線粗細
    
cv2.imshow('image',image)#顯示框框
cv2.waitKey(0)#避免框框一顯示就消失，也就是一直顯示
cv2.destroyAllWindows()#結束就銷毀所有視窗
           

效果：

當然，這裡統一讀取的是0000000000.png圖檔，如果想看其他圖檔效果的，可以根據圖檔資料集，更改路徑上圖檔名稱即可，這裡不再展示。

2）連續幀圖檔，多個物體2d偵測框在rviz效果

通過jupyter notebook的預測試，下面将上述的代碼移植到下面三個檔案中，目的在rviz顯示連續幀不同類型不同顔色偵測框顯示。

對于讀取資料檔案，

data_utils.py

：

#!/usr/bin/env python
# -*- coding:utf8 -*-

import cv2
import numpy as np
import os
import pandas as pd #用于讀取imu資料

IMU_COLUMN_NAMES = ['lat','lon','alt','roll','pitch','yaw','vn','ve','vf','vl','vu',
                    'ax','ay','az','af','al','au','wx','wy','wz','wf','wl','wu',
                    'posacc','velacc','navstat','numsats','posmode','velmode','orimode'
                    ]#根據kitti資料集中的名稱進行定義的，個人了解是對照c裡面的宏定義

TRACKING_COLUMN_NAMES=['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 
                'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 
                'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']#tracking資料機關


#讀取圖檔路徑函數
def read_camera(path):
    return cv2.imread(path)

#讀取點雲路徑函數
def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

#讀取imu資料
def read_imu(path):
    df=pd.read_csv(path,header=None,sep=' ')#讀取資料
    df.columns=IMU_COLUMN_NAMES#給資料賦予機關
    return df

#讀取trackiing資料
def read_tracking(path):
    df=pd.read_csv(path,header=None,sep=' ')#讀取tracking資料
    df.columns=TRACKING_COLUMN_NAMES#給資料資料添加機關
    df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'#将這三種車子，統一定義為Car
    df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]#隻是擷取資料集中類型為指定的資料，注意car為重定義類型
    return df#傳回讀取的資料
           

對于釋出函數定義檔案，

publish_utils.py

：

#!/usr/bin/env python
# -*- coding:utf8 -*-

import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray#Marker繪制相機視野訓示線子產品,MarkerArray解決Marker帶來釋出的不同步問題
from sensor_msgs.msg import Image,PointCloud2,Imu,NavSatFix
from geometry_msgs.msg import Point#Point來自ros包定義，是以需要定義;若不清楚，則需要到ros官網上面檢視具體那個包
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import numpy as np
import tf
import cv2

FRAME_ID='map'
DETECTION_COLOR_DICT = {'Car':(255,255,0),'Pedestrian':(0,226,255),'Cyclist':(141,40,255)}#顔色字典

#釋出圖檔函數
def publish_camera(cam_pub,bridge,image,boxes,types):#增加參數boxes、types
    #繪制框框到圖檔中
    for typ,box in zip(types,boxes):#給對應類型每個box繪制對應顔色圖線
        top_left=int(box[0]),int(box[1])#box的左上角點，像素為整數，是以需要轉換int類型
        bottom_right=int(box[2]),int(box[3])#box的右下角點
        #繪制框框，依次指定圖檔、左上角點、右下角點、根據類型不同給的顔色（bgr）、線粗細
        cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2)
    cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))

#釋出點雲函數
def publish_point_cloud(pcl_pub,point_clond):
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))

#釋出相機視野以及車子模型marker函數
def publish_ego_car(ego_car_pub):
#publish left and right 45 degree FOV lines and ego car model mesh
    
    marker_array=MarkerArray()#解決marker釋出不同步問題

    marker=Marker()
    marker.header.frame_id=FRAME_ID
    marker.header.stamp=rospy.Time.now()

    marker.id=0#每個marker隻能有一個id，有重複的id，隻會顯示一個
    marker.action=Marker.ADD#表示添加marker
    marker.lifetime=rospy.Duration()#lifetime表示marker在畫面中顯示的時長;Duration()函數，不給任何參數時，表示一直存在
    marker.type=Marker.LINE_STRIP#所釋出marker的類型

    #設定訓示線顔色
    marker.color.r=0.0
    marker.color.g=1.0
    marker.color.b=0.0
    marker.color.a=1.0#透明度，1表示完全不透明
    marker.scale.x=0.2#大小，這裡表示線的粗細

    #根據雷射點雲的坐标系來定義2号相機的視野範圍
    marker.points=[]
    marker.points.append(Point(10,-10,0))#Point,屬于ros的資料包裡面的定義，是以需要導入
    marker.points.append(Point(0,0,0))
    marker.points.append(Point(10,10,0))

    marker_array.markers.append(marker)#将訓示線marker放到MarkerArray中

    #釋出車子外形函數
    mesh_marker=Marker()
    mesh_marker.header.frame_id=FRAME_ID
    mesh_marker.header.stamp=rospy.Time.now()

    mesh_marker.id=-1#id隻能設定整數，不能設定帶有小數的
    mesh_marker.lifetime=rospy.Duration()
    mesh_marker.type=Marker.MESH_RESOURCE#這裡的MESH_RESOURCE表示導入的是3d模型
    mesh_marker.mesh_resource="package://kitti_tutorial/Audi R8/Models/Audi R8.dae"#下載下傳的dae模型存在問題，隻是顯示部分

    #設定模型位置
    mesh_marker.pose.position.x=0.0
    mesh_marker.pose.position.y=0.0
    mesh_marker.pose.position.z=-1.73#這裡負數，是因為以雷射雷達坐标系而定義的，1.73是根據官方釋出的位置定義所取的

    #設計車子模型的旋轉量
    q=tf.transformations.quaternion_from_euler(0,0,np.pi/2)#(np.pi/2,0,np.pi)這裡根據下載下傳的車子模型進行調整
    mesh_marker.pose.orientation.x=q[0]
    mesh_marker.pose.orientation.y=q[1]
    mesh_marker.pose.orientation.z=q[2]
    mesh_marker.pose.orientation.w=q[3]

    #設定車子模型的顔色
    mesh_marker.color.r=1.0
    mesh_marker.color.g=1.0
    mesh_marker.color.b=1.0
    mesh_marker.color.a=1.0

    #設定車子模型的大小
    mesh_marker.scale.x=0.6
    mesh_marker.scale.y=0.6
    mesh_marker.scale.z=0.6

    marker_array.markers.append(mesh_marker)#将車子marker放到MarkerArray中

    ego_car_pub.publish(marker_array)

#釋出imu資料函數
def publish_imu(imu_pub,imu_data):
    imu=Imu()#ros,imu 進行google可以檢視文檔說明
    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))#(np.pi/2,0,np.pi)這裡根據下載下傳的車子模型進行調整
    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#根據雷達坐标系，确定x方向線性加速度
    imu.linear_acceleration.y=imu_data.al#根據雷達坐标系，确定y方向線性加速度
    imu.linear_acceleration.z=imu_data.au#根據雷達坐标系，确定z方向線性加速度
    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)

#釋出gps資料函數
def publish_gps(gps_pub,imu_data):
    gps=NavSatFix()#ros裡面對于gps資料識别包
    gps.header.frame_id=FRAME_ID
    gps.header.stamp=rospy.Time.now()

    gps.latitude=imu_data.lat#緯度
    gps.longitude=imu_data.lon#經度
    gps.altitude=imu_data.alt#海拔

    gps_pub.publish(gps)
           

對于執行檔案，

p11_kitti.py

：

#!/usr/bin/env python
# -*- coding:utf8 -*-

from data_utils import *
from publish_utils import *

DATA_PATH='/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync'

if __name__=='__main__':
    frame = 0
    rospy.init_node('kitti_node',anonymous=True)
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)#建立釋出圖檔topic
    pcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#建立釋出點雲topic
    #ego_pub=rospy.Publisher('kitti_ego_car',Marker,queue_size=10)#建立釋出訓示線marker的topic
    ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)#MarkerArray方式釋出
    #model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)#建立釋出車子模型的marker的topic
    imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)#建立釋出imu資料的topic
    gps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)#建立釋出gps資料的topic，NavSatFix,ros裡面固定衛星偵測

    bridge=CvBridge()

    rate=rospy.Rate(10)

    #讀取tracking資料
    df_tracking=read_tracking('/home/ylh/data/kitti/training/label_02/0000.txt')


    while not rospy.is_shutdown():
        #将tracking資料的繪制框框所需資料篩選并處理
        boxes = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])#擷取tracking資料第frame幀圖檔中的box們對應的四邊坐标
        types=np.array(df_tracking[df_tracking.frame==frame]['type'])#讀取tracking資料第frame幀圖檔中的物體種類類型并儲存到tpyes數組中
        #讀取圖檔
        image=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        
        #釋出圖檔
        #publish_camera(cam_pub,bridge,image)
        publish_camera(cam_pub,bridge,image,boxes,types)#增加參數boxes,types，為了給圖檔指定類型繪制框框     
        
        #讀取點雲
        point_clond=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))

        #釋出點雲
        publish_point_cloud(pcl_pub,point_clond)

        #釋出訓示線marker;由于不需要讀取資料，是以直接釋出即可
        #當采用markerarray釋出方式，則車子和訓示線都放在這個topic
        #進行釋出即可。故下面的釋出車子模型marker可以删除。這樣子，可以解決不同marker釋出不同步問題
        publish_ego_car(ego_pub)

        #釋出車子模型marker;由于不需要讀取資料，是以直接釋出即可
        #publish_car_model(model_pub)

        #讀取imu資料，這裡也包含了gps資料了
        imu_data=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))

        #釋出imu資料
        publish_imu(imu_pub,imu_data)

        #釋出gps資料
        publish_gps(gps_pub,imu_data)
        
        #釋出
        rospy.loginfo("published")
        rate.sleep()
        frame+=1
        frame%=154
           

2、效果展示

包建立、源檔案儲存位置，運作編譯，rviz添加新topic，這裡不再羅嗦了，有不了解的，可以參考這系列之前部落格内容。

那麼，啟動rviz，可以發現，播放視訊裡面，有了連續的2d偵測框框，而且，根據物體類型不同，框框顯示不同顔色。具體效果如圖中左下方。

3、一些想法

1）檔案編寫時候，一定要注意檔案路徑的編寫正确性，個人建議終端使用

pwd

指令擷取最有保障。

2）在讀取tracking資料時候，發現隻是讀取traning檔案夾裡面的0000.txt，個人感覺這是因為這一個檔案已經包含繪制框框所需要的全部資訊了，覺得後面有時間可以查一下或者測試一下。

至此，kitti資料集中圖檔資料，根據不同類型繪制不同顔色偵測框，釋出到ros中，在rviz顯示任務完成～

#####################

學習課程來源up主，AI葵：

https://www.youtube.com/watch?v=TBdcwwr5Wyk

緻謝AI葵老師

不積矽步，無以至千裡

好記性不如爛筆頭

感覺有點收獲的話，麻煩大大們點贊收藏哈