之前所做的工作参考上篇博客ubuntu1804发布kitti数据集的gps资料,imu资料(包含发布图片,点云过程)_FYY2LHH的博客-CSDN博客
本次将详解如何在ros播放的kitti数据集上作出检测框,首先需要知道的是对于kitti数据集中的每一帧都必须提前进行标注,而这个工作已经被别人完成了,所以我们的任务就是调用别人已经完成的文件,显示出来。
提前下载好tracking的文件,如果没找到链接,可以用下面的链接
链接:https://pan.baidu.com/s/1Acs4113jGlGMZz0PfLjT7w
提取码:l4bg
下载之后一定要放在下图所示的路径下
然后仍然是和以前一样的三步走战略(不了解的可以参考本文开头的链接,所有过程均已实现)
一、下面是只有相机检测框的实现代码(激光雷达和相机同时检测的代码见二、部分)
1、kitti.py
#!/usr/bin/env python
from data_utils import *
from publish_utils import *
from kitti_utils import *
import os
DATA_PATH = '/home/ros/dianyun/2011_09_26_drive_0005_sync/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)
pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
bridge = CvBridge()
ego_pub = rospy.Publisher('kitti_ego_car',Marker, queue_size=10)
# model_car_pub = rospy.Publisher('kitti_model_car',Marker, queue_size=10)
imu_pub = rospy.Publisher('kitti_imu',Imu, queue_size=10)
gps_pub = rospy.Publisher('kitti_gps',NavSatFix, queue_size=10)
rate = rospy.Rate(10)
df_tracking = read_tracking("/home/ros/dianyun/2011_09_26_drive_0005_sync/data_tracking_label_2/training/label_02/0000.txt")
while not rospy.is_shutdown():
boxes = np.array(df_tracking[df_tracking.frame==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])
types = np.array(df_tracking[df_tracking.frame==frame]['type'])
image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))
imu_data = read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
publish_camera(cam_pub, bridge, image, boxes, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_ego_car(ego_pub)
# publish_car_model(model_car_pub)
publish_imu(imu_pub, imu_data )
publish_gps(gps_pub, imu_data ) #gps rviz cannot visulize, only use rostopic echo
rospy.loginfo("kitti published")
rate.sleep()
frame += 1
frame %= 154
2、data_utils.py
#!/usr/bin/env python
import cv2
import numpy as np
import pandas as pd
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']
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']
def read_camera(path):
return cv2.imread(path)
def read_point_cloud(path):
return np.fromfile(path,dtype=np.float32).reshape(-1, 4)
def read_imu(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = IMU_COLUMN_NAMES
return df
def read_tracking(path):
df = pd.read_csv(path, header=None, sep=' ')
df.columns = TRACKING_COLUMN_NAMES
df = df[df['track_id']>=0] # remove DontCare objects
df.loc[df.type.isin(['Bus', 'Truck', 'Van', 'Tram']), 'type'] = 'Car' # Set all vehicle type to Car
df = df[df.type.isin(['Car', 'Pedestrian', 'Cyclist'])]
return df
3、publish_utils.py
#!/usr/bin/env python
import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from geometry_msgs.msg import Point
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import tf
import numpy as np
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):
for typ, box in zip(types, boxes):
top_left = int(box[0]),int(box[1])
bottom_right = int(box[2]),int(box[3])
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_cloud):
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = FRAME_ID
pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))
def publish_ego_car(ego_car_pub):
# publish left and right 45 degree FOV lines and ego car model mesh
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration()
marker.type = Marker.LINE_STRIP
# line
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2 # line width
marker.points = []
# check the kitti axis model
marker.points.append(Point(5,-5,0)) # left up
marker.points.append(Point(0,0,0)) # center
marker.points.append(Point(5, 5,0)) # right up
ego_car_pub.publish(marker)
def publish_car_model(model_pub):
mesh_marker = Marker()
mesh_marker.header.frame_id = FRAME_ID
mesh_marker.header.stamp = rospy.Time.now()
mesh_marker.id = -1
mesh_marker.lifetime = rospy.Duration()
mesh_marker.type = Marker.MESH_RESOURCE
mesh_marker.mesh_resource = "/root/catkin_ws/src/kitti_tutorial/AudiR8.dae" #LOAD ERROR, DON'T KNOW WHY
mesh_marker.pose.position.x = 0.0
mesh_marker.pose.position.y = 0.0
mesh_marker.po