kitti之ros可视化_学习笔记--第10课：添加id到3d侦测盒上方
Posted 鸿_H
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了kitti之ros可视化_学习笔记--第10课：添加id到3d侦测盒上方相关的知识，希望对你有一定的参考价值。
环境：ubuntu16.04，ros-kinetic，python2，vscode，opencv，rviz
概要：这节课笔记，新增展示的是，在物体3d侦测盒上方显示id。
介绍3d侦测盒绘制，使用的参考资料以及原理介绍挺多的，个人建议，先看源码部分，再看jupyter notebook里面的原理介绍。
资料准备及预处理可参考博客，https://blog.csdn.net/qq_45701501/article/details/116447770
tracking资料准备：https://blog.csdn.net/qq_45701501/article/details/116586427
1、思路
包存储位置、创建、编译、运行这些参考本人这系列前面的博客。
主要思路：添加id给3d侦测盒，也就是从tracking数据集中，读取track_id，并转为np数组格式;给发布3d侦测盒函数添加一个用于传入id的形参，在函数体中添加用于显示id的marker。
2、源码
包含四个文件：读取资料文件data_utils.py,发布函数文件publish_utils.py,将3d侦测框从相机坐标系转为雷达坐标系显示文件kitti_utils.py,主函数文件p15_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)#发布
kitti_utils.py:
""" Helper methods for loading and parsing KITTI data.
Author: Charles R. Qi
Date: September 2017
"""
from __future__ import print_function
import numpy as np
import cv2
import os
class Object3d(object):
''' 3d object label '''
def __init__(self, label_file_line):
data = label_file_line.split(' ')
data[1:] = [float(x) for x in data[1:]]
# extract label, truncation, occlusion
self.type = data[0] # 'Car', 'Pedestrian', ...
self.truncation = data[1] # truncated pixel ratio [0..1]
self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown
self.alpha = data[3] # object observation angle [-pi..pi]
# extract 2d bounding box in 0-based coordinates
self.xmin = data[4] # left
self.ymin = data[5] # top
self.xmax = data[6] # right
self.ymax = data[7] # bottom
self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax])
# extract 3d bounding box information
self.h = data[8] # box height
self.w = data[9] # box width
self.l = data[10] # box length (in meters)
self.t = (data[11],data[12],data[13]) # location (x,y,z) in camera coord.
self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
def print_object(self):
print('Type, truncation, occlusion, alpha: %s, %d, %d, %f' % \\
(self.type, self.truncation, self.occlusion, self.alpha))
print('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f' % \\
(self.xmin, self.ymin, self.xmax, self.ymax))
print('3d bbox h,w,l: %f, %f, %f' % \\
(self.h, self.w, self.l))
print('3d bbox location, ry: (%f, %f, %f), %f' % \\
(self.t[0]kitti之ros可视化_学习笔记--第1课：资料准备及环境介绍
kitti之ros可视化_学习笔记--第6课：发布imu资料
kitti之ros可视化_学习笔记--第7课：发布gps资料