def __init__(self): super(MemTrack, self).__init__("MemTrack") config_proto = tf.ConfigProto() config_proto.gpu_options.allow_growth = True sess = tf.Session(config=config_proto) ckpt = tf.train.get_checkpoint_state(path_config.MEMTRACK_MODEL) model = Model(sess, ckpt.model_checkpoint_path) self.tracker = Tracker(model)
class MemTrack(BaseTracker): def __init__(self): super(MemTrack, self).__init__("MemTrack") config_proto = tf.ConfigProto() config_proto.gpu_options.allow_growth = True sess = tf.Session(config=config_proto) ckpt = tf.train.get_checkpoint_state(path_config.MEMTRACK_MODEL) model = Model(sess, ckpt.model_checkpoint_path) self.tracker = Tracker(model) def initialize(self, image_file, box): self.tracker.initialize(image_file, box) def track(self, image_file): bbox, _ = self.tracker.track(image_file) return bbox
def __init__(self, mqtt_master, model, camera_front, camera_down): self.master = mqtt_master self.model = model self.cam_handler_front = camera_front self.cam_handler_down = camera_down self.detector = Detector(self.model) self.tracker = Tracker(self.model) self.tracker.reset() self.avoider = AvoidObject() self.avoid_start_time = None self.already_avoided = 0 self._policy()
def run_tracker(): config_proto = tf.ConfigProto() config_proto.gpu_options.allow_growth = True with tf.Graph().as_default(), tf.Session(config=config_proto) as sess: os.chdir('../') model = Model(sess) tracker = Tracker(model) init_rect, s_frames = load_seq_config('Basketball') bbox = init_rect res = [] res.append(bbox) start_time = time.time() tracker.initialize(s_frames[0], bbox) for idx in range(1, len(s_frames)): tracker.idx = idx bbox, cur_frame = tracker.track(s_frames[idx]) display_result(cur_frame, bbox, idx) res.append(bbox.tolist()) end_time = time.time() type = 'rect' fps = idx / (end_time - start_time) return res, type, fps
from inferences.non_optimized import Network from tracking.tracker import Tracker import cv2 import os BASE_DIR = "/".join(os.path.realpath(__file__).split("/")[:-1]) trk_thresh = 0.4 det_thresh = 0.5 net = Network() video = BASE_DIR + "/video/Pedestrian_Detect_2_1_1.mp4" video = cv2.VideoCapture(video) FPS = video.get(cv2.CAP_PROP_FPS) Trk = Tracker(trk_thresh,FPS) while True: _,frame = video.read() if not _: break detections = net.predict(frame,det_thresh,False,10) image, conteo = Trk.track_dets(detections,frame) print("Personas ingresadas:",conteo) cv2.imshow("win",image) cv2.waitKey(1) cv2.destroyAllWindows()
class Policy: STRAIGHT_THRESH_BRICK = 400 STRAIGHT_THRESH_BOX = 330 IGNORE_OBSTACLE_THRESH = 150 BASELINE = 30 FACTOR = 0.3 THRES_R = lambda y: 320 + Policy.BASELINE + Policy.FACTOR * (480 - y) THRES_L = lambda y: 320 - Policy.BASELINE - Policy.FACTOR * (480 - y) MOVEMENT_STATE_LEFT = "move_left" MOVEMENT_STATE_RIGHT = "move_right" MOVEMENT_STATE_STRAIGHT = "move_straight" MOVEMENT_STATE_APPROACH = "move_approach" POLICY_STATE_SCANNING = "scanning" POLICY_STATE_ON_TARGET = "on_target" POLICY_STATE_APPROACHING = "approaching" POLICY_STATE_AVOID = "avoid" POLICY_STATE_PICK_UP_DURING_AVOIDANCE = "pick_up_during_avoidance" GRIPPER_STATE_EMPTY = "gripper_empty" GRIPPER_STATE_FULL = "gripper_full" AVOID_STRAIGHT_FOR = 10 def __init__(self, mqtt_master, model, camera_front, camera_down): self.master = mqtt_master self.model = model self.cam_handler_front = camera_front self.cam_handler_down = camera_down self.detector = Detector(self.model) self.tracker = Tracker(self.model) self.tracker.reset() self.avoider = AvoidObject() self.avoid_start_time = None self.already_avoided = 0 self._policy() def _policy(self): policy_state = Policy.POLICY_STATE_SCANNING gripper_state = Policy.GRIPPER_STATE_EMPTY cluster_id = -1 while True: if policy_state == Policy.POLICY_STATE_SCANNING: print("scanning") if gripper_state == Policy.GRIPPER_STATE_EMPTY: policy_state = self._scan_policy(assess_fn=self._assess_brick) else: policy_state = self._scan_policy(assess_fn=lambda: self._assess_box(cluster_id + 1)) elif policy_state == Policy.POLICY_STATE_ON_TARGET: if gripper_state == Policy.GRIPPER_STATE_EMPTY: policy_state = self._go_to_target_policy(assess_fn=self._assess_brick, straight_thresh=Policy.STRAIGHT_THRESH_BRICK) else: policy_state = self._go_to_target_policy(assess_fn=lambda: self._assess_box(cluster_id + 1), straight_thresh=Policy.STRAIGHT_THRESH_BOX) elif policy_state == Policy.POLICY_STATE_APPROACHING: if gripper_state == Policy.GRIPPER_STATE_EMPTY: assert gripper_state == Policy.GRIPPER_STATE_EMPTY, "Wrong state - gripper not empty for pick up" policy_state, cluster_id = self._pick_up_brick() gripper_state = Policy.GRIPPER_STATE_FULL else: assert gripper_state == Policy.GRIPPER_STATE_FULL, "Wrong state - gripper not full for release" policy_state = self._put_brick_into_box() gripper_state = Policy.GRIPPER_STATE_EMPTY elif policy_state == Policy.POLICY_STATE_AVOID: policy_state = self._avoid_policy(pick_up_during_avoidance=(gripper_state == Policy.GRIPPER_STATE_EMPTY)) elif policy_state == Policy.POLICY_STATE_PICK_UP_DURING_AVOIDANCE: assert gripper_state == Policy.GRIPPER_STATE_EMPTY, "Wrong state - gripper not empty for pick up" policy_state, cluster_id = self._pick_up_brick() policy_state = Policy.POLICY_STATE_AVOID gripper_state = Policy.GRIPPER_STATE_FULL else: assert False, "Unknown State" def _put_brick_into_box(self): self.master.blocking_call(Master.to_slave, 'stop_motion()') self.master.blocking_call(Master.to_slave, 'raise_lift()') self.master.blocking_call(Master.to_slave, 'approach_brick()') self.master.blocking_call(Master.to_slave, 'open_gripper()') self.master.blocking_call(Master.to_slave, 'move_straight(-1000, 1000)') self.master.blocking_call(Master.to_slave, 'turn_right(180)') self.master.blocking_call(Master.to_slave, 'raise_lift_driving()') return Policy.POLICY_STATE_SCANNING def _pick_up_brick(self): self.master.blocking_call(Master.to_slave, 'stop_motion()') self.master.blocking_call(Master.to_slave, 'lower_lift()') cluster_id = self._approach_brick() self.master.blocking_call(Master.to_slave, 'close_gripper()') self.master.blocking_call(Master.to_slave, 'raise_lift_driving()') return Policy.POLICY_STATE_SCANNING, cluster_id def _approach_brick(self): self.master.blocking_call(Master.to_slave, "move_forever()") while True: cluster_id = self.detector.detect_brick_gripper(self.cam_handler_down.get_next_frame()) if cluster_id is not None: print("Cluster", cluster_id) break self.master.blocking_call(Master.to_slave, "stop_motion()") return cluster_id def _assess_brick(self): img = self.cam_handler_front.get_next_frame() lego_map, obstacle_map = self.model.extract_hmap(img) obstacle = self.avoider.getObstacle(obstacle_map) cv2.imshow('Legomap',lego_map) print("Obstacle brick: ",obstacle) if obstacle is not None: return AssessResult(object=AssessResult.OBJECT_OBSTACLE, point=obstacle[0], max_y=obstacle[1]) else: target = self.tracker.track(lego_map) if target is not None: return AssessResult(object=AssessResult.OBJECT_TARGET_BRICK, point=target) else: return AssessResult(object=AssessResult.OBJECT_NONE) def _assess_box(self, cid): assert cid >= 0, "Invalid Cluster ID" acc_point = np.zeros(2) not_none = 0 iters = 3 img = self.cam_handler_front.get_next_frame() _, obstacle_map = self.model.extract_hmap(img, True) obstacle = self.avoider.getObstacle(obstacle_map) target = Detector.detect_box(img, cid) if target is not None: not_none += 1 acc_point += target print("Obstacle box: ",obstacle) for _ in range(iters): print("Request_frame") img = self.cam_handler_front.get_next_frame() target = Detector.detect_box(img, cid) '''if the chilitag is far away we can avoid obstacles''' if target is not None: if obstacle is not None and target[1] < Policy.IGNORE_OBSTACLE_THRESH: return AssessResult(object=AssessResult.OBJECT_OBSTACLE, point=obstacle[0], max_y=obstacle[1]) not_none += 1 acc_point += target if not_none == 0: return AssessResult(object=AssessResult.OBJECT_NONE) else: return AssessResult(object=AssessResult.OBJECT_TARGET_BOX, point=acc_point / not_none) def _start_approach(self, x, y, straight_thresh): return y > straight_thresh and Policy.THRES_L(y) < x < Policy.THRES_R(y) def _avoid_policy(self, pick_up_during_avoidance): self.master.blocking_call(Master.to_slave, 'turn_left_forever()') while True: print("turning") img = self.cam_handler_front.get_next_frame() _, obstacle_map = self.model.extract_hmap(img) obstacle = self.avoider.getObstacle(obstacle_map) if obstacle is None: break print("Moving past the obstacle") self.avoid_start_time = time.time() self.master.blocking_call(Master.to_slave, 'move_forever()') while time.time() - self.avoid_start_time < Policy.AVOID_STRAIGHT_FOR - self.already_avoided: if pick_up_during_avoidance: print("approach while avoid") self.tracker.reset() res = self._assess_brick() if res.object == AssessResult.OBJECT_TARGET_BRICK and self._start_approach(res.point[0], res.point[1], Policy.STRAIGHT_THRESH_BRICK): self.already_avoided = time.time() - self.avoid_start_time return Policy.POLICY_STATE_PICK_UP_DURING_AVOIDANCE print("Done moving") self.already_avoided = 0 return Policy.POLICY_STATE_SCANNING def _go_to_target_policy(self, assess_fn, straight_thresh): state = '' while True: res = assess_fn() if res.object == AssessResult.OBJECT_TARGET_BRICK or res.object == AssessResult.OBJECT_TARGET_BOX: x = res.point[0] y = res.point[1] if self._start_approach(x, y, straight_thresh): return Policy.POLICY_STATE_APPROACHING elif x > Policy.THRES_R(y): if state != Policy.MOVEMENT_STATE_RIGHT: state = Policy.MOVEMENT_STATE_RIGHT print('changed to right') self.master.blocking_call(Master.to_slave, 'turn_right_forever()') elif x < Policy.THRES_L(y): if state != Policy.MOVEMENT_STATE_LEFT: print('changed to left') state = Policy.MOVEMENT_STATE_LEFT self.master.blocking_call(Master.to_slave, 'turn_left_forever()') else: if state != Policy.MOVEMENT_STATE_STRAIGHT: print('changed to straight') state = Policy.MOVEMENT_STATE_STRAIGHT self.master.blocking_call(Master.to_slave, 'move_forever()') elif res.object == AssessResult.OBJECT_OBSTACLE: print("Obstacle detected") return Policy.POLICY_STATE_AVOID else: return Policy.POLICY_STATE_SCANNING def _scan_policy(self, assess_fn): turning = False while True: print("scan policy") res = assess_fn() if not turning: self.master.blocking_call(Master.to_slave, 'turn_right_forever()') turning = True if res.object == AssessResult.OBJECT_TARGET_BRICK: start = time.time() run_time = start old_point = res.point print('Overshooting') while run_time-start<3: run_time = time.time() self.tracker.reset() res = assess_fn() print(old_point) print(res.point) if res.point is None or old_point[1]>res.point[1]: start = time.time() run_time = start self.master.blocking_call(Master.to_slave, 'turn_left_forever()') while run_time-start<3: res = assess_fn() run_time = time.time() return Policy.POLICY_STATE_ON_TARGET elif res.object == AssessResult.OBJECT_TARGET_BOX: return Policy.POLICY_STATE_ON_TARGET
def demo(save=True, use_cluster=True): file_names = ['4p-c0', '4p-c1', '4p-c2', '4p-c3'] video_files = [ os.path.join(currentUrl, 'data', 'train', 'lab', file_name+'.avi')\ for file_name in file_names ] detection_files = [ os.path.join(currentUrl, 'data', 'train', 'lab', file_name+'.pickle')\ for file_name in file_names ] model_file = os.path.join(currentUrl, 'box2vec', 'model', 'model-86000') cluster_distance_threshold = 0.7 print('video_files:') print(video_files) print('detection_files:') print(detection_files) caps, detections, video_length = get_caps_and_pickles( video_files, detection_files) if save: fourcc = cv2.VideoWriter_fourcc(*'XVID') now_str = str(datetime.now()).replace(':', '-')[:-7] cwd = os.getcwd() save_dir = os.path.join(cwd, 'result', now_str) os.mkdir(save_dir) video_names = [ os.path.join(save_dir, 'c_' + str(i) + '.avi') for i in range(len(caps)) ] outers = [ cv2.VideoWriter(video_names[i], fourcc, 30.0, (360, 288)) for i in range(len(caps)) ] box_to_vect, sess = init_box_to_vect_net(model_file) tracker = Tracker(traj_smoth_alpha=0.99, image_boundary=None, lost_times_thresh=30, lost_times_thresh_edge=3, appear_times_thresh=30, assoc_score_thresh=0.7, cost_metric='distance') for frame_id in range(100, video_length): print('-' * 60) print('frame_id:', frame_id) frames, boxes = get_frames_and_boxes(caps, detections, frame_id, show=False) embedding = get_embedding(boxes, frames, box_to_vect, sess) if embedding is None: continue if embedding.shape[0] > 1: embedding_tsne = TSNE().fit_transform(embedding) plt.scatter(embedding_tsne[:, 0], embedding_tsne[:, 1]) plt.pause(1) plt.close('all') if use_cluster: cluster_set, fused_embeddings = fusion_cluster( bboxes=embedding, method='cluster', distance_threshold=1, nms_threshold=0.3) else: distance = l2_distance(embedding, embedding) print('distance:') print(distance) cluster_set = fusion(distance, distance_threshold=cluster_distance_threshold) print('cluster_sets:', len(cluster_set.sets)) fused_embeddings = [] for embedding_ids in cluster_set.sets: clustered_embedding = embedding[embedding_ids, :] fused_embeddings.append(np.mean(clustered_embedding, axis=0)) fused_embeddings = np.concatenate(fused_embeddings, axis=0) fused_embeddings = np.reshape(fused_embeddings, [-1, 128]) print('fused_embeddings:') print(fused_embeddings.shape) tracker.update_tracker(candidate_bboxes_original=fused_embeddings, time_stamp=frame_id) obj_to_traj = {} for valid_index in tracker.real_alive_index: print('valid_index:', valid_index) traj = tracker.trajectories[valid_index] current_object_id = traj.object_id[-1] traj_serial_num = tracker.whole_real_alive_index.index(valid_index) obj_to_traj[current_object_id] = traj_serial_num #print(traj_serial_num, ':', current_object_id) print(obj_to_traj) global_object_id_to_traj_id = [-1] * embedding.shape[0] for obj_id, cluster in enumerate(cluster_set.sets): for global_object_id in cluster: if obj_id in obj_to_traj: global_object_id_to_traj_id[ global_object_id] = obj_to_traj[obj_id] global_object_id = 0 for camera_id in range(4): frame = frames[camera_id] local_bbox = boxes[camera_id] for box in local_bbox: traj_id = global_object_id_to_traj_id[global_object_id] # print(traj_id) np.random.seed(traj_id + 10) color = np.random.randint(256, size=3) cv2.rectangle(frame, (box[0], box[1]), (box[2], box[3]), (int(color[0]), int(color[1]), int(color[2])), 1) global_object_id += 1 cv2.putText(frame, str(traj_id), (box[0], box[3]),\ cv2.FONT_HERSHEY_COMPLEX, 0.8, (int(color[0]), int(color[1]), int(color[2])), thickness = 2, lineType = -1) cv2.putText(frame, str(frame_id), (0, 20),\ cv2.FONT_HERSHEY_COMPLEX, 0.8, (0, 0, 255), thickness=2, lineType=-1) # print(frame.shape) if save: outers[camera_id].write(frame) cv2.imshow('camera' + str(camera_id), frame) cv2.waitKey(1)
def main(args, dataset, load_results, frame_range, interpolate, write_images, oracle=None): output_dir = osp.join(get_output_dir(args.out_dir, args.name)) if not osp.exists(output_dir): os.makedirs(output_dir) ########################## # Initialize the modules # ########################## # object detection print("Initializing object detector(s).") obj_detects = [] #FIXME: Bruno fix this to reflect DETRa if not args.use_detr: obj_detect = FRCNN_FPN(num_classes=2) obj_detect.load_state_dict( torch.load(args.obj_detect_model, map_location="cpu")) else: obj_detect = build(args) obj_detect.eval() if torch.cuda.is_available(): obj_detect.cuda() obj_detects.append(obj_detect) # reid print("Initializing reID network(s).") reid_networks = [] reid_cfg = os.path.join(os.path.dirname(args.reid_model), 'sacred_config.yaml') reid_cfg = yaml.safe_load(open(reid_cfg)) reid_network = ReIDNetwork_resnet50(pretrained=False, **reid_cfg['model_args']) reid_network.load_state_dict( torch.load(args.reid_model, map_location="cpu")) reid_network.eval() if torch.cuda.is_available(): reid_network.cuda() reid_networks.append(reid_network) #FIXME: where is tracker coming from # tracktor # if oracle is not None: # tracker = OracleTracker( # obj_detect, reid_network, tracker, oracle) # else: tracker = Tracker(obj_detect, reid_network, vars(args)) if args.profile: import torch.autograd.profiler as profiler time_total = 0 num_frames = 0 mot_accums = [] dset_file = dataset dataset = Datasets(dataset) print(dataset) for seq, obj_detect, reid_network in zip(dataset, cycle(obj_detects), cycle(reid_networks)): print("IN da loop once") obj_detect.eval() tracker.obj_detect = obj_detect tracker.reid_network = reid_network tracker.reset() print(f"Tracking: {seq}") start_frame = int(frame_range['start'] * len(seq)) end_frame = int(frame_range['end'] * len(seq)) seq_loader = DataLoader( torch.utils.data.Subset(seq, range(start_frame, end_frame))) num_frames += len(seq_loader) results = {} if load_results: print("Reading results") results = seq.load_results(output_dir) if not results: start = time.time() with torch.no_grad(): for frame_data in tqdm(seq_loader): tracker.step(frame_data) results = tracker.get_results() time_total += time.time() - start print(f"Tracks found: {len(results)}") print(f"Runtime for {seq}: {time.time() - start :.2f} s.") if interpolate: results = interpolate_tracks(results) print(f"Writing predictions to: {output_dir}") seq.write_results(results, output_dir) if seq.no_gt: print("No GT data for evaluation available.") else: mot_accums.append(get_mot_accum(results, seq_loader)) if write_images: plot_sequence(results, seq, osp.join(output_dir, str(dataset), str(seq)), write_images) if time_total: print( f"Tracking runtime for all sequences (without evaluation or image writing): " f"{time_total:.2f} s for {num_frames} frames ({num_frames / time_total:.2f} Hz)" ) if mot_accums: print("Evaluation:") print(mot_accums) evaluate_mot_accums(mot_accums, [str(s) for s in dataset if not s.no_gt], generate_overall=True)
R = np.array([[s_xx_R, s_xy_R], [s_xy_R, s_yy_R]]) measurement = DataGenerator( segments=[ Steady(duration=40.0, heading=0.5 * math.pi, abs_speed=1.0), Turn(duration=15.0, abs_speed=1.0, turnrate=0.2), Turn(duration=15.0, abs_speed=1.0, turnrate=-0.2) ] ) tracker = Tracker(filter_factory=filter_factory) np.random.seed(42) dt = 1.0 for _ in range(200): z = measurement.draw(dt=dt, R=R) tracker.filter(dt=dt, z=z, R=R) for i in range(1, 2): tracker.filter(dt=0.0, z=z + np.array([10.0 * i, 10.0 * i]), R=R) tracker.plot_all(vertical=False, dim=2) tracker.show()