这节细节有点多，下面只是粗略的实现，还有很多细节还在完善中（2022.10.03）！！！

  ROS 将kitti图像的目标检测2D检测框画出来，并用rviz显示，效果如下：

在前文，我们已经讲kitti的图像进行发布，并用rviz显示，所以本文只将2D的检测框画出来即可。

1.准备数据

2.读取数据

2.1data_utils.py中添加的内容

在data_utils.py中添加读取标签数据的代码

import pandas as pd

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_tracking(path):
    df=pd.read_csv(path,header=None,sep=' ')
    df.columns=COLUMN_NAMES
    df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'
    df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]
    return df

2.2标签数据解释

  我们用jupyter notebook加载数据，对数据进行逐一的解释。

1. 首先是加载功能包

import numpy as np
import pandas as pd

2. 读取数据，并给每一列起一个名字

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']

df=pd.read_csv("/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/training/label_02/0000.txt",header=None,sep=' ')
df.columns=COLUMN_NAMES
df.head()

  其中2011_09_26/2011_09_26_drive_0005_sync/training/label_02/0000.txt"是我们要用到的标签数据；关于COLUMN_NAMES中每一项的详细解释，可以参考这篇文章。

  我们只对关键数据进行解释。frame是当前检测到的物体属于的帧数；type是该物体的类型，包含有:'Car', 'Van', 'Truck', 'Pedestrian', 'Person_sitting', 'Cyclist', 'Tram', 'Misc' or 'DontCare'；'bbox_left','bbox_top','bbox_right','bbox_bottom'是2D包围框的信息。

3. 将各类机动车都合并，归为一类Car

df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'
df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]

4. 获取某一各物体的2D检测框

box=np.array(df.loc[2,['bbox_left','bbox_top','bbox_right','bbox_bottom']])

5. 读取一张图片，并画出2D检测框

DETECTION_COLOR_DATA={
    'Car':(255,255,0),'Pedestrian':(0,226,255),'Cyclist':(141,40,255)}
frame=120

image=cv2.imread('/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/image_02/data/%010d.png'%frame)


boxes=np.array(df[df["frame"]==frame][['bbox_left','bbox_top','bbox_right','bbox_bottom']])
types=np.array(df[df["frame"]==frame]['type'])

for typ,box in list(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_DATA[typ],2)
    
    
cv2.imshow("image",image)
cv2.waitKey(0)
cv2.destroyAllWindows()

3.发布数据

def publish_3dbox(box3d_pub,corner_3d_velos,object_type):
    marker_array=MarkerArray()
    for i, corners_3d_velo in enumerate(corner_3d_velos):
        marker=Marker()
        marker.header.frame_id=FRAME_ID
        marker.header.stamp=rospy.Time.now()

        marker.id=i
        marker.action=Marker.ADD
        marker.lifetime=rospy.Duration(LIFETIME)
        marker.type=Marker.LINE_STRIP
        b,g,r=DETECTION_COLOR_DATA[object_type[i]]

        marker.color.r=r/255.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:
            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)

    box3d_pub.publish(marker_array)

4.发布节点

#!/usr/bin/env python

import os

from data_utils import *
from publish_utils import *
from kitti_utils import *


DATA_PATH='/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/'
TRACKING_DATA_PATH="/home/chen/Downloads/kittidata/2011_09_26/2011_09_26_drive_0005_sync/training/label_02/0000.txt"

def compute_3d_box_cam2(h,w,l,x,y,z,yaw):
    """
    Return : 3xn in cam2 coordinate
    """
    R=np.array([[np.cos(yaw),0,np.sin(yaw)],[0,1,0],[-np.sin(yaw),0,np.cos(yaw)]])
    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,]
    corners_3d_cam2=np.dot(R,np.vstack([x_corners,y_corners,z_corners]))
    corners_3d_cam2+=np.vstack([x,y,z])
    return corners_3d_cam2

if __name__=='__main__':
    rospy.init_node('kitti_node',anonymous=True)
    cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub=rospy.Publisher('kitti_pcl',PointCloud2,queue_size=10)
    ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)
    imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)
    gps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)
    box3d_pub=rospy.Publisher('kitti_box3d',MarkerArray,queue_size=10)
    bridge=CvBridge()
    rate=rospy.Rate(10)

    df_tracking=read_tracking(TRACKING_DATA_PATH)
    calib=Calibration('/home/chen/Downloads/kittidata/2011_09_26',from_video=True)

    frame=0

    while not rospy.is_shutdown():
        df_tracking_frame=df_tracking[df_tracking["frame"]==frame]

        img=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
        pcl=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
        imu=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
        boxes_2d=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'])
        boxes_3d=np.array(df_tracking_frame[['height','width','length','pos_x','pos_y','pos_z','rot_y']])

        corners_3d_velos=[]
        for box_3d in boxes_3d:
            corners_3d_cam2=compute_3d_box_cam2(*box_3d)
            corners_3d_velo=calib.project_rect_to_velo(corners_3d_cam2.T)
            corners_3d_velos+=[corners_3d_velo]

        publish_camera(cam_pub,bridge,img,boxes_2d,types)
        publish_pcl(pcl_pub,pcl)
        publish_ego_car(ego_pub)
        publish_imu(imu_pub,imu)
        publish_gps(gps_pub,imu)
        publish_3dbox(box3d_pub,corners_3d_velos,types)



        rospy.loginfo('published')
        rate.sleep()
        frame+=1
        frame%=154