ubuntu1804ros播放kitti数据集之目标追踪，显示物体的id

本文是在上篇博客的基础上，增加部分代码，以实现标题内容

1、kitti.py

#!/usr/bin/env python
from data_utils import *
from publish_utils import *
from kitti_utils import *
import os
from kitti_utils import *


DATA_PATH = '/home/ros/dianyun/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync/'


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__":
    frame = 0
    rospy.init_node('kitti_node',anonymous=True)