def generate_trainmsgs(bag_file, tracklet_file):
obs_generator = None
frame = 0
order_checker = OrderChecker(ordercheck=True)
if tracklet_file is not None:
tracklets = parse_tracklet.parse_xml(tracklet_file)
assert (1 == len(tracklets))
tracklet = tracklets[0]
assert (0 == tracklet.first_frame)
obs_generator = evaluate_tracklets.generate_obstacles(tracklets)
msg_generator = sensor.generate_sensormsgs(bag_file)
synced_generator = generate_syncedmsgs(msg_generator)
if obs_generator is None:
for img, lidar in synced_generator:
sample = TrainMsg(None, img, lidar)
order_checker.check_sample(sample)
yield sample
else:
for obs, (img, lidar) in itertools.izip(obs_generator,
synced_generator):
sample = TrainMsg(obs, img, lidar)
order_checker.check_sample(sample)
yield sample
def __init__(self, tracklet_file):
self.offset = [0, 0, 0]
self.rotation_offset = [0, 0, 0, 1]
self.orient_offset = (0, 0, 0, 1)
self.tracklets = parse_tracklet.parse_xml(tracklet_file)
self.collection = TrackletCollection()
self.ground_corr = None
def __init__(self):
self.image_sub = rospy.Subscriber("/image_raw", Image, self.imageCb)
self.true_car_markers_pub = rospy.Publisher("/true_car_markers",
Marker,
queue_size=100)
# Parse tracklet_labels.xml for this xml_file
self.tracklets_list = pt.parse_xml(xml_file)
self.car_tracklet = self.tracklets_list[0]
self.car_tracklet_ctr = 0
self.car_tracklet_num_frames = len(self.car_tracklet.trans)
def load_tracklets(folder_path, xml_filename='tracklet_labels.xml'):
xml_filepath = os.path.join(folder_path, xml_filename)
if os.path.exists(xml_filepath):
print("Loading tracklets from '%s'" % xml_filepath)
tracklets = pt.parse_xml(xml_filepath)
for i in range(len(tracklets)):
tracklet = tracklets[i]
h, w, l = tracklet.size
print("%s. %s" % ((i + 1), tracklet.object_type))
print(" LxWxH: %s x %s x %s" % (l, w, h))
print(" Frame0: %s " % (tracklet.first_frame))
print(" Frames: %s " % (tracklet.num_frames))
print(" Start: (%s, %s, %s) " % tuple(tracklet.trans[0]))
print(" End: (%s, %s, %s) " % tuple(tracklet.trans[-2]))
return tracklets
def start_read(self, bag_file, tracklet_file):
if not self.empty():
raise RuntimeError(
'Cannot start read because read is in progress.')
self.reset()
if tracklet_file is not None:
tracklets = parse_tracklet.parse_xml(tracklet_file)
assert (1 == len(tracklets))
self.tracklet = tracklets[0]
assert (0 == self.tracklet.first_frame)
self.msg_queue.start_read(bag_file, warmup_timeout_secs=15)
def __init__(self, basedir, date, drive, yaw_correction=0., xml_filename="tracklet_labels_refined.xml", flip=False, box_scaling=(1.,1.,1.)):
self.basedir = basedir
self.date = date
self.drive = drive
self.kitti_data = pykitti.raw(basedir, date, drive,
range(0, 1)) # , range(start_frame, start_frame + total_frames))
self.xml_path = os.path.join(basedir, date, drive)
self.tracklet_data = tracklets.parse_xml(os.path.join(self.xml_path, xml_filename))
# correct yaw in all frames if yaw_correction provided
if yaw_correction is not 0.:
assert len(self.tracklet_data) == 1 # only one tracklet supported for now!
for t in self.tracklet_data:
for frame_offset in range(t.first_frame, t.first_frame + t.num_frames):
idx = frame_offset - t.first_frame
t.rots[idx][2] += yaw_correction
self.kitti_data.load_calib() # Calibration data are accessible as named tuples
# lidars is a dict indexed by frame: e.g. lidars[10] = np(N,4)
self.lidars = {}
# images is a dict indexed by frame: e.g. lidars[10] = np(SY,SX,3)
self.images = {}
self.im_dim = (1242, 375) # by default
# boxes is a dict indexed by frame: e.g. boxes[10] = [box, box, ...]
self._boxes = None # defaultdict(list)
self._last_refined_box = None
self._box_scaling = box_scaling
reference_file = os.path.join(basedir, date, 'obs.txt')
if os.path.isfile(reference_file):
if flip:
print("Flipping")
else:
print("Not flipping")
self.reference = self.__load_reference(reference_file, flip)
else:
self.reference = None
self._init_boxes(only_with=None)
def __init__(self):
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/image_raw", Image, self.imageCb, queue_size = 1000)
self.laser_sub = rospy.Subscriber("/velodyne_points", PointCloud2, self.laserCb, queue_size = 1000)
# LASER CB
self.gen_car_markers_pub = rospy.Publisher("gen_car_markers", Marker, queue_size = 1000)
self.true_car_tracklet_size = [1.397, 4.5212, 4.5212]
# preprocess tracklets
if testing_mode == False:
self.true_tracklet_collection = pt.parse_xml(truexml_path)
self.true_car_tracklet = self.true_tracklet_collection[0]
self.true_car_tracklet_ctr = 0
# Set up tracklet generator
self.gen_tracklet_collection = gt.TrackletCollection()
self.gen_car_tracklet = gt.Tracklet(object_type="Car", l=self.true_car_tracklet_size[2],
w=self.true_car_tracklet_size[2], h=self.true_car_tracklet_size[0], first_frame=0)
self.gen_tracklet_collection.tracklets.append(self.gen_car_tracklet)
# bbox settings
self.bbox_length = self.true_car_tracklet_size[2]
self.bbox_height = self.true_car_tracklet_size[0]
# current sensor data
self.cur_image = None
self.cur_image_ctr = 0
self.cur_laser = None
self.cur_laser_ctr = 0
# current state of obs_car
self.im_gen_obs_pose = [0,0,0]
self.laser_gen_obs_pose = [0,0,0]
self.combo_gen_obs_pose = [0,0,0]
self.marker_base = Marker()
self.fill_marker_base()
from itertools import combinations
from sklearn.utils.linear_assignment_ import linear_assignment
from scipy.cluster.hierarchy import linkage, fcluster
import os
import pickle
from sort_car import Sort
bag_name = 'ford01'
bag_file = os.path.join(
'/home/stu/competition_data/didi_dataset/round2/test_car',
bag_name + '.bag')
tracklet_dir = './'
tracklet_file = os.path.join(tracklet_dir, 'ori', bag_name + '.xml')
new_tracklet_file = os.path.join(tracklet_dir, 'corrected', bag_name + '.xml')
tracklets = parse_xml(tracklet_file)
bag = rosbag.Bag(bag_file)
print('Reading timestamps from bag ', bag_file)
n_stamps = bag.get_message_count(topic_filters=['/image_raw'])
timestamps, radar_timestamps = [], []
radar_tracks = {}
for topic, msg, t in bag.read_messages(topics=['/image_raw', '/radar/tracks']):
if topic == '/image_raw':
timestamps.append(t.to_nsec())
else:
radar_time = t.to_nsec()
radar_timestamps.append(radar_time)
radar_tracks[radar_time] = []
for track in msg.tracks:
if (track.status != 3) or (track.range > 60) or (track.range < 4):
def read_tracklet(path):
tracklets = pt.parse_xml(path)
assert (len(tracklets) == 1)
return tracklets[0]
bbox = Bbox()
bbox.x, bbox.y, bbox.z = f.trans
bbox.h, bbox.w, bbox.l = f.size
bbox.score=1.
bboxArray.bboxes.append(bbox)
time.sleep(0.3) #simulate MV3D delay
self.detect_pub.publish(bboxArray)
rospy.logerr('detect_pub bboxArray={} '.format(bboxArray))
self.is_busy=False
#------------------------------------------------------------
if __name__ == "__main__" :
parser = argparse.ArgumentParser(description="tracker")
parser.add_argument('bag', type=str, nargs='?', default='', help='bag filename')
parser.add_argument('tracklet', type=str, nargs='?', default='', help='tracklet filename')
args = parser.parse_args(rospy.myargv()[1:])
bag_file = args.bag
bag_dir = os.path.dirname(bag_file)
tracklet_file = args.tracklet
assert os.path.isfile(bag_file), 'Bag file %s does not exist' % bag_file
assert os.path.isfile(tracklet_file), 'Tracklet file %s does not exist' % tracklet_file
tracklets = parse_xml(tracklet_file)
try :
tracker = Tracker(bag_file, tracklets) # TODO : remove tracklets, use detector node instead
tracker.startlistening()
except rospy.ROSInterruptException:
pass
def __init__(self, tracklet_file):
self.offset = [0,0,0]
self.rotation_offset = [0,0,0,1]
self.orient = (0,0,0,1)
self.tracklets = parse_tracklet.parse_xml(tracklet_file)
self.collection = TrackletCollection()