ubuntu1804在rviz中显示kitti数据集的2D检测框和激光雷达的3D检测框

之前所做的工作参考上篇博客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