Skip to content

L
lidar_tracking
Commit 8a1978dd authored by markshih91's avatar markshih91
Browse files
Options

update

parent ed6f4503
Hide whitespace changes
Inline Side-by-side
Showing with 23 additions and 290 deletions
tracking/scripts/AB3DMOT_libs/kalman_filter.py
View file @ 8a1978dd
......@@ -6,32 +6,6 @@ from filterpy.kalman import KalmanFilter
from sklearn.decomposition import PCA
NUM_FRAME = 5
DIST_THRED = 0.5
ANGLE_THRED = 2.735884
def center_rot_y_f(points):
x_list = points[:, 0]
y_list = points[:, 1]
pca = PCA(n_components=2)
pca.fit(points)
PCA(copy=True, n_components=2, whiten=False)
orientation = pca.components_[0]
dx = x_list[-1] - x_list[0]
dy = y_list[-1] - y_list[0]
alpha = np.dot(np.array([dx, dy]), orientation)
if alpha < 0:
orientation = -orientation
center_rot_y = math.atan2(orientation[1], orientation[0])
return center_rot_y
class KalmanBoxTracker(object):
"""
......@@ -87,12 +61,13 @@ class KalmanBoxTracker(object):
# [0,0,0,0,0,0,1,0,0,0,0]])
# self.kf.R[0:,0:] *= 10. # measurement uncertainty
self.kf.P[7:,
7:] *= 1000. # state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
# self.kf.P[7:, 7:] *= 1000. # state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
self.kf.P[:, :] *= 1000.
self.kf.P *= 10.
# self.kf.Q[-1,-1] *= 0.01 # process uncertainty
self.kf.Q[7:, 7:] *= 0.01
# self.kf.Q[7:, 7:] *= 0.01
self.kf.Q[:, :] *= 0.01
self.kf.x[:7] = bbox3D.reshape((7, 1))
self.time_since_update = 0
......@@ -105,45 +80,11 @@ class KalmanBoxTracker(object):
self.still_first = True
self.age = 0
self.info = info # other info associated
last_x_y = self.kf.x[:2]
self.last_x_y_list = [last_x_y]
def update(self, bbox3D, info):
"""
Updates the state vector with observed bbox.
"""
points = self.last_x_y_list[:]
points = np.array(points)
points = np.squeeze(points, axis=(2,))
x_list = points[:, 0]
y_list = points[:, 1]
if len(points) >= NUM_FRAME and abs(x_list[-1] - x_list[0]) > DIST_THRED or abs(y_list[-1] - y_list[0]) > DIST_THRED:
center_rot_y = center_rot_y_f(x_list, y_list)
rot_y = bbox3D[3]
if abs(center_rot_y - rot_y) > np.pi / 2.0 and abs(
center_rot_y - rot_y) < np.pi * 3 / 2.0: # if the angle of two theta is not acute angle
rot_y += np.pi
if rot_y > np.pi: rot_y -= np.pi * 2 # make the theta still in the range
if rot_y < -np.pi: rot_y += np.pi * 2
refine_alpha = (math.pi*ANGLE_THRED/180)
if abs(rot_y - center_rot_y) >= np.pi * 3 / 2.0:
if rot_y > center_rot_y + refine_alpha:
rot_y += refine_alpha
if rot_y < center_rot_y - refine_alpha:
rot_y -= refine_alpha
else:
if rot_y > center_rot_y + refine_alpha:
rot_y -= refine_alpha
if rot_y < center_rot_y - refine_alpha:
rot_y += refine_alpha
if rot_y > np.pi: rot_y -= np.pi * 2
if rot_y < -np.pi: rot_y += np.pi * 2
bbox3D[3] = rot_y
self.time_since_update = 0
self.history = []
......@@ -187,11 +128,6 @@ class KalmanBoxTracker(object):
"""
Advances the state vector and returns the predicted bounding box estimate.
"""
if self.time_since_update == 0:
last_x_y = self.kf.x[:2]
self.last_x_y_list.append(last_x_y)
if len(self.last_x_y_list) > NUM_FRAME:
del(self.last_x_y_list[0])
self.kf.predict()
if self.kf.x[3] >= np.pi: self.kf.x[3] -= np.pi * 2
......
tracking/scripts/AB3DMOT_libs/model.py
View file @ 8a1978dd
......@@ -145,7 +145,7 @@ class AB3DMOT(object): # A baseline of 3D multi-object tracking
if (trk.time_since_update >= self.max_age):
self.trackers.pop(i)
if (len(ret) > 0): return np.concatenate(ret) # h,w,l,x,y,z,theta, ID, other info, confidence
return np.empty((0, 24))
return np.empty((0, 17))
def predict(self, timestamp):
......
tracking/scripts/listener.py
View file @ 8a1978dd
......@@ -48,18 +48,6 @@ def rotz(t):
[0, 0, 1]])
def loc2backwheel(msg, deal_type=0, bw_distance=3.6):
hypotenuse = np.sqrt(msg.v_x ** 2 + msg.v_y ** 2)
if msg.type != deal_type or hypotenuse <= 0:
return msg.loc_x, msg.loc_y, msg.loc_z
a = -msg.v_x*bw_distance / hypotenuse
b = -msg.v_y*bw_distance / hypotenuse
return msg.loc_x + a, msg.loc_y + b, msg.loc_z
def callback(msgs):
log_file = open(log_file_path, 'a')
......@@ -122,7 +110,7 @@ def callback(msgs):
msg.name = str(tracker[14])
msg.license_plate_number = str(tracker[15])
msg.color_name = str(tracker[16])
lidar_loc, out_BL = get_loc([msg.x, msg.y, msg.z, msg.l, msg.w, msg.h, msg.rot_y], msg.type)
lidar_loc, out_BL, out_center_BL = get_loc([msg.x, msg.y, msg.z, msg.l, msg.w, msg.h, msg.rot_y], msg.type)
msg.loc_x = lidar_loc[0]
msg.loc_y = lidar_loc[1]
msg.loc_z = lidar_loc[2]
......@@ -171,7 +159,7 @@ def callback(msgs):
marker_bbox_arr.markers.append(marker_bbox)
bbox_i += 1
if msg.type != 4 and msg.loc_x < 30 and msg.loc_y < -2 and (msg.loc_x < 28 or msg.loc_y < -7):
if msg.type != 4:
marker = Marker()
marker.id = i
marker.header.frame_id = "Pandar64"
......@@ -215,14 +203,14 @@ def callback(msgs):
t_marker.color.g = 1.0
t_marker.color.b = 0
t_marker.lifetime = rospy.Duration(0.5)
t_marker.text = 'id:{0},(x,y):({1:.2f},{2:.2f}),v:{3:.2f}m/s\ntype:{4},No:{6}'.format(msg.obj_id, msg.x, msg.y, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y), msg.name, msg.color_name, msg.license_plate_number)
t_marker.text = 'id:{0},type:{1},v:{2:.2f}m/s,No:{3}'.format(msg.obj_id, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number)
marker_arr.markers.append(t_marker)
i += 1
log_file.write('%u,%u,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%f,%f\n' % (
log_file.write('%u,%u,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%.8f,%.8f,%s,%s,%s\n' % (
msg.frame, msg.type, msg.score, msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y,
msg.obj_id, msg.v_x, msg.v_y, msg.v_z,msg.loc_x, msg.loc_y, msg.loc_z, out_BL[0], out_BL[1]))
msg.obj_id, msg.v_x, msg.v_y, msg.v_z,msg.loc_x, msg.loc_y, msg.loc_z, out_BL[0], out_BL[1], out_center_BL[0], out_center_BL[1], msg.name, msg.license_plate_number, msg.color_name))
log_file.close()
......@@ -245,14 +233,14 @@ def listener():
if __name__ == '__main__':
global mot_tracker, log_file_path
mot_tracker = AB3DMOT(max_age=3, min_hits=1)
mot_tracker = AB3DMOT(max_age=3, min_hits=3)
cur_time = time.strftime('%Y%m%d_%H:%M:%S', time.localtime(time.time()))
log_file_path = 'src/tracking/logs/' + cur_time + '.txt'
if not os.path.exists('src/tracking/logs/'):
os.mkdir('src/tracking/logs/')
with open(log_file_path, 'w') as f:
f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long\n")
f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
listener()
......
tracking/scripts/listener_10fps.py
View file @ 8a1978dd
......@@ -48,23 +48,9 @@ def rotz(t):
[0, 0, 1]])
def loc2backwheel(msg, deal_type=0, bw_distance=3.6):
hypotenuse = np.sqrt(msg.v_x ** 2 + msg.v_y ** 2)
if msg.type != deal_type or hypotenuse <= 0:
return msg.loc_x, msg.loc_y, msg.loc_z
a = -msg.v_x*bw_distance / hypotenuse
b = -msg.v_y*bw_distance / hypotenuse
return msg.loc_x + a, msg.loc_y + b, msg.loc_z
def callback(msgs):
log_file = open(log_file_path, 'a')
if save_ori_info:
ori_log_file = open(ori_log_file_path, 'a')
cloud_pub = rospy.Publisher("tracking_cloud", PointCloud2, queue_size=10)
cloud_pub.publish(msgs.cloud)
......@@ -122,7 +108,7 @@ def callback(msgs):
infos = []
for msg in msgs.array:
det = [msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y]
info = [msg.frame, msg.type, msg.score, msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y, msg.name, msg.license_plate_number, msg.color_name]
info = [msg.frame, msg.type, msg.score, msg.name, msg.license_plate_number, msg.color_name]
dets.append(det)
infos.append(info)
dets_all = {'dets': np.asarray(dets), 'info': np.asarray(infos)}
......@@ -155,26 +141,14 @@ def callback(msgs):
msg.frame = int(tracker[11])
msg.type = int(tracker[12])
msg.score = float(tracker[13])
ori_h = float(tracker[14])
ori_w = float(tracker[15])
ori_l = float(tracker[16])
ori_x = float(tracker[17])
ori_y = float(tracker[18])
ori_z = float(tracker[19])
ori_rot_y = float(tracker[20])
msg.name = str(tracker[21])
msg.license_plate_number = str(tracker[22])
msg.color_name = str(tracker[23])
msg.name = str(tracker[14])
msg.license_plate_number = str(tracker[15])
msg.color_name = str(tracker[16])
lidar_loc, out_BL, out_center_BL = get_loc([msg.x, msg.y, msg.z, msg.l, msg.w, msg.h, msg.rot_y], msg.type)
msg.loc_x = lidar_loc[0]
msg.loc_y = lidar_loc[1]
msg.loc_z = lidar_loc[2]
ori_lidar_loc, ori_out_BL, ori_out_center_BL = get_loc([ori_x, ori_y, ori_z, ori_l, ori_w, ori_h, ori_rot_y], msg.type)
ori_loc_x = ori_lidar_loc[0]
ori_loc_y = ori_lidar_loc[1]
ori_loc_z = ori_lidar_loc[2]
bbox = BoundingBox()
bbox.label = msg.obj_id
bbox.value = 2
......@@ -272,14 +246,7 @@ def callback(msgs):
msg.frame, msg.type, msg.score, msg.h, msg.w, msg.l, msg.x, msg.y, msg.z, msg.rot_y,
msg.obj_id, msg.v_x, msg.v_y, msg.v_z,msg.loc_x, msg.loc_y, msg.loc_z, out_BL[0], out_BL[1], out_center_BL[0], out_center_BL[1], msg.name, msg.license_plate_number, msg.color_name))
if save_ori_info:
ori_log_file.write('%u,%u,%f,%f,%f,%f,%f,%f,%f,%f,%d,%f,%f,%f,%f,%f,%f,%.8f,%.8f,%.8f,%.8f\n' % (
msg.frame, msg.type, msg.score, ori_h, ori_w, ori_l, ori_x, ori_y, ori_z, ori_rot_y,
msg.obj_id, msg.v_x, msg.v_y, msg.v_z,ori_loc_x, ori_loc_y, ori_loc_z, ori_out_BL[0], ori_out_BL[1], ori_out_center_BL[0], ori_out_center_BL[1]))
log_file.close()
if save_ori_info:
ori_log_file.close()
bbox_pub = rospy.Publisher("tracking_bbox", BoundingBoxArray, queue_size=10)
bbox_pub.publish(bbox_arr)
......@@ -299,10 +266,7 @@ def listener():
if __name__ == '__main__':
global mot_tracker, log_file_path, save_ori_info
save_ori_info = True
global mot_tracker, log_file_path
mot_tracker = AB3DMOT(max_age=3, min_hits=3)
cur_time = time.strftime('%Y%m%d_%H:%M:%S', time.localtime(time.time()))
......@@ -312,11 +276,6 @@ if __name__ == '__main__':
with open(log_file_path, 'w') as f:
f.write("frame,type,score,h,w,l,x,y,z,rot_y,obj_id,v_x,v_y,v_z,loc_x,loc_y,loc_z,Lat,Long,center_Lat,center_Long,name,license_plate_number,color_name\n")
if save_ori_info:
ori_log_file_path = 'src/tracking/logs/' + cur_time + '_ori.txt'
with open(ori_log_file_path, 'w') as f:
f.write("frame,type,score,ori_h,ori_w,ori_l,ori_x,ori_y,ori_z,ori_rot_y,obj_id,v_x,v_y,v_z,ori_loc_x,ori_loc_y,ori_loc_z,ori_Lat,ori_Long,ori_center_Lat,ori_center_Long\n")
listener()
......
tracking/scripts/listener_logshow.py
View file @ 8a1978dd
......@@ -48,18 +48,6 @@ def rotz(t):
[0, 0, 1]])
def loc2backwheel(msg, deal_type=0, bw_distance=3.6):
hypotenuse = np.sqrt(msg.v_x ** 2 + msg.v_y ** 2)
if msg.type != deal_type or hypotenuse <= 0:
return msg.loc_x, msg.loc_y, msg.loc_z
a = -msg.v_x*bw_distance / hypotenuse
b = -msg.v_y*bw_distance / hypotenuse
return msg.loc_x + a, msg.loc_y + b, msg.loc_z
def callback(msgs):
cloud_pub = rospy.Publisher("tracking_cloud", PointCloud2, queue_size=10)
......@@ -179,9 +167,7 @@ def callback(msgs):
t_marker.color.g = 1.0
t_marker.color.b = 0
t_marker.lifetime = rospy.Duration(0.5)
#t_marker.text = 'id:{0},(x,y):({1:.2f},{2:.2f}),v:{3:.2f}m/s\ntype:{4},No:{6},timestamp:{7}'.format(msg.obj_id, msg.x, msg.y, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.name, msg.color_name, msg.license_plate_number,msg.frame)
t_marker.text = 'rot_y: {0}'.format(msg.rot_y)
#t_marker.text = 'vx: {0}, vy: {1}'.format(msg.v_x, mag.v_y)
t_marker.text = 'id:{0},type:{1},v:{2:.2f}m/s,No:{3}'.format(msg.obj_id, msg.name, math.sqrt(msg.v_x*msg.v_x+msg.v_y*msg.v_y)*10, msg.license_plate_number)
marker_arr.markers.append(t_marker)
i += 1
......@@ -204,7 +190,7 @@ def listener():
if __name__ == '__main__':
global mot_tracker
mot_tracker = AB3DMOT(max_age=5, min_hits=1)
mot_tracker = AB3DMOT(max_age=3, min_hits=3)
cur_time = time.strftime('%Y%m%d_%H:%M:%S', time.localtime(time.time()))
listener()
......
tracking/scripts/talker.py
View file @ 8a1978dd
......@@ -60,8 +60,8 @@ def talker(cloud_folder, seq_file):
msgs = det_tracking_array()
msgs.cloud = cloud
for seq_det in cur_frame_seq_dets:
msg = det_tracking()
msg.frame = int(int(seq_det[17])/1000000.0)
msg = det_tracking()
msg.frame = int(int(seq_det[17])/1000000.0)
msg.type = int(seq_det[1])
msg.score = float(seq_det[6])
msg.h = float(seq_det[7])
......
tracking/scripts/talker_logshow.py
View file @ 8a1978dd
......@@ -25,15 +25,12 @@ def talker(seq_file):
while frame_id < len(frames) and not rospy.is_shutdown():
frame = frames[frame_id]
# if frame < 1618475108934:
# frame_id += 1
# continue
cur_frame_seq_dets = seq_dets[seq_dets[:, 0].astype(np.uint64) == frame]
msgs = det_tracking_array()
for seq_det in cur_frame_seq_dets:
msg = det_tracking()
msg.frame = int(seq_det[0])
msg = det_tracking()
msg.frame = int(seq_det[0])
msg.type = int(seq_det[1])
msg.score = float(seq_det[2])
msg.h = float(seq_det[3])
......
tracking/scripts/talker_logshow_GT.py deleted 100755 → 0
View file @ ed6f4503
#!/usr/bin/env python
# license removed for brevity
import os
import math
import time
import rospy
from sensor_msgs.msg import PointCloud2, PointField
import numpy as np
from tracking.msg import det_tracking, det_tracking_array
import open3d as o3d
def my_atan(x, y):
if (x > 0.1 and y > 0.1) or (x < -0.1 and y > 0.1):
return -math.atan(x/y)
elif x > 0.1 and y < -0.1:
return -math.pi - math.atan(x/y)
elif x < -0.1 and y < -0.1:
return math.pi - math.atan(x/y)
else:
return 0
def show(theta):
if 0 <= theta <= math.pi/2.0:
return theta + math.pi/2.0
elif math.pi/2.0 < theta <= math.pi:
return theta - math.pi*3/2.0
elif -math.pi <= theta <= 0:
return theta + math.pi/2.0
else:
return theta
def talker(seq_file):
pub = rospy.Publisher('/detection/lidar_detector/objects', det_tracking_array, queue_size=10)
rospy.init_node('lidar_detector_talker', anonymous=True)
rate = rospy.Rate(10)
seq_dets = np.loadtxt(seq_file, delimiter=',', skiprows=1, dtype=np.str_)
if len(seq_dets.shape) == 1:
seq_dets = np.expand_dims(seq_dets, axis=0)
if seq_dets.shape[1] == 0:
return
frame_id = 0
frames = np.unique(seq_dets[:, 0].astype(np.uint64))
cloud_file_name = '/home/shishuai/Develop/Projects/catkin_ws/src/tracking_logs/GT/2012_329523133_ground_5cm_segment.pcd'
pcd = o3d.io.read_point_cloud(cloud_file_name)
points = np.asarray(pcd.points)
print(points.shape)
# points = np.genfromtxt(cloud_file_name, skip_header=11, delimiter=' ', dtype=np.float32).reshape(-1, 6)
# points = np.fromfile(cloud_file_name, dtype=np.float32, count=-1).reshape([-1, 6])
# points = points[:, :3]
cloud = PointCloud2()
cloud.header.stamp = rospy.Time().now()
cloud.header.frame_id = "Pandar64"
if len(points.shape) == 3:
cloud.height = points.shape[1]
cloud.width = points.shape[0]
else:
cloud.height = 1
cloud.width = len(points)
cloud.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
cloud.is_bigendian = False
cloud.point_step = 12
cloud.row_step = cloud.point_step * points.shape[0]
# msg.is_dense = int(np.isfinite(points).all())
cloud.is_dense = False
cloud.data = np.asarray(points, np.float32).tostring()
last_dict = {}
while frame_id < len(frames) and not rospy.is_shutdown():
frame = frames[frame_id]
cur_frame_seq_dets = seq_dets[seq_dets[:, 0].astype(np.uint64) == frame]
msgs = det_tracking_array()
msgs.cloud = cloud
for seq_det in cur_frame_seq_dets:
center_rot_y = 0
if int(seq_det[10]) in last_dict.keys():
l_x, l_y = last_dict[int(seq_det[10])]
dx = float(seq_det[6]) - l_x
dy = float(seq_det[7]) - l_y
if abs(dx) > 0.01 or abs(dy) > 0.01:
center_rot_y = my_atan(dx, dy)
last_dict[int(seq_det[10])] = [float(seq_det[6]), float(seq_det[7])]
msg = det_tracking()
msg.frame = int(seq_det[0])
msg.type = int(seq_det[1])
msg.score = float(seq_det[2])
msg.h = float(seq_det[3])
msg.w = float(seq_det[4])
msg.l = float(seq_det[5])
msg.x = float(seq_det[6])
msg.y = float(seq_det[7])
msg.z = float(seq_det[8])
if float(seq_det[9]) != 10000:
msg.rot_y = float(seq_det[9])
else:
msg.rot_y = my_atan(float(seq_det[11]), float(seq_det[12]))
msg.obj_id = int(seq_det[10])
msg.v_x = float(seq_det[11])
msg.v_y = float(seq_det[12])
msg.v_z = float(seq_det[13])
msg.loc_x = float(seq_det[14])
msg.loc_y = float(seq_det[15])
msg.loc_z = float(seq_det[16])
msgs.array.append(msg)
msgs_info = "msgs: {}".format(msgs.array)
rospy.loginfo(msgs_info)
pub.publish(msgs)
rate.sleep()
frame_id += 1
if __name__ == '__main__':
seq_file = '/home/shishuai/Develop/Projects/catkin_ws/src/tracking_logs/GT/20210427_15:16:04.txt'
try:
talker(seq_file)
except rospy.ROSInterruptException:
pass
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment