kitti之ros可视化_学习笔记--第11课：无人车本身轨迹添加

环境：.04，ros-，，，，rviz

概要：这节课笔记，新增展示的是，给无人车本身添加运动轨迹显示。

源码是承接之前课程新增内容所得的，操作以及解析可以参考之前blog～

资料准备及预处理可参考博客，

资料准备：

1、思路

包存储位置、创建、编译、运行这些参考本人这系列前面的博客。

思路：

0）源码里面计算坐标，比如两帧的平移量、旋转角，来源数据集里面的imu资料;

1）坐标转换时，注意只是考虑俯视图情况，也就是将3d情况转为2d情况来降低思考难度;

2）无人车过去所有帧的坐标都变为当前帧和上一帧的坐标系考虑，最后都是转为当前帧坐标系表示;

3）坐标转换时，对于既有平移又有旋转情况，先旋转再平移，也就是先乘以一个旋转矩阵，再乘以一个平移矩阵，达到所有坐标都转为当前坐标系表示。

注意：这里可能说得比较抽象，建议参考文末参考老师油管帐号课程链接或者b站搬运的视频

2、源码

四个文件：读取资料文件.py,发布函数文件.py,将3d侦测框从相机坐标系转为雷达坐标系显示文件.py,主函数文件.py.

.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'#将这三种车子，统一定义为Cardf=df[df.type.isin(['Car','Pedestrian','Cyclist'])]#只是获取数据集中类型为指定的数据，注意car为重定义类型return df#返回读取的资料

.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 cv2FRAME_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_IDpcl_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 meshmarker_array=MarkerArray()#解决marker发布不同步问题marker=Marker()marker.header.frame_id=FRAME_IDmarker.header.stamp=rospy.Time.now()marker.id=0#每个marker只能有一个id，有重复的id，只会显示一个marker.action=Marker.ADD#表示添加markermarker.lifetime=rospy.Duration()#lifetime表示marker在画面中显示的时长;Duration()函数，不给任何参数时，表示一直存在marker.type=Marker.LINE_STRIP#所发布marker的类型#设定指示线颜色marker.color.r=0.0marker.color.g=1.0marker.color.b=0.0marker.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_IDmesh_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.0mesh_marker.pose.position.y=0.0mesh_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.0mesh_marker.color.g=1.0mesh_marker.color.b=1.0mesh_marker.color.a=1.0#设置车子模型的大小mesh_marker.scale.x=0.6mesh_marker.scale.y=0.6mesh_marker.scale.z=0.6marker_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_IDimu.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.wlimu.angular_velocity.z=imu_data.wuimu_pub.publish(imu)#发布gps资料函数
def publish_gps(gps_pub,imu_data):gps=NavSatFix()#ros里面对于gps资料识别包gps.header.frame_id=FRAME_IDgps.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):#对每个顶点建立markermarker = Marker()marker.header.frame_id = FRAME_IDmarker.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.0b, g, r = DETECTION_COLOR_DICT[types[i]]#根据不同类型，侦测盒颜色给不一样marker.color.r = r/255.0   #由于是python2,所以需要加.0才会做小数点除法marker.color.g = g/255.0marker.color.b = b/255.0marker.color.a = 1.0marker.scale.x = 0.1marker.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的markertext_marker = Marker()text_marker.header.frame_id = FRAME_IDtext_marker.header.stamp = rospy.Time.now()text_marker.id = i +1000 #i和上面定义一致，保证发布正常显示text_marker.action = Marker.ADDtext_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 = 1text_marker.scale.y = 1text_marker.scale.z = 1b, g, r = DETECTION_COLOR_DICT[types[i]] #track_id文字显示颜色根据物体种类显示text_marker.color.r = r/255.0text_marker.color.g = g/255.0text_marker.color.b = b/255.0text_marker.color.a = 1.0marker_array.markers.append(text_marker)box3d_pub.publish(marker_array)#发布#发布车子本身轨迹
def publish_loc(loc_pub,locations):marker_array = MarkerArray()marker = Marker()marker.header.frame_id = FRAME_IDmarker.header.stamp =rospy.Time.now()marker.action = Marker.ADDmarker.type = Marker.LINE_STRIP#定义车子轨迹颜色以及大小marker.color.r = 1.0marker.color.g = 0.0marker.color.b = 0.0marker.color.a = 1.0marker.scale.x = 0.2marker.points = []for p in locations:#把所有的点连接起来marker.points.append(Point(p[0],p[1],0))marker_array.markers.append(marker)loc_pub.publish(marker_array)#把车子自身轨迹发布出去

.py：

（这个文件和链接里面一样的，copy一份就行了）

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

.py：

#!/usr/bin/env python
# -*- coding:utf8 -*-import os
from collections import dequefrom 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):#self.locations = [] #保存的是物体过去所有的轨迹self.locations = deque(maxlen=20)#方法2：指定过去轨迹点，最大保存20个def update(self,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)-displacementy1 = -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 =self.locations[:20]#方法1：每次只是显示前20个点，以免周期内，轨迹都全部显示，很长也难看;测试有问题，只是最初20个而已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 = 0rospy.init_node('kitti_node',anonymous=True)cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)#建立发布图片topicpcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#建立发布点云topic#ego_pub=rospy.Publisher('kitti_ego_car',Marker,queue_size=10)#建立发布指示线marker的topicego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)#MarkerArray方式发布#model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)#建立发布车子模型的marker的topicimu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)#建立发布imu资料的topicgps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)#建立发布gps资料的topic，NavSatFix,ros里面固定卫星侦测资料包box3d_pub=rospy.Publisher('kitti_3d',MarkerArray,queue_size=10)#创建发布侦测盒的topicloc_pub = rospy.Publisher('kitti_loc',MarkerArray,queue_size=10)#建立发布车子本身轨迹发布topicbridge=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()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_idtrack_ids = np.array(df_tracking_frame['track_id'])corners_3d_velos = []#存放侦测盒8个顶点数据for box_3d in 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顶点数据#读取图片image=read_camera(os.path.join(DATA_PATH,'image_02/data/0d.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/0d.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/0d.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 not None:#如果前一帧imu资料不为空，则计算距离displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])#计算速度的矢量和，然后乘以0.1秒（一帧0.1秒），得到两帧间距离,又称移动量yaw_change = float(imu_data.yaw-prev_imu_data.yaw)#计算两帧之间的转角ego_car.update(displacement,yaw_change)#更新车子过去所有轨迹prev_imu_data = imu_data #更新过去imu数据publish_loc(loc_pub,ego_car.locations)#发布车子本身轨迹#发布rospy.loginfo("published")rate.sleep()frame+=1#frame%=154#实现循环播放if frame == 154:#实现循环播放frame = 0ego_car.reset()#实现循环开始，之前轨迹清空

3、效果

在rviz添加无人车轨迹topic后，可见如下效果：

红色尾巴表示无人车自身的轨迹

4、思考

1）看过AI葵老师视频都知道，关于每帧坐标数据获取，不仅可以使用imu数据，也是可以使用gps数据的，但是，个人运行代码是有bug的，原因不清楚。

2）案例里面使用imu数据原因，基于短距离内，imu数据比gps数据更准确。

3）关于轨迹发布函数，关键理解里面的将过去每帧坐标与当前帧关系理解为上一帧和当前帧关系;坐标转换，先旋转再平移，涉及线性代数里面的旋转矩阵和平移矩阵的知识。

4）使用队列depue来保证轨迹显示指定时长

至此，无人车自身的轨迹显示完成～

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

学习课程来源up主，AI葵：

致谢AI葵老师

不积硅步，无以至千里

好记性不如烂笔头

感觉有点收获的话，麻烦大大们点赞收藏哈