kitti之ros可视化_学习笔记--第12课：无人车和侦测盒物体的轨迹显示

环境：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一份就行了）

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

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葵老师
不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈