環境：ubuntu16.04，ros-kinetic，python2，vscode，opencv，rviz
概要：這節課筆記，接着上一節課的筆記，給所有偵測盒添加軌迹顯示，注意無人車的軌迹顯示也是有的。
源碼是承接之前課程新增内容所得的，操作以及解析可以參考之前blog～
資料準備及預處理可參考部落格，
https://blog.csdn.net/qq_45701501/article/details/116447770
tracking資料準備：
https://blog.csdn.net/qq_45701501/article/details/116586427
1、思路
上一節課筆記說的是，釋出無人車自身軌迹在rviz顯示，而這節課是給所有偵測盒物體和無人車軌迹顯示。老師采用的思路是，在釋出無人車的軌迹函數裡面增加一個id參數，根據id來進行繪制軌迹。注意在id擷取函數裡面手動添加無人車id，否則無人車軌迹是無法顯示的。
2、源碼
四個檔案：讀取資料檔案data_utils.py,釋出函數檔案publish_utils.py,将3d偵測框從相機坐标系轉為雷達坐标系顯示檔案kitti_utils.py,主函數檔案p21_kitti.py.
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)}#顔色字典
#車頭朝前，左上點為0,順時針，0,1,2，3四個點，頂部同樣順時針，依次為（0頂部）4,5,6,7
#偵測盒資料，連線順序
LINES = [[0, 1], [1, 2], [2, 3], [3, 0]] # lower face
LINES+= [[4, 5], [5, 6], [6, 7], [7, 4]] #upper face
LINES+= [[4, 0], [5, 1], [6, 2], [7, 3]] #connect lower face and upper face
LINES+= [[4, 1], [5, 0]] #front face 對角線表示叉叉以表示正前方
#偵測盒存在的時長
LIFETIME = 0.1
#釋出圖檔函數
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)
#釋出偵測盒函數
#def publish_3dbox(box3d_pub,corners_3d_velos):#偵測盒顔色一緻寫法
#def publish_3dbox(box3d_pub,corners_3d_velos,types):#types指定物體種類以表示不同顔色
def publish_3dbox(box3d_pub,corners_3d_velos,types,track_ids):#再增加track_id參數
marker_array=MarkerArray()#把所有marker放在一起釋出
for i,corners_3d_velo in enumerate(corners_3d_velos):#對每個頂點建立marker
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp =rospy.Time.now()
marker.id =i
marker.action = Marker.ADD
#由于車子一直在運動，0.1秒會更新一次，是以偵測盒更新時間為LIFETIME=0.1秒，防止偵測盒一直存在
marker.lifetime =rospy.Duration(LIFETIME)
marker.type = Marker.LINE_LIST
# marker.color.r = 0.0#這幾行表示釋出的偵查盒顔色都一樣的
# marker.color.g = 1.0
# marker.color.b = 1.0
b, g, r = DETECTION_COLOR_DICT[types[i]]#根據不同類型，偵測盒顔色給不一樣
marker.color.r = r/255.0 #由于是python2,是以需要加.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:#給8個頂點指定連線順序，上面有定義
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)
#track_id的marker
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i +1000 #i和上面定義一緻，保證釋出正常顯示
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING #TEXT表示文字，VIEW_FACING表示一直朝向你觀看方向
#p4 = corners_3d_velo[4]#upper front left corner定義設定的marker位置,這裡表示上左角
p4 = np.mean(corners_3d_velo,axis=0)#axis=0表示取的是垂直方向的軸的平均，是的顯示在偵測盒中心上方
text_marker.pose.position.x = p4[0]
text_marker.pose.position.y = p4[1]
text_marker.pose.position.z = p4[2] + 1 #讓track_id顯示在偵測盒上方
text_marker.text = str(track_ids[i]) #指定marker顯示文字内容，str将track_id内容轉換為string類型才行顯示
#指定marker大小
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
b, g, r = DETECTION_COLOR_DICT[types[i]] #track_id文字顯示顔色根據物體種類顯示
text_marker.color.r = r/255.0
text_marker.color.g = g/255.0
text_marker.color.b = b/255.0
text_marker.color.a = 1.0
marker_array.markers.append(text_marker)
box3d_pub.publish(marker_array)#釋出
#釋出物體軌迹
def publish_loc(loc_pub,tracker,centers):
marker_array = MarkerArray()
for track_id in centers:#表示隻是顯示在目前幀所偵測到的物體的軌迹
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp =rospy.Time.now()
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.id = track_id#表示每個物體有不同的id
#定義車子軌迹顔色以及大小
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.points = []
for p in tracker[track_id].locations:#把所有的點連接配接起來
marker.points.append(Point(p[0],p[1],0))
marker_array.markers.append(marker)
loc_pub.publish(marker_array)#把物體軌迹釋出出去
kitti_utils.py：
（這個檔案和連結裡面一樣的，copy一份就行了）
p21_kitti.py：
#!/usr/bin/env python
# -*- coding:utf8 -*-
import os
from collections import deque
from data_utils import *
from publish_utils import *
from kitti_utils import * #kitti_utils.py檔案有報錯，但是不影響運作
DATA_PATH='/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync'
#建立一個類别，用于實作不同物體的坐标轉換計算
class Object():
def __init__(self,center):
#self.locations = [] #儲存的是物體過去所有的軌迹
self.locations = deque(maxlen=20)#方法2：指定過去軌迹點，最大儲存20個
self.locations.appendleft(center)
def update(self,center,displacement,yaw): #更新自己本身過去的位置，過去軌迹，移動量，旋轉角;
#其中，移動量和旋轉角跟具體物體無關，都隻是關系到自身移動量或者旋轉角，是以跟其他物體無關，是以都是同一個值（個人有點迷）
for i in range(len(self.locations)):#對每一幀坐标進行轉換
x0,y0 = self.locations[i]#初始位置坐标
#坐标轉換後的新坐标
x1 = x0*np.cos(yaw_change)+y0*np.sin(yaw_change)-displacement
y1 = -x0*np.sin(yaw_change)+y0*np.cos(yaw_change)
self.locations[i] = np.array([x1,y1])#将原來第i個坐标值更新為新坐标系下的值
#self.locations += [np.array([0,0])] #增加車子自身目前所觀察到的位置量，
#由于車子會一直在坐标系中[0,0] 位置，是以一直加的是[0,0]位置
#self.locations.appendleft(np.array([0,0]))#增加的自身觀測量，儲存在隊列的左邊
#self.locations.appendleft(center)#由于其他物體的中心點不一定在（0,0）位置，是以寫的是目前物體的中心center
#self.locations =self.locations[:20]#方法1：每次隻是顯示前20個點，以免周期内，軌迹都全部顯示，很長也難看;測試有問題，隻是最初20個而已
if center is not None:#如果是被遮擋的物體，不用給加到所顯示軌迹裡面，也就是說，遮擋了，軌迹顯示暫停至已有的
self.locations.appendleft(center)
def reset(self):#循環播放時，把過去的所有軌迹清空
#self.locations = []
self.locations = deque(maxlen=20)#這裡是增加了deque限制軌迹顯示長度的重置方式寫法
#3d偵測盒生成函數
#以特殊情況為例，當rot_y=0時，(pos_x,pos_y,pos_z)就是位于偵測盒的下方平面的中心點
#根據資料中的長寬，可以擷取下方平面的四角坐标，然後根據高資料，進而擷取偵測盒的八個點的坐标
#對于rot_y!=0情況，需要每個點乘以一個旋轉矩陣（對相機坐标系中的y軸進行旋轉），那麼就可以得到
#帶有rot_y!=0也就是yaw非0情況，8個頂點坐标（yaw=0情況時）乘以旋轉矩陣，可得到新的8個頂點坐标
def compute_3d_box_cam2(h,w,l,x,y,z,yaw):
#return:3xn in can2 coordinate
#rot_y!=0時的旋轉矩陣
R = np.array([[np.cos(yaw),0,np.sin(yaw)],[0,1,0],[-np.sin(yaw),0,np.cos(yaw)]])
#8個頂點所對應的xyz坐标（rot_y=0時）
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
y_corners = [0,0,0,0,-h,-h,-h,-h]
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
#做旋轉，rot_y=0可視為旋轉特例，隻不過角度為0而已，然後，讓8個頂點坐标與旋轉矩陣相乘
corners_3d_cam2 = np.dot(R,np.vstack([x_corners,y_corners,z_corners]))
#由于以下方中心點做旋轉的，是以，需要加上該旋轉中心點坐标（x,y,z）
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2#傳回偵測盒8個頂點在相機坐标系中的坐标
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裡面固定衛星偵測資料包
box3d_pub=rospy.Publisher('kitti_3d',MarkerArray,queue_size=10)#建立釋出偵測盒的topic
loc_pub = rospy.Publisher('kitti_loc',MarkerArray,queue_size=10)#建立釋出車子本身軌迹釋出topic
bridge=CvBridge()
rate=rospy.Rate(10)
#讀取tracking資料
df_tracking=read_tracking('/home/ylh/data/kitti/training/label_02/0000.txt')
#讀取坐标轉換檔案，from_video=True表示會讀取路徑中三個.txt坐标轉換檔案
calib = Calibration('/home/ylh/data/kitti/RawData/2011_09_26/',from_video=True)
#釋出車子自身軌迹
#ego_car = Object()
#釋出所有物體軌迹
#tracker記錄的是從過去到現在的所有物體;centers記錄的是，目前幀所檢測到的物體
tracker = {} #track_id:Object,物體和id對應上
prev_imu_data = None #最初的imu資料是沒有的
while not rospy.is_shutdown():
#将tracking資料的繪制框框所需資料篩選并處理
df_tracking_frame = df_tracking[df_tracking.frame==frame]
boxes_2d = np.array(df_tracking_frame[['bbox_left','bbox_top','bbox_right','bbox_bottom']])#擷取tracking資料第frame幀圖檔中的box們對應的四邊坐标
types=np.array(df_tracking_frame['type'])#讀取tracking資料第frame幀圖檔中的物體種類類型并儲存到tpyes數組中
#讀取tracking裡面偵測盒參數
boxes_3d = np.array(df_tracking_frame[['height','width','length','pos_x','pos_y','pos_z','rot_y']])
#擷取track_id
track_ids = np.array(df_tracking_frame['track_id'])
corners_3d_velos = []#存放偵測盒8個頂點資料
#計算每個物體中心點位置
centers = {} #track_id : center，注意centers記錄的是，目前幀所檢測到的物體
for track_id,box_3d in zip(track_ids,boxes_3d):#根據資料生成所有偵測盒
corners_3d_cam2 = compute_3d_box_cam2(*box_3d)#由于該函數有7個參數，是以使用星号自動展開;計算擷取偵測盒8個頂點坐标
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)#把8個頂點，從相機坐标系裝換到雷達坐标系
corners_3d_velos += [corners_3d_velo]#存放所有偵測盒8頂點資料，corners_3d_velo是8x3矩陣：8頂點，xyz坐标
centers[track_id] = np.mean(corners_3d_velo,axis=0)[:2]#對每個物體進行垂直方向取平均,擷取中心點（xyz）;[:2]表示取的是xy的平均，這是因為考慮的是俯視圖
#這裡的centers隻是儲存其他物體，而對于車子本身并沒有儲存，是以沒法顯示出來
#為了解決沒法顯示自身的問題，這裡就是把自身中心（0,0）添加進去，由于車子自身會一直在目前坐标的原點處
centers[-1] = np.array([0,0])#這裡的-1的目的是，避免無人車id和其他的id重複
#讀取圖檔
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_2d,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)
#釋出偵測盒
#publish_3dbox(box3d_pub,corners_3d_velos)#偵測盒顔色一緻寫法
#publish_3dbox(box3d_pub,corners_3d_velos,types) #增加偵測盒類型不同而不一樣寫法
publish_3dbox(box3d_pub,corners_3d_velos,types,track_ids) #增加傳遞track_id
#釋出所有物體軌迹
if prev_imu_data is None:#如果前一幀imu資料不為空，則計算距離
for track_id in centers:#對各個物體進行執行個體化，也就是建構物體，同時與id比對上
tracker[track_id] = Object(centers[track_id])#執行個體化
else:
displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])#計算速度的矢量和，然後乘以0.1秒（一幀0.1秒），得到兩幀間距離,又稱移動量
yaw_change = float(imu_data.yaw-prev_imu_data.yaw)#計算兩幀之間的轉角
for track_id in centers:#對所檢測到的物體進行更新
if track_id in tracker:#表示已經被偵測過的物體
tracker[track_id].update(centers[track_id],displacement,yaw_change)#更新物體過去所有軌迹
else:#表示該物體第一次被偵測到的
tracker[track_id] = Object(centers[track_id])#新物體執行個體化她，而不是更新她過去軌迹
for track_id in tracker:#表示過去被偵測到的物體，目前幀被遮擋，同樣需要更新該物體軌迹
if track_id not in centers:
tracker[track_id].update(None,displacement,yaw_change)#更新物體過去所有軌迹;由于該物體被遮擋，中心點未知，是以給None
prev_imu_data = imu_data #更新過去imu資料
#publish_loc(loc_pub,ego_car.locations)#釋出車子本身軌迹
publish_loc(loc_pub,tracker,centers)#增加參數，已偵測過的物體tracker，目前幀所有偵測到的物體
#釋出
rospy.loginfo("published")
rate.sleep()
frame+=1
#frame%=154#實作循環播放
if frame == 154:#實作循環播放
frame = 0
#ego_car.reset()#實作循環開始，之前軌迹清空
for track_id in tracker:#表示在tracker裡面的物件，循環開始時，軌迹全部重置
tracker[track_id].reset()
3、效果
圖中不僅無人車本身有軌迹顯示，而且其他活動偵測盒也是有的，這表示偵測盒和無人車軌迹在rviz中顯示成功
4、思考
看過老師整個課程應該知道，後面還有一個關于偵測盒物體與無人車的實時距離顯示内容。個人邏輯以及代碼邏輯能了解，但代碼在jupyter notebook一直複現失敗，是以，等我有空複現再更新了。如果有小夥伴解決了，麻煩@一下哈，謝謝～
這系列課程筆記基本更新完成了，後面有時間的話，再把前面學習過程中的數學原理更新或者把自己個人改造折騰更新上來。
#####################
學習課程來源up主，AI葵：
https://www.youtube.com/watch?v=TBdcwwr5Wyk
緻謝AI葵老師
不積矽步，無以至千裡
好記性不如爛筆頭
感覺有點收獲的話，麻煩大大們點贊收藏哈
![Stereo Matching文獻筆記之（九）：經典算法Semi-Global Matching（SGM）之神奇的HMI代價計算~[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)