画出track_id
publish_utils.py中
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):
marker_array = MarkerArray()
for i, corners_3d_velo in enumerate(corners_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_LIST
b, g, r = DETECTION_COLOR_DICT[types[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)
# 添加track_id
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i + 1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
# 计算中心点的位置
p = np.mean(corners_3d_velo, axis=0) # upper front left corner
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = p[2] + 1
text_marker.text = str(track_ids[i])
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
b, g, r = DETECTION_COLOR_DICT[types[i]]
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.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as np
import rospy
from data_utils import *
from publish_utils import *
from kitti_util import *
import os
DATA_PATH = "/home/d501/kitti_data/2011_09_26/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):
# 计算旋转矩阵
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
# 8个顶点的xyz
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]
# 旋转矩阵点乘(3，8)顶点矩阵
corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
# 加上location中心点，得出8个顶点旋转后的坐标
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2
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)
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_3d', MarkerArray, queue_size=10)
bridge = CvBridge()
df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')
calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
df_tracking_frame = df_tracking[df_tracking.frame==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[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])
track_ids = np.array(df_tracking_frame['track id'])
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]
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_2d, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_ego_car(ego_pub)
publish_imu(imu_pub, imu_data)
publish_gps(gps_pub, imu_data)
publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)
rospy.loginfo("published")
rate.sleep()
frame += 1
frame %= 154运行结果截图：
画出历史轨迹
画出所有物体历史轨迹
kitti.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as np
from collections import deque
import rospy
from data_utils import *
from publish_utils import *
from kitti_util import *
import os
DATA_PATH = "/home/d501/kitti_data/2011_09_26/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):
# 计算旋转矩阵
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
# 8个顶点的xyz
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]
# 旋转矩阵点乘(3，8)顶点矩阵
corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
# 加上location中心点，得出8个顶点旋转后的坐标
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2
# 记录一个物体的历轨迹
class Object():
def __init__(self, center):
self.locations = deque(maxlen=20)
self.locations.appendleft(center)
def update(self, center, displacement, yaw_change):
for i in range(len(self.locations)):
x0, y0 = self.locations[i]
x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
self.locations[i] = np.array([x1, y1])
if center is not None:
self.locations.appendleft(center)
def reset(self):
self.locations = deque(maxlen=20)
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)
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_3d', MarkerArray, queue_size=10)
loc_pub = rospy.Publisher('kitti_loc', MarkerArray, queue_size=10)
bridge = CvBridge()
df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')
calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)
rate = rospy.Rate(10)
tracker = {} # track_id : Object
prev_imu_data = None
while not rospy.is_shutdown():
df_tracking_frame = df_tracking[df_tracking.frame==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[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])
track_ids = np.array(df_tracking_frame['track id'])
corners_3d_velos = []
# 表示当前帧所有检测到物体的中心
centers = {} # track_id : center
for track_id, box_3d in zip(track_ids, 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]
centers[track_id] = np.mean(corners_3d_velo, axis=0)[:2]
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))
if prev_imu_data is None:
# 之前没有过数据，则表示第一帧
for track_id in centers:
tracker[track_id] = Object(centers[track_id])
else:
displacement = 0.1 * np.linalg.norm(imu_data[['vf', 'vl']])
yaw_change = float(imu_data.yaw - prev_imu_data.yaw)
# 将当前帧检测到的物体历史轨迹进行更新
for track_id in centers:
# 如果当前帧检测的物体之前就检测到过
if track_id in tracker:
tracker[track_id].update(centers[track_id], displacement, yaw_change)
else:
# 当前帧物体第一次被检测到
tracker[track_id] = Object(centers[track_id])
# 更新当前帧没有检测到，但是之前检测到过的物体
for track_id in tracker:
if track_id not in centers:
tracker[track_id].update(None, displacement, yaw_change)
prev_imu_data = imu_data
publish_camera(cam_pub, bridge, image, boxes_2d, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_ego_car(ego_pub)
publish_imu(imu_pub, imu_data)
publish_gps(gps_pub, imu_data)
publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)
# 发布历史轨迹
publish_loc(loc_pub, tracker, centers)
rospy.loginfo("published")
rate.sleep()
frame += 1
if frame == 154:
frame = 0
for track_id in tracker:
tracker[track_id].reset()publish_utils.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import cv2
import rospy
from geometry_msgs.msg import Point
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from tf import transformations
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np
import pandas as pd
import os
FRAME_ID = 'map'
DETECTION_COLOR_DICT = {'Car': (255, 255, 0), 'Pedestrian': (0, 226, 255), 'Cyclist': (141, 40, 255)}
LIFETIME = 0.1
LINES = [[0, 1], [1, 2], [2, 3], [3, 0]] # Lower plane parallel to Z=0 plane
LINES += [[4, 5], [5, 6], [6, 7], [7, 4]] # Upper plane parallel to Z=0 plane
LINES += [[0, 4], [1, 5], [2, 6], [3, 7]] # Connections between upper and lower planes
LINES += [[4,1], [5,0]]
def publish_camera(cam_pub, bridge, image, boxes, types):
for type, 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[type], 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
:param ego_car_pub:
:return:
"""
marker_array = MarkerArray()
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
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
marker.points = []
marker.points.append(Point(10, -10, 0))
marker.points.append(Point(0, 0, 0))
marker.points.append(Point(10, 10, 0))
marker_array.markers.append(marker)
mech_marker = Marker()
mech_marker.header.frame_id = "map"
mech_marker.header.stamp = rospy.Time.now()
mech_marker.id = -1
mech_marker.type = Marker.MESH_RESOURCE
mech_marker.action = Marker.ADD
mech_marker.lifetime = rospy.Duration()
mech_marker.mesh_resource = "package://kitti_tutorial/meshes/Car.dae"
mech_marker.pose.position.x = 0.0
mech_marker.pose.position.y = 0.0
mech_marker.pose.position.z = -1.73
q = transformations.quaternion_from_euler(np.pi, np.pi, -np.pi / 2.0)
mech_marker.pose.orientation.x = q[0]
mech_marker.pose.orientation.y = q[1]
mech_marker.pose.orientation.z = q[2]
mech_marker.pose.orientation.w = q[3]
mech_marker.color.r = 1.0
mech_marker.color.g = 1.0
mech_marker.color.b = 1.0
mech_marker.color.a = 1.0
mech_marker.scale.x = 1.0
mech_marker.scale.y = 1.0
mech_marker.scale.z = 1.0
marker_array.markers.append(mech_marker)
ego_car_pub.publish(marker_array)
def publish_imu(imu_pub, imu_data):
imu = Imu()
imu.header.frame_id = "map"
imu.header.stamp = rospy.Time.now()
q = transformations.quaternion_from_euler(float(imu_data.roll), float(imu_data.pitch), float(imu_data.yaw))
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
imu.linear_acceleration.y = imu_data.al
imu.linear_acceleration.z = imu_data.au
imu.angular_velocity.x = imu_data.wx
imu.angular_velocity.y = imu_data.wy
imu.angular_velocity.z = imu_data.wz
imu_pub.publish(imu)
def publish_gps(gps_pub, gps_data):
gps = NavSatFix()
gps.header.frame_id = "map"
gps.header.stamp = rospy.Time.now()
gps.latitude = gps_data.lat
gps.longitude = gps_data.lon
gps.altitude = gps_data.alt
gps_pub.publish(gps)
def publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids):
marker_array = MarkerArray()
for i, corners_3d_velo in enumerate(corners_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_LIST
b, g, r = DETECTION_COLOR_DICT[types[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)
# 添加track_id
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i + 1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
# 计算中心点的位置
p = np.mean(corners_3d_velo, axis=0) # upper front left corner
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = p[2] + 1
text_marker.text = str(track_ids[i])
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
b, g, r = DETECTION_COLOR_DICT[types[i]]
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)
# 发布历史轨迹
def publish_loc(loc_pub, tracker, centers):
marker_array = MarkerArray()
for track_id in centers:
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.id = track_id
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.points = []
for p in tracker[track_id].locations:
marker.points.append(Point(p[0], p[1], 0))
marker_array.markers.append(marker)
loc_pub.publish(marker_array)
显示所有物体到汽车最近的距离
添加两个函数：
def distance_point_to_segment(P, A, B):
"""
Calculate the min distance of a point P to a segment AB.
Return the point Q in AB on which the min distance is reached.
:param P:
:param A:
:param B:
:return:
"""
AP = P - A
BP = P - B
AB = B - A
# 如果是锐角三角形
if np.dot(AB, AP) >= 0 and np.dot(-AB, BP) >= 0:
return np.abs(np.cross(AP, AB))/np.linalg.norm(AB), np.dot(AP, AB)/np.dot(AB, AB) * AB + A
# 如果是钝角三角形
d_PA = np.linalg.norm(AP)
d_PB = np.linalg.norm(BP)
if d_PA < d_PB:
return d_PA, A
return d_PB, B
def min_distance_cuboids(cub1, cub2):
"""
Computes the minimum distance between two non-overlapping cuboids (3D) of shape (8, 3)
They are projected to BEV and the minium distance of the two rectangles are returned
:param cub1:
:param cub2:
:return:
"""
minD = 1e5
for i in range(4):
for j in range(4):
d, Q = distance_point_to_segment(cub1[i, :2], cub2[j, :2], cub2[j+1, :2])
if d < minD:
minD = d
minP = cub1[i, :2]
minQ = Q
for i in range(4):
for j in range(4):
d, Q = distance_point_to_segment(cub2[i, :2], cub1[j, :2], cub1[j+1, :2])
if d < minD:
minD = d
minP = cub2[i, :2]
minQ = Q
return minP, minQ, minD修改kitti.py
#!/usr/bin/env python
# -*-coding:utf8 -*-
import numpy as np
from collections import deque
import rospy
from data_utils import *
from publish_utils import *
from kitti_util import *
import os
DATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"
EGOCAR = np.array([[2.15, 0.9, -1.73], [2.15, -0.9, -1.73], [-1.95, -0.9, -1.73], [-1.95, 0.9, -1.73],
[2.15, 0.9, -0.23], [2.15, -0.9, -0.23], [-1.95, -0.9, -0.23], [-1.95, 0.9, -0.23]])
def compute_3d_box_cam2(h, w, l, x, y, z, yaw):
# 计算旋转矩阵
R = np.array([[np.cos(yaw), 0, np.sin(yaw)], [0, 1, 0], [-np.sin(yaw), 0, np.cos(yaw)]])
# 8个顶点的xyz
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]
# 旋转矩阵点乘(3，8)顶点矩阵
corners_3d_cam2 = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
# 加上location中心点，得出8个顶点旋转后的坐标
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2
# 记录一个物体的历轨迹
class Object():
def __init__(self, center):
self.locations = deque(maxlen=20)
self.locations.appendleft(center)
def update(self, center, displacement, yaw_change):
for i in range(len(self.locations)):
x0, y0 = self.locations[i]
x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
self.locations[i] = np.array([x1, y1])
if center is not None:
self.locations.appendleft(center)
def reset(self):
self.locations = deque(maxlen=20)
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)
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_3d', MarkerArray, queue_size=10)
loc_pub = rospy.Publisher('kitti_loc', MarkerArray, queue_size=10)
# 显示车距
dist_pub = rospy.Publisher('kitti_dist', MarkerArray, queue_size=10)
bridge = CvBridge()
df_tracking = read_labelfile('/home/d501//kitti_data/2011_09_26/data_tracking_label_2/training/label_02/0000.txt')
calib = Calibration("/home/d501//kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26", from_video=True)
rate = rospy.Rate(10)
# 所有物体
tracker = {} # track_id : Object
prev_imu_data = None
while not rospy.is_shutdown():
df_tracking_frame = df_tracking[df_tracking.frame==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[['dimensions_height','dimensions_width','dimensions_length','location_x','location_y','location_z','rotation_y']])
track_ids = np.array(df_tracking_frame['track id'])
corners_3d_velos = []
# 表示当前帧所有检测到物体的中心
centers = {} # track_id : center
# 最短距离和点
minPQDs = []
for track_id, box_3d in zip(track_ids, boxes_3d):
corners_3d_cam2 = compute_3d_box_cam2(*box_3d)
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)
minPQDs.append(min_distance_cuboids(EGOCAR, corners_3d_velo))
corners_3d_velos += [corners_3d_velo]
centers[track_id] = np.mean(corners_3d_velo, axis=0)[:2]
corners_3d_velos.append(EGOCAR)
types = np.append(types, 'Car')
track_ids = np.append(track_ids, -1)
# 表示汽车本身
centers[-1] = np.array([0, 0])
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))
if prev_imu_data is None:
# 之前没有过数据，则表示第一帧
for track_id in centers:
tracker[track_id] = Object(centers[track_id])
else:
displacement = 0.1 * np.linalg.norm(imu_data[['vf', 'vl']])
yaw_change = float(imu_data.yaw - prev_imu_data.yaw)
# 将当前帧检测到的物体历史轨迹进行更新
for track_id in centers:
# 如果当前帧检测的物体之前就检测到过
if track_id in tracker:
tracker[track_id].update(centers[track_id], displacement, yaw_change)
else:
# 当前帧物体第一次被检测到
tracker[track_id] = Object(centers[track_id])
# 更新当前帧没有检测到，但是之前检测到过的物体
for track_id in tracker:
if track_id not in centers:
tracker[track_id].update(None, displacement, yaw_change)
prev_imu_data = imu_data
publish_camera(cam_pub, bridge, image, boxes_2d, types)
publish_point_cloud(pcl_pub, point_cloud)
publish_ego_car(ego_pub)
publish_imu(imu_pub, imu_data)
publish_gps(gps_pub, imu_data)
publish_3dbox(box3d_pub, corners_3d_velos, types, track_ids)
# 发布历史轨迹
publish_loc(loc_pub, tracker, centers)
publish_dist(dist_pub, minPQDs)
rospy.loginfo("published")
rate.sleep()
frame += 1
if frame == 154:
frame = 0
for track_id in tracker:
tracker[track_id].reset()publish_utils.py添加函数
def publish_dist(dist_pub, minPQDs):
marker_array = MarkerArray()
for i, (minP, minQ, minD) in enumerate(minPQDs):
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.id = i
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.5
marker.scale.x = 0.1
marker.points = []
marker.points.append(Point(minP[0], minP[1], 0))
marker.points.append(Point(minQ[0], minQ[1], 0))
marker_array.markers.append(marker)
text_marker = Marker()
text_marker.header.frame_id = FRAME_ID
text_marker.header.stamp = rospy.Time.now()
text_marker.id = i + 1000
text_marker.action = Marker.ADD
text_marker.lifetime = rospy.Duration(LIFETIME)
text_marker.type = Marker.TEXT_VIEW_FACING
p = (minP + minQ) / 2.0
text_marker.pose.position.x = p[0]
text_marker.pose.position.y = p[1]
text_marker.pose.position.z = 0
text_marker.text = '%.2f'%minD
text_marker.scale.x = 1
text_marker.scale.y = 1
text_marker.scale.z = 1
text_marker.color.r = 1.0
text_marker.color.g = 1.0
text_marker.color.b = 1.0
text_marker.color.r = 0.0
marker_array.markers.append(text_marker)
dist_pub.publish(marker_array)
