def create_view(self, im_size=(800, 800), dist_max=0.0): """Create a satellite view of the HD map. Usefull to plot trajectories. Args: im_size (tuple, optional): Size of the image Returns: TYPE: CameraModel, Image """ focal_lenght = max(im_size) half_size_pix = max(im_size) / 2 if dist_max < 0.1: for road_mark in self.road_marks: for point in road_mark.point_list: if max(abs(point[0]), abs(point[1])) > dist_max: dist_max = max(abs(point[0]), abs(point[1])) z_cam = float(dist_max * focal_lenght) / float(half_size_pix) # Create camera parameters rot_CF_F = np.identity(3) trans_CF_F = np.array([0.0, 0.0, z_cam]) trans_CF_F.shape = (3, 1) center = (im_size[1] / 2, im_size[0] / 2) camera_matrix = np.array([[focal_lenght, 0, center[0]], [0, focal_lenght, center[1]], [0, 0, 1]], dtype="double") dist_coeffs = np.zeros((4, 1)) # Create camera model cam_model = CameraModel(rot_CF_F, trans_CF_F, camera_matrix, dist_coeffs) # Create blank image image = 255 * np.ones((im_size[0], im_size[1], 3), np.uint8) return cam_model, image
def run_EKF_BM2(show_plot=False): cam_model = CameraModel.read_from_yml( "traj_ext/camera_calib/calib_file/biloxi/biloxi_cam_cfg.yml") image_street = cv2.imread( os.path.join("traj_ext/camera_calib/calib_file/biloxi/biloxi_cam.jpg")) traj = trajectory.Trajectory(1, 'car') x_vel = -8.0 * np.array([0.3, -1.0]) x_vel.shape = (2, 1) dt = 0.1 x_pos = -0.1 * np.array([-20.0, 35.0]) x_pos.shape = (2, 1) list_time_ms = [] for i in range(1000): time_ms = int(i * dt * 1000) list_time_ms.append(time_ms) alpha = 0.1 if i > 200 and i < 500: alpha = -0.1 x_vel_ccurent = copy.copy(x_vel) if i > 20 and i < 60: x_vel_ccurent = 0.0 * x_vel else: R = get_mat_rot(alpha) x_vel = R.dot(x_vel) x_vel_ccurent = copy.copy(x_vel) x_pos = x_pos + x_vel_ccurent * dt psi_rad = (np.arctan2(float(x_vel[1]), float(x_vel[0]))) traj.add_point(time_ms, x_pos[0, 0], x_pos[1, 0], x_vel_ccurent[0, 0], x_vel_ccurent[0, 0], psi_rad) time_ms = list_time_ms[0] traj_point = traj.get_point_at_timestamp(time_ms) pt_pix = cam_model.project_points( np.array([(traj_point.x, traj_point.y, 0.0)])) # Create new traker: psi_rad = (np.arctan2(-float(traj_point.vy), float(traj_point.vx))) v_init = np.sqrt(traj_point.vx * traj_point.vx + traj_point.vy * traj_point.vy) x_init = np.array([traj_point.x, traj_point.y, v_init, psi_rad, 0], np.float64) x_init.shape = (5, 1) P_init, Q, R = EKF_BM2.EKF_BM2_track.get_default_param() track_ekf = EKF_BM2.EKF_BM2_track(Q, R, P_init, 1, 'car') track_ekf.set_x_init(x_init, time_ms) pt_pix.shape = (2, 1) track_ekf._push_pix_meas(time_ms, pt_pix) for index, time_ms in enumerate(list_time_ms[1:]): track_ekf.kf_predict(time_ms) # if not (index > 100 and index < 150): if index % 3 == 0: # Fuse measurement if there is one traj_point = traj.get_point_at_timestamp(time_ms) pt_pix = cam_model.project_points( np.array([(traj_point.x, traj_point.y, 0.0)])) pt_pix.shape = (2, 1) # Fuse measurement: noise = np.random.normal(0, 10, 2) noise.shape = (2, 1) pix_meas = pt_pix + noise track_ekf.kf_fuse(pix_meas, cam_model, time_ms) print('Track_postprocess: Smoothing track_id: {}'.format(0)) track_ekf.smooth(list_time_ms, post_proces=True) traj_smooth = trajectory.Trajectory(1, 'car', color=(0, 0, 255)) for index, t_current_ms in enumerate(list_time_ms): # Getting info from EKF # TO DO: Clean this xy, vxy, phi_rad = track_ekf.get_processed_parameters_smoothed( t_current_ms) if not (xy is None): time_ms = int(t_current_ms) x = xy[0] y = xy[1] vx = vxy[0] vy = vxy[1] psi_rad = phi_rad traj_smooth.add_point(time_ms, x, y, vx, vy, psi_rad) if show_plot: for index, time_ms in enumerate(list_time_ms): print('Index: {} time_ms: {}'.format(index, t_current_ms)) image_current = copy.copy(image_street) image_current, _ = traj.display_on_image(time_ms, image_current, cam_model) image_current, _ = traj_smooth.display_on_image( time_ms, image_current, cam_model) pix_meas_2D = track_ekf.get_2Dpix_meas(time_ms) if not (pix_meas_2D is None): pix_meas = (int(pix_meas_2D[0]), int(pix_meas_2D[1])) image_current = cv2.circle(image_current, pix_meas, 3, (255, 0, 255), -1) smooth_state = track_ekf.get_state_smooth_at_time(time_ms) print(smooth_state.P_cov) cv2.imshow('image_current', image_current) key = cv2.waitKey(0) & 0xFF if key == ord("q"): break return True
def test_camera_calib(): # Create camera model: rot_CF_F = mathutil.eulerAnglesToRotationMatrix([1.13, 0.01, -2.22]) #-np.pi/2 trans_CF_F = np.array([5.0, -2.6, 186.0], dtype=float) trans_CF_F.shape = (3, 1) im_size = (1280, 720) center = (im_size[1] / 2, im_size[0] / 2) focal_lenght = max(im_size) camera_matrix = np.array([[focal_lenght, 0, center[0]], [0, focal_lenght, center[1]], [0, 0, 1]], dtype="double") dist_coeffs = np.zeros((4, 1)) # Create camera model cam_model = CameraModel(rot_CF_F, trans_CF_F, camera_matrix, dist_coeffs) points_FNED = np.array([[ 10., 10., 0.0],\ [-10., -10., 0.0],\ [ 10., -10., 0.0],\ [-10., 10., 0.0]], dtype="double") image_points = np.zeros([1, 2], dtype="double") for i in range(points_FNED.shape[0]): point_FNED = points_FNED[i, :] point_FNED.shape = (3, 1) pt_current = cam_model.project_points(point_FNED) if i == 0: image_points[0, :] = (pt_current[0], pt_current[1]) else: d = np.array([[pt_current[0], pt_current[1]]], dtype='double') image_points = np.append(image_points, d, axis=0) rot_vec_CF_F_calib, trans_CF_F_calib, camera_matrix_calib, dist_coeffs, image_points_reproj = calib_utils.find_camera_params_opt( im_size, image_points, points_FNED) # Convert rotation vector in rotation matrix rot_CF_F_calib = cv2.Rodrigues(rot_vec_CF_F_calib)[0] print('rot_vec_CF_F_calib: ') print(rot_vec_CF_F_calib) # Convert rotation matrix in euler angle: euler_CF_F_calib = mathutil.rotationMatrixToEulerAngles(rot_CF_F_calib) print('euler_CF_F_calib: ') print(euler_CF_F_calib) print('euler_CF_F: ') print(mathutil.rotationMatrixToEulerAngles(rot_CF_F)) # Position of the origin expresssed in CF print('trans_CF_F_calib (position of the origin expressed in CF): ') print(trans_CF_F_calib) print('trans_CF_F (position of the origin expressed in CF): ') print(trans_CF_F) print('camera_matrix_calib') print(camera_matrix_calib) print('camera_matrix') print(camera_matrix) print('image_points:\n {}'.format(image_points)) print('image_points_reproj:\n {}'.format(image_points_reproj))
def run_detection_zone(cam_model_street_path, image_street_path, cam_model_sat_path, image_sat_path, output_name): # Print instructions print("Instruction:") print(" - Click on the image to define the detection zone") print(" - Press 'd' to delete last point") print(" - Press Enter to save the detection file") print(" - Press 'q' to exit without saving detection zone file \n") # Construct camera model cam_model_1 = None if os.path.isfile(cam_model_street_path): cam_model_1 = CameraModel.read_from_yml(cam_model_street_path) else: print('\n[Error]: Camera Street model not found: {}'.format( cam_model_street_path)) return # load the image, clone it, and setup the mouse callback function image_1 = None if os.path.isfile(image_street_path): image_1 = cv2.imread(image_street_path) else: print('\n[Error]: image_street_path is not valid: {}'.format( image_street_path)) return cam_model_2 = None # 注释卫星视角标定模型 if os.path.isfile(cam_model_sat_path): cam_model_2 = CameraModel.read_from_yml(cam_model_sat_path) else: print('\n[Error]: Camera sat model not found: {}'.format( cam_model_sat_path)) return # load the image, clone it, and setup the mouse callback function image_2 = None # 注释卫星视角图片 if os.path.isfile(image_sat_path): image_2 = cv2.imread(image_sat_path) else: print('\n[Error]: image_sat_path is not valid: {}'.format( image_sat_path)) return # Create windows cv2.namedWindow("image_1") if not (image_2 is None): cv2.namedWindow("image_2") pt_image_list = [] cv2.setMouseCallback("image_1", click, param=(pt_image_list, cam_model_1, image_1, cam_model_2, image_2)) cv2.imshow("image_1", image_1) if not (image_2 is None): cv2.imshow("image_2", image_2) # keep looping until the 'q' key is pressed save_zone = False while True: # display the image and wait for a keypress key = cv2.waitKey(1) & 0xFF # if key == ord("c"): # if the 'Enter' key is pressed, end of the program if key == 13: save_zone = True break elif key == ord("q"): return elif key == ord("d"): if len(pt_image_list) > 0: pt_image_list.pop() draw_image(image_1, image_2, cam_model_1, cam_model_2, pt_image_list) # Make sure there is enough point if not len(pt_image_list) > 2: print( '[Error]: Not enough points to define the detection zone (3 points minimum): {}' .format(pt_image_list)) return if not (cam_model_1 is None): model_points_FNED = np.array([]) model_points_FNED.shape = (0, 3) # Convert the point in pixel in point in FNED for index, pt_image in enumerate(pt_image_list): print(pt_image) pos_FNED = cam_model_1.projection_ground(0, pt_image) pos_FNED.shape = (1, 3) model_points_FNED = np.append(model_points_FNED, pos_FNED, axis=0) # Only keep convex hull model_points_FNED_convex = calib_utils.convex_hull(model_points_FNED) det_zone_FNED = det_zone.DetZoneFNED(model_points_FNED_convex) if save_zone: # Define output name output_name_det = output_name + '_detection_zone.yml' output_path = os.path.join(os.path.dirname(cam_model_street_path), output_name_det) # Write Param in YAML file det_zone_FNED.save_to_yml(output_path) model_points_IM = np.array([]) model_points_IM.shape = (0, 2) # Convert the point in pixel in point in FNED for index, pt_image in enumerate(pt_image_list): pt = np.array(pt_image) pt.shape = (1, 2) model_points_IM = np.append(model_points_IM, pt, axis=0) # Only keep convex hull model_points_IM_convex = calib_utils.convex_hull(model_points_IM) det_zone_IM = det_zone.DetZoneImage(model_points_IM_convex) if save_zone: # Define output name output_name_det_im = output_name + '_detection_zone_im.yml' output_im_path = os.path.join(os.path.dirname(cam_model_street_path), output_name_det_im) # Write Param in YAML file det_zone_IM.save_to_yml(output_im_path) print("\nProgram Exit") print("############################################################\n")
'-camera', dest="cam_model_path", type=str, help='Path of the satellite camera model yml', default= 'traj_ext/camera_calib/calib_file/brest/brest_area1_street_cfg.yml') args = parser.parse_args() # Create HD map # hd_map = HDmap.from_csv_berkeley(args.hd_map_path); hd_map = HDmap.from_csv(args.hd_map_path) # hd_map.to_csv('brest_berkeley_hdmap.csv') # Open camera model if os.path.isfile(args.cam_model_path) and os.path.isfile(args.image_path): # Open camera model cam_model = CameraModel.read_from_yml(args.cam_model_path) # Open image image = cv2.imread(args.image_path) # Create image and camera for top-down view else: cam_model, image = hd_map.create_view() image = hd_map.display_on_image(image, cam_model) cv2.imshow('HD map', image) key = cv2.waitKey(0) & 0xFF
def run_inspect_traj(config): # Create output folder output_dir = config.output_dir output_dir = os.path.join(output_dir, 'traj_inspect') os.makedirs(output_dir, exist_ok=True) # Create output sub-folder image_raw_saving_dir = os.path.join(output_dir, 'img_raw') image_annoted_saving_dir = os.path.join(output_dir, 'img_annoted') image_hd_map_saving_dir = os.path.join(output_dir, 'img_hdmap') traj_saving_dir = os.path.join(output_dir, 'csv') os.makedirs(image_raw_saving_dir, exist_ok=True) os.makedirs(image_annoted_saving_dir, exist_ok=True) os.makedirs(image_hd_map_saving_dir, exist_ok=True) os.makedirs(traj_saving_dir, exist_ok=True) # Save the cfg file with the output: try: cfg_save_path = os.path.join(output_dir, 'inspect_traj_cfg.json') with open(cfg_save_path, 'w') as json_file: config_dict = vars(config) json.dump(config_dict, json_file, indent=4) except Exception as e: print('[ERROR]: Error saving config file in output folder:\n') print('{}'.format(e)) return # Check if det data available: det_data_available = os.path.isdir(config.det_dir) print('Detection data: {}'.format(det_data_available)) det_asso_data_available = os.path.isdir(config.det_asso_dir) print('Detection Association data: {}'.format(det_asso_data_available)) track_merge_data_available = os.path.isfile(config.track_merge) print('Detection Merge data: {}'.format(track_merge_data_available)) # Check if trajectory ingore exist: traj_ingore_path = config.traj_ignore if traj_ingore_path == '': traj_ingore_path = config.traj.replace('traj.csv', 'traj_ignore.csv') if not os.path.isfile(traj_ingore_path): print('[ERROR]: Trajectory ignore file not found: {}'.format( traj_ingore_path)) return list_traj_ignore = trajutil.read_traj_ignore_list_csv(traj_ingore_path) # Trajectory reverse exist: traj_reverse_path = config.traj_reverse if traj_reverse_path == '': traj_reverse_path = config.traj.replace('traj.csv', 'traj_reverse.csv') if not os.path.isfile(traj_reverse_path): print('[ERROR]: Trajectory reverse file not found: {}'.format( traj_reverse_path)) return # Agent type correction agenttype_corr_path = config.traj_type_corr if agenttype_corr_path == '': agenttype_corr_path = config.traj.replace('traj.csv', 'traj_type_corr.csv') if not os.path.isfile(agenttype_corr_path): print('[ERROR]: Agent type correction file not found: {}'.format( agenttype_corr_path)) return list_agenttype_correct = AgentTypeCorrect.from_csv(agenttype_corr_path) # Time ignore time_ignore_path = config.traj_time_ignore if time_ignore_path == '': time_ignore_path = config.traj.replace('traj.csv', 'time_ignore.csv') if not os.path.isfile(time_ignore_path): print('[ERROR]: Traj time ignore file not found: {}'.format( time_ignore_path)) return time_ignore_list = TimeIgnore.from_csv(time_ignore_path) # Time list_times_ms_path = config.time if list_times_ms_path == '': list_times_ms_path = config.traj.replace('traj.csv', 'time_traj.csv') if not os.path.isfile(list_times_ms_path): print( '[ERROR]: Traj time file not found: {}'.format(list_times_ms_path)) return list_times_ms = trajutil.read_time_list_csv(list_times_ms_path) # Trajectory file if not os.path.isfile(config.traj): print('[ERROR]: Traj file not found: {}'.format(config.traj)) return print('Reading trajectories:') traj_list = Trajectory.read_trajectory_panda_csv(config.traj) # Traj not merged traj_not_merged_csv_path = config.traj_not_merged if traj_not_merged_csv_path == '': traj_not_merged_csv_path = config.traj.replace('traj.csv', 'traj_not_merged.csv') if not os.path.isfile(traj_not_merged_csv_path): print('[ERROR]: Traj not merged file not found: {}'.format( traj_not_merged_csv_path)) return print('Reading trajectories not merged:') traj_list_not_merged = Trajectory.read_trajectory_panda_csv( traj_not_merged_csv_path) # Open objects cam_model = CameraModel.read_from_yml(config.camera_street) det_zone_FNED = det_zone.DetZoneFNED.read_from_yml(config.det_zone_fned) # Ignore trajectories that are smaller than min_length list_traj_ignore_automatic = [] for traj in list(traj_list): if traj.get_length() < config.min_length: if not (traj.get_id() in list_traj_ignore): list_traj_ignore_automatic.append(traj.get_id()) print('Ignoring Traj {} length {}'.format( traj.get_id(), traj.get_length())) sat_view_available = False sat_view_enable = False if os.path.isfile(config.camera_sat_img) and os.path.isfile( config.camera_sat): cam_model_sat = CameraModel.read_from_yml(config.camera_sat) image_sat = cv2.imread(os.path.join(config.camera_sat_img)) sat_view_available = True # Check if image is directoty image_in_dir = os.path.isdir(config.image_dir) # If image directory, open list of images if image_in_dir: list_img_file = os.listdir(config.image_dir) list_img_file.sort(key=lambda f: int(''.join(filter(str.isdigit, f)))) # Else open the unique image else: image = cv2.imread(config.image_dir) hd_map_mode = False if os.path.isfile(config.hd_map): hd_map_mode = True hd_map = HDmap.from_csv(config.hd_map) cam_model_hdmap, image_hdmap = hd_map.create_view() image_hdmap = hd_map.display_on_image(image_hdmap, cam_model_hdmap) # Shrink det zone for complete det_zone_FNED_complete = None if config.shrink_zone < 1: det_zone_FNED_complete = det_zone_FNED.shrink_zone(config.shrink_zone) for traj in traj_list: if not traj.check_is_complete(det_zone_FNED_complete): if not (traj.get_id() in list_traj_ignore) and not ( traj.get_id() in list_traj_ignore_automatic): print('Track: {} time_ms: {} not complete'.format( traj.get_id(), traj.get_start_trajoint().time_ms)) elif config.shrink_zone > 1: det_zone_FNED_complete = det_zone_FNED.shrink_zone(config.shrink_zone) # Get name sequence name_sequence = os.path.basename(config.traj) name_sequence = name_sequence.split('.')[0] name_sequence = name_sequence.replace('_traj', '') # Remove in the det_zone ignore det_zone_ignore_list = [] for det_zone_ignore_path in config.det_zone_ignore: if os.path.isfile(det_zone_ignore_path): det_zone_FNED_ignore = det_zone.DetZoneFNED.read_from_yml( det_zone_ignore_path) det_zone_ignore_list.append(det_zone_FNED_ignore) only_complete = False skip_value = 1 det_view_enable = False frame_index = 0 saving_mode = False print_instructions() while True: ####################################################### ## Update Information ####################################################### # Read traj ignore list: time_ignore_list = TimeIgnore.from_csv(time_ignore_path) list_traj_ignore_timeignore = [] # if len(time_ignore_list) > 0: # for traj in traj_list: # if traj.check_startend_time_ignore(time_ignore_list): # list_traj_ignore_timeignore.append(traj.get_id()); ####################################################### ## Update Information ####################################################### # Read traj ignore list: list_traj_ignore = trajutil.read_traj_ignore_list_csv(traj_ingore_path) # Read agent corrections list_agenttype_correct = AgentTypeCorrect.from_csv(agenttype_corr_path) for agenttype_correct in list_agenttype_correct: traj = trajutil.get_traj_in_list(traj_list, agenttype_correct.track_id) if not (traj is None): traj.set_agent_type(agenttype_correct.agent_type) ####################################################### ## Show Trajectories ####################################################### # Get Time ms frame_index = mathutil.clip(frame_index, 0, len(list_img_file) - 1) frame_index = mathutil.clip(frame_index, 0, len(list_times_ms) - 1) time_ms = list_times_ms[frame_index] if image_in_dir: img_file_name = list_img_file[frame_index] image_current = cv2.imread( os.path.join(config.image_dir, img_file_name)) else: # Copy image image_current = image if not (image_current is None): print('Showing: frame_id: {} time_ms: {} image: {}'.format( frame_index, time_ms, img_file_name)) display_traj = not TimeIgnore.check_time_inside_list( time_ignore_list, time_ms) # Display traj det_zone_FNED_list = [det_zone_FNED, det_zone_FNED_complete ] + det_zone_ignore_list image_current_traj, det_track_list_current = display_traj_on_image( time_ms, cam_model, image_current, traj_list, display_traj=display_traj, traj_ignore_list_1=list_traj_ignore, traj_ignore_list_2=list_traj_ignore_automatic, traj_ignore_list_3=list_traj_ignore_timeignore, det_zone_FNED_list=det_zone_FNED_list, complete_marker=config.shrink_zone < 1) # Show image cv2.imshow('Trajectory visualizer', image_current_traj) cv2.setMouseCallback("Trajectory visualizer", click_hd_merged, param=[det_track_list_current]) # Display traj image_current_traj_not_merged, det_track_list_not_merged = display_traj_on_image( time_ms, cam_model, image_current, traj_list_not_merged, det_zone_FNED_list=det_zone_FNED_list) # Show image cv2.imshow('Trajectory not merged', image_current_traj_not_merged) cv2.setMouseCallback("Trajectory not merged", click_hd_merged, param=[det_track_list_not_merged]) if sat_view_available: if sat_view_enable: # Display traj image_sat_current_not_merged, _ = display_traj_on_image( time_ms, cam_model_sat, image_sat, traj_list_not_merged, det_zone_FNED_list=det_zone_FNED_list) # Show image cv2.imshow('Sat View not merged', image_sat_current_not_merged) # Display traj image_sat_current, _ = display_traj_on_image( time_ms, cam_model_sat, image_sat, traj_list, display_traj=display_traj, traj_ignore_list_1=list_traj_ignore, traj_ignore_list_2=list_traj_ignore_automatic, traj_ignore_list_3=list_traj_ignore_timeignore, det_zone_FNED_list=det_zone_FNED_list) # Show image cv2.imshow('Sat View merged', image_sat_current) if hd_map_mode: # Display traj image_hdmap_current, det_track_list_hd_merged = display_traj_on_image( time_ms, cam_model_hdmap, image_hdmap, traj_list, display_traj=display_traj, traj_ignore_list_1=list_traj_ignore, traj_ignore_list_2=list_traj_ignore_automatic, traj_ignore_list_3=list_traj_ignore_timeignore, det_zone_FNED_list=det_zone_FNED_list, complete_marker=config.shrink_zone < 1) # Show image cv2.imshow('HD map view merged', image_hdmap_current) cv2.setMouseCallback("HD map view merged", click_hd_merged, param=[det_track_list_hd_merged]) # Display traj image_hdmap_current_not_merged, det_track_list_hd_not_merged = display_traj_on_image( time_ms, cam_model_hdmap, image_hdmap, traj_list_not_merged, det_zone_FNED_list=det_zone_FNED_list) # Show image cv2.imshow('HD map view not merged', image_hdmap_current_not_merged) cv2.setMouseCallback("HD map view not merged", click_hd_not_merged, param=[det_track_list_hd_not_merged]) if saving_mode: img_annoted_name = img_file_name.split( '.')[0] + '_annotated.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_annoted_name)) image_annoted_path = os.path.join(image_annoted_saving_dir, img_annoted_name) cv2.imwrite(image_annoted_path, image_current_traj) print('Saving: frame_id: {} image: {}'.format( frame_index, img_file_name)) image_raw_path = os.path.join(image_raw_saving_dir, img_file_name) cv2.imwrite(image_raw_path, image_current) if hd_map_mode: img_hdmap_name = img_file_name.split('.')[0] + '_hdmap.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_hdmap_name)) image_hdmap_path = os.path.join(image_hd_map_saving_dir, img_hdmap_name) cv2.imwrite(image_hdmap_path, image_hdmap_current) ####################################################### ## Show Detections ####################################################### det_csv_path = '' if det_view_enable and det_data_available: image_current_det = copy.copy(image_current) # CSV name management det_csv_name = img_file_name.split('.')[0] + '_det.csv' # det_csv_path = os.path.join(config.det_dir, 'csv'); det_csv_path = config.det_dir det_csv_path = os.path.join(det_csv_path, det_csv_name) det_object_list = DetObject.from_csv(det_csv_path, expand_mask=True) track_id_text = False if det_asso_data_available: track_id_text = True det_asso_csv_name = img_file_name.split( '.')[0] + '_detassociation.csv' # det_asso_csv_path = os.path.join(config.det_asso_dir, 'csv') det_asso_csv_path = config.det_asso_dir det_asso_csv_path = os.path.join(det_asso_csv_path, det_asso_csv_name) try: det_asso_list = detect_utils.read_dict_csv( det_asso_csv_path) except Exception as e: print("ERROR: Could not open csv: {}".format(e)) det_asso_list = [] for det_object in det_object_list: for det_asso in det_asso_list: track_id = det_asso['track_id'] det_id = det_asso['det_id'] if det_id == det_object.det_id: det_object.track_id = track_id for det in det_object_list: det.display_on_image(image_current_det, track_id_text=track_id_text) # Show image detection cv2.imshow('Detection', image_current_det) # Set callback to enable / disable detections cv2.setMouseCallback("Detection", click_detection, param=[ det_object_list, det_csv_path, image_current, track_id_text, config.label_replace ]) else: print('Not found: frame_id: {} image: {}'.format( frame_index, img_file_name)) if config.export: # Export trajectorie export_trajectories(list_times_ms, traj_list, list_traj_ignore_list=[ list_traj_ignore, list_traj_ignore_automatic, list_traj_ignore_timeignore ], time_ignore_list=time_ignore_list, det_zone_ignore_list=det_zone_ignore_list, name_sequence=name_sequence, traj_saving_dir=traj_saving_dir, location_name=config.location_name, date=config.date, start_time=config.start_time, delta_ms=config.delta_ms) #Exit the program break ####################################################### ## Control keys ####################################################### key = cv2.waitKey(0) & 0xFF if key == ord("n"): frame_index += skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("b"): frame_index += 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("p"): frame_index -= skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("o"): frame_index -= 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) # Only complete traj elif key == ord("f"): if config.shrink_zone < 1: only_complete = not only_complete print('Mode: Display complete trajectroy only: {}'.format( only_complete)) # Escape: Quit the program elif key == 27: break # Escape: Quit the program elif key == ord('z'): if sat_view_available: if sat_view_enable: cv2.destroyWindow('Sat View not merged') cv2.destroyWindow('Sat View merged') sat_view_enable = False else: sat_view_enable = True elif key == ord("+"): skip_value += 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("-"): skip_value -= 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("d"): if det_data_available: if det_view_enable: cv2.destroyWindow('Detection') det_view_enable = False else: det_view_enable = True else: print('[Error]: Detection data not available in: {}'.format( config.det_dir)) elif key == ord("a"): if det_data_available and det_view_enable: image_current_create_det = copy.copy(image_current) det_object_list_new, save_flag = run_create_det_object.create_detection( image_current_create_det, config.label_replace, frame_name=img_file_name, frame_id=frame_index, det_object_list=det_object_list) if save_flag: det_object_list = det_object_list_new print('Saving detections: {}'.format(det_csv_path)) DetObject.to_csv(det_csv_path, det_object_list) elif key == ord("c"): subprocess.call(["subl", "--new-window", agenttype_corr_path]) elif key == ord("m"): if track_merge_data_available: subprocess.call(["subl", "--new-window", config.track_merge]) elif key == ord("r"): subprocess.call(["subl", "--new-window", traj_reverse_path]) elif key == ord("w"): if det_view_enable: if det_data_available: subprocess.call(["subl", "--new-window", det_csv_path]) if det_asso_data_available: subprocess.call( ["subl", "--new-window", det_asso_csv_path]) elif key == ord("i"): subprocess.call(["subl", "--new-window", traj_ingore_path]) elif key == ord("t"): subprocess.call(["subl", "--new-window", time_ignore_path]) elif key == ord("e"): # Export trajectorie export_trajectories(list_times_ms, traj_list, list_traj_ignore_list=[ list_traj_ignore, list_traj_ignore_automatic, list_traj_ignore_timeignore ], time_ignore_list=time_ignore_list, det_zone_ignore_list=det_zone_ignore_list, name_sequence=name_sequence, traj_saving_dir=traj_saving_dir, location_name=config.location_name, date=config.date, start_time=config.start_time, delta_ms=config.delta_ms) else: print_instructions()
def run_visualize_conflict_and_traj( config, Thread=None): # 模仿yolo_gpu_bugged.py加入 thread 参数,关注是否存在进程 # Create output folder output_dir = config.output_dir output_dir = os.path.join(output_dir, 'visualizer') os.makedirs(output_dir, exist_ok=True) # Create output sub-folder image_raw_saving_dir = os.path.join(output_dir, 'img_raw') image_annoted_saving_dir = os.path.join(output_dir, 'img_annoted') image_hd_map_saving_dir = os.path.join(output_dir, 'img_hdmap') image_concat_saving_dir = os.path.join(output_dir, 'img_concat') # 用于保存画了 冲突点 的图片 image_concat_with_conflict_point_saving_dir = os.path.join( output_dir, 'img_concat_with_conflict') os.makedirs(image_raw_saving_dir, exist_ok=True) os.makedirs(image_annoted_saving_dir, exist_ok=True) os.makedirs(image_hd_map_saving_dir, exist_ok=True) os.makedirs(image_concat_saving_dir, exist_ok=True) os.makedirs(image_concat_with_conflict_point_saving_dir, exist_ok=True) # Save the cfg file with the output: try: cfg_save_path = os.path.join(output_dir, 'visualize_traj_cfg.json') with open(cfg_save_path, 'w') as json_file: config_dict = vars(config) json.dump(config_dict, json_file, indent=4) except Exception as e: print('[ERROR]: Error saving config file in output folder:\n') print('{}'.format(e)) return #Person trajectories traj_person_list = [] traj_person_available = os.path.isfile(config.traj_person) if traj_person_available: traj_person_list = Trajectory.read_trajectory_panda_csv( config.traj_person) # Open objects cam_model = CameraModel.read_from_yml(config.camera_street) det_zone_FNED = det_zone.DetZoneFNED.read_from_yml(config.det_zone_fned) # Time list_times_ms_path = config.time if list_times_ms_path == '': list_times_ms_path = config.traj.replace('traj.csv', 'time_traj.csv') if not os.path.isfile(list_times_ms_path): print( '[ERROR]: Traj time file not found: {}'.format(list_times_ms_path)) return list_times_ms = trajutil.read_time_list_csv(list_times_ms_path) # Trajectories traj_list = Trajectory.read_trajectory_panda_csv(config.traj) # 读取Conflict_list conflict_list = Conflict.read_conflict_panda_csv(config.conflicts) sat_view_available = False sat_view_enable = False if os.path.isfile(config.camera_sat) and os.path.isfile( config.camera_sat_img): cam_model_sat = CameraModel.read_from_yml(config.camera_sat) image_sat = cv2.imread(os.path.join(config.camera_sat_img)) sat_view_available = True # Check if image is directoty image_in_dir = os.path.isdir(config.image_dir) # If image directory, open list of images if image_in_dir: list_img_file = os.listdir(config.image_dir) list_img_file.sort(key=lambda f: int(''.join(filter(str.isdigit, f)))) # Else open the unique image else: image = cv2.imread(config.image_dir) hd_map_available = os.path.isfile(config.hd_map) if hd_map_available: hd_map = HDmap.from_csv(config.hd_map) cam_model_hdmap, image_hdmap = hd_map.create_view() image_hdmap = hd_map.display_on_image(image_hdmap, cam_model_hdmap) # Shrink det zone for complete det_zone_FNED_complete = None if config.shrink_zone < 1: det_zone_FNED_complete = det_zone_FNED.shrink_zone(config.shrink_zone) for traj in traj_list: if not traj.check_is_complete(det_zone_FNED_complete): print('Track: {} time_ms: {} not complete'.format( traj.get_id(), traj.get_start_trajoint().time_ms)) skip_value = 1 frame_index = 0 export_mode = False # If export mode is direclty asked if config.export: export_mode = True while True: ####################################################### ## Show Trajectories And Conflict ####################################################### # Get Time ms frame_index = mathutil.clip(frame_index, 0, len(list_times_ms) - 1) #剪辑返回的是帧序,保证帧序在0到最大之间 time_ms = list_times_ms[frame_index] # 读取当前的时刻,用于遍历所有存在轨迹的时间段 if image_in_dir: img_file_name = list_img_file[frame_index] image_current = cv2.imread( os.path.join(config.image_dir, img_file_name)) else: # Copy image image_current = image if (not (image_current is None)) & ( not Thread.pause): # 加上 (not Thread.pause)是为了在暂停时也停下画图 print('Showing: frame_id: {} image: {}'.format( frame_index, img_file_name)) Thread.signalCanvas("Showing: frame_id: {} image: {}".format( frame_index, img_file_name)) # Display traj ################# 改为展示从0到当前time_ms所有点画在图上 # 要实现速度标签打上去,加上“velocity_label = True” image_current_traj, _ = run_inspect_traj.display_conflict_traj_on_image( time_ms, cam_model, image_current, traj_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label, velocity_label=True) image_current_traj, _ = run_inspect_traj.display_conflict_traj_on_image( time_ms, cam_model, image_current_traj, traj_person_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label, velocity_label=True) # 在图片上画出轨迹线? # Show image # cv2.imshow('Trajectory visualizer', image_current_traj) # if sat_view_available: # # if sat_view_enable: # # # Display traj # image_sat_current, _ = run_inspect_traj.display_traj_on_image(time_ms, cam_model_sat, image_sat, traj_list, det_zone_FNED_list = [det_zone_FNED], no_label = config.no_label); # image_sat_current, _ = run_inspect_traj.display_traj_on_image(time_ms, cam_model_sat, image_sat_current, traj_person_list, det_zone_FNED_list = [det_zone_FNED], no_label = config.no_label) # # Show image # cv2.imshow('Sat View merged', image_sat_current) # 为了降低耦合性,分为两个函数写 # Display Conflicts # 设置要画冲突点的图片 image_traj_with_conflict = image_current_traj # 判断当前 整个视频 是否存在冲突点,若冲突数组(即冲突csv文件为空)不为空,继续 if len(conflict_list) != 0: for per_ms_conflict_list in conflict_list: # 如果当前时刻存在冲突数组,则循环画上去,画 点 和 线 以及标 TTC if per_ms_conflict_list[0].get_time_ms() == time_ms: # print('画' + str(time_ms) + 'ms 时刻冲突点\n\n\n') for conflict_point in per_ms_conflict_list: image_traj_with_conflict = conflict_point.display_conflict_point_on_image(time_ms, \ cam_model, \ image_traj_with_conflict, \ traj_list, \ TTC_label = True,\ is_hd_map = False) Thread.signalCanvas(str(time_ms) + "ms 时刻检测到冲突,TTC:{} s, 冲突车1的x坐标:{},冲突车1的y坐标{}," "冲突车2的x坐标:{},冲突车1的y坐标{}\n"\ .format(round(float(conflict_point.get_TTC()), 2), \ round(float(conflict_point.get_point_A_x()), 2),\ round(float(conflict_point.get_point_A_y()), 2),\ round(float(conflict_point.get_point_B_x()), 2),\ round(float(conflict_point.get_point_B_y()), 2))) if hd_map_available: # Display traj image_hdmap_current, _ = run_inspect_traj.display_traj_on_image(time_ms,\ cam_model_hdmap,\ image_hdmap,\ traj_list,\ det_zone_FNED_list = [det_zone_FNED],\ no_label = config.no_label,\ velocity_label=True) image_hdmap_current, _ = run_inspect_traj.display_traj_on_image(time_ms,\ cam_model_hdmap,\ image_hdmap_current,\ traj_person_list,\ det_zone_FNED_list = [det_zone_FNED],\ no_label = config.no_label,\ velocity_label=True) # # Show image # cv2.imshow('HD map view merged', image_hdmap_current) # 模仿上面的函数在 俯视 视角的图片中画 冲突点 image_hd_traj_with_conflict = image_hdmap_current # 判断当前 整个视频 是否存在冲突点,若冲突数组(即冲突csv文件为空)不为空,继续 if len(conflict_list) != 0: for per_ms_conflict_list in conflict_list: # 如果当前时刻存在冲突数组,则循环画上去,画 点 和 线 以及标 TTC if per_ms_conflict_list[0].get_time_ms() == time_ms: # print('画' + str(time_ms) + 'ms 时刻俯视冲突点\n\n\n') for conflict_point in per_ms_conflict_list: image_hd_traj_with_conflict = conflict_point.display_conflict_point_on_image(time_ms,\ cam_model_hdmap,\ image_hd_traj_with_conflict,\ traj_list,\ TTC_label=True, \ is_hd_map = True) image_concat = EKF_utils.concatenate_images( image_traj_with_conflict, image_hd_traj_with_conflict) # 隐藏了结合视图 # cv2.imshow('View: Camera and HD map', image_concat) if export_mode: img_annoted_name = img_file_name.split( '.')[0] + '_annotated.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_annoted_name)) image_annoted_path = os.path.join(image_annoted_saving_dir, img_annoted_name) cv2.imwrite(image_annoted_path, image_traj_with_conflict) print('Saving: frame_id: {} image: {}'.format( frame_index, img_file_name)) image_raw_path = os.path.join(image_raw_saving_dir, img_file_name) cv2.imwrite(image_raw_path, image_current) if hd_map_available: img_hdmap_name = img_file_name.split('.')[0] + '_hdmap.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_hdmap_name)) image_hdmap_path = os.path.join(image_hd_map_saving_dir, img_hdmap_name) cv2.imwrite(image_hdmap_path, image_hd_traj_with_conflict) img_concat_name = img_file_name.split( '.')[0] + '_concat.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_concat_name)) Thread.signalCanvas( 'Saving: frame_id: {} image: {}'.format( frame_index, img_concat_name)) image_concat_path = os.path.join(image_concat_saving_dir, img_concat_name) cv2.imwrite(image_concat_path, image_concat) # 发送信号给meau_controller,在窗口实现图片展示 if not Thread.pause: # 线程未暂停 Thread.signalImg(image_concat_path) # 如何实现线程的暂停与再次启动 elif Thread.pause: # 若是因为暂停 a = 1 # print('暂停检测') # time.sleep(3) else: print('Not found: frame_id: {} image: {}'.format( frame_index, img_file_name)) if Thread.kill: # 若点击“结束”,则结束循环 break ####################################################### ## Control keys ####################################################### if frame_index == (len(list_times_ms) - 1): print('Export view: Done') Thread.signalCanvas('Export view: Done') export_mode = False # 展示所有 检测冲突数据 print_conflicts_result(conflict_list, Thread) # Exit when it is done if in export directly mode if config.export: break wait_key = 0 if export_mode & (not Thread.pause): # 暂停,不再读取下一帧 frame_index += 1 wait_key = 1 key = cv2.waitKey(wait_key) & 0xFF if key == ord("n"): frame_index += skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("b"): frame_index += 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("p"): frame_index -= skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("o"): frame_index -= 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) # Escape: Quit the program elif key == 27: break # Escape: Quit the program elif key == ord('z'): if sat_view_available: if sat_view_enable: cv2.destroyWindow('Sat View merged') sat_view_enable = False else: sat_view_enable = True elif key == ord("+"): skip_value += 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("-"): skip_value -= 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("e"): if not export_mode: export_mode = True frame_index = 0 print('Mode: Export mode started') else: export_mode = False print('Mode: Export mode stopped') elif key == 255: pass else: print('\nInstruction:\n- n: Next frame\ \n- b: Jump 1000 frame forward\ \n- p: Previous frame\ \n- b: Jump 1000 frame backward\ \n- +: Increase skip value\ \n- -: Decrease skip value\ \n- d: Open detection window\ \n- c: Open Agent type correction\ \n- m: Open merging file with sublime text\ \n- s: Enable saving form current frame\ \n- Click on Detection window: Enable/disable detections\ \n- f: Display only complete trajectories\ \n- i: Open ignore trajectory file\ \n- e: Export trajectory file\ \n- esc: Quit\ \n')
def run_calib_manual(calib_points_path, image_path, sat_mode, output_folder, auto_save=False): ################################################################### # INPUT VALUES ################################################################### # Read csv: try: data_csv = read_csv(calib_points_path) except Exception as e: print('[Error]: reading calib points {}'.format(e)) return # If no csv or not enough data: if not data_csv or len(data_csv) < 4: print( '[Error]: Not enough data in the input csv (minimum 4 points) {}'. format(calib_points_path)) return # Detect if in latlon or Cartesian mode: csv_latlon = False if 'lat_deg' in data_csv[0]: print('Mode: csv in latlon mode') csv_latlon = True # Create the pixel points vector from csv image_points = np.zeros([1, 2], dtype="double") first_init = True for data in data_csv: if first_init: image_points[0, :] = (data['pixel_x'], data['pixel_y']) first_init = False else: d = np.array([[data['pixel_x'], data['pixel_y']]], dtype='double') image_points = np.append(image_points, d, axis=0) # In lat/lon mode, input are given in Latitude/Longitude (degrees) if csv_latlon: # Create the latlon points vector from csv latlon_points = np.zeros([1, 2], dtype="double") first_init = True # Go through line of the csv for data in data_csv: # For the first line, fill directly the first row of latlon_points if first_init: latlon_points[0, :] = (data['lat_deg'], data['lon_deg']) latlon_origin = np.array( [data['origin_lat_deg'], data['origin_lon_deg']], dtype='double') first_init = False # For the next row, append new row to latlon_points else: d = np.array([[data['lat_deg'], data['lon_deg']]], dtype='double') latlon_points = np.append(latlon_points, d, axis=0) # Convert the lat-lon array in F_NED array of points model_points_F = calib_utils.convert_latlon_F(latlon_origin, latlon_points) else: # Create the model_points_F points vector from csv model_points_F = np.zeros([1, 3], dtype="double") first_init = True for data in data_csv: if first_init: model_points_F[0, :] = (data['pos_x'], data['pos_y'], 0.0) first_init = False else: d = np.array([[data['pos_x'], data['pos_y'], 0.0]], dtype='double') model_points_F = np.append(model_points_F, d, axis=0) # Satellite Mode: print("Satellite Mode: {}\n".format(sat_mode)) print("Calibration details:\n") # Open image im = cv2.imread(image_path) if im is None: raise ValueError('[ERROR]: Image path is not valid: {}'.format()) # Project a 3D point (0, 0, 1000.0) onto the image plane. # We use this to draw a line sticking out of the nose im_size = im.shape # Getting Image size rot_vec_CF_F, trans_CF_F, camera_matrix, dist_coeffs, image_points_reproj = calib_utils.find_camera_params_opt( im_size, image_points, model_points_F, sat_mode) # Convert rotation vector in rotation matrix rot_CF_F = cv2.Rodrigues(rot_vec_CF_F)[0] print('rot_CF_F: ') print(rot_CF_F) # Convert rotation matrix in euler angle: euler_CF_F = mathutil.rotationMatrixToEulerAngles(rot_CF_F) print('euler_CF_F: ') print(euler_CF_F) # Position of the origin expresssed in CF print('trans_CF_F (position of the origin expressed in CF): ') print(trans_CF_F) cam_model = CameraModel(rot_CF_F, trans_CF_F, camera_matrix, dist_coeffs) im = cam_model.display_NED_frame(im) im = calib_utils.display_keypoints(im, image_points_reproj, image_points) # Save image with key-points cv2.imshow("Output", im) print("\nInstruction:") print(" - Press Enter to save the calibration file") print( " - Press any other key to exit without saving calibration file \n") save_bool = False if auto_save: save_bool = True else: key = cv2.waitKey(0) # if the 'Enter' key is pressed, end of the program if key == 13: save_bool = True if not os.path.isdir(output_folder): output_folder = os.path.dirname(image_path) if save_bool: # Save the calibration points with the camera model calib_points_path_name = calib_points_path.split('/')[-1] try: shutil.copyfile( calib_points_path, os.path.join(output_folder, calib_points_path_name)) except Exception as e: print('[Error]: Copying point file: {}'.format(e)) # Save the camera model cam_file_name = image_path.split('/')[-1] cam_file_name = cam_file_name.split('.')[0] + '_cfg.yml' output_path = os.path.join(output_folder, cam_file_name) cam_model.save_to_yml(output_path) # Save the camera model im_calib_file_name = image_path.split('/')[-1] im_calib_file_name = cam_file_name.split('.')[0] + '_calib.png' im_calib_path = os.path.join(output_folder, im_calib_file_name) cv2.imwrite(im_calib_path, im) print('\n Image config file saved %s \n' % (im_calib_path)) cv2.destroyAllWindows() print("Program Exit\n") print("############################################################\n")
def run_visualize_traj(config): # Create output folder output_dir = config.output_dir output_dir = os.path.join(output_dir, 'visualizer') os.makedirs(output_dir, exist_ok=True) # Create output sub-folder image_raw_saving_dir = os.path.join(output_dir, 'img_raw') image_annoted_saving_dir = os.path.join(output_dir, 'img_annoted') image_hd_map_saving_dir = os.path.join(output_dir, 'img_hdmap') image_concat_saving_dir = os.path.join(output_dir, 'img_concat') os.makedirs(image_raw_saving_dir, exist_ok=True) os.makedirs(image_annoted_saving_dir, exist_ok=True) os.makedirs(image_hd_map_saving_dir, exist_ok=True) os.makedirs(image_concat_saving_dir, exist_ok=True) # Save the cfg file with the output: try: cfg_save_path = os.path.join(output_dir, 'visualize_traj_cfg.json') with open(cfg_save_path, 'w') as json_file: config_dict = vars(config) json.dump(config_dict, json_file, indent=4) except Exception as e: print('[ERROR]: Error saving config file in output folder:\n') print('{}'.format(e)) return #Person trajectories traj_person_list = [] traj_person_available = os.path.isfile(config.traj_person) if traj_person_available: traj_person_list = Trajectory.read_trajectory_panda_csv( config.traj_person) # Open objects cam_model = CameraModel.read_from_yml(config.camera_street) det_zone_FNED = det_zone.DetZoneFNED.read_from_yml(config.det_zone_fned) # Time list_times_ms_path = config.time if list_times_ms_path == '': list_times_ms_path = config.traj.replace('traj.csv', 'time_traj.csv') if not os.path.isfile(list_times_ms_path): print( '[ERROR]: Traj time file not found: {}'.format(list_times_ms_path)) return list_times_ms = trajutil.read_time_list_csv(list_times_ms_path) # Trajectories traj_list = Trajectory.read_trajectory_panda_csv(config.traj) sat_view_available = False sat_view_enable = False if os.path.isfile(config.camera_sat) and os.path.isfile( config.camera_sat_img): cam_model_sat = CameraModel.read_from_yml(config.camera_sat) image_sat = cv2.imread(os.path.join(config.camera_sat_img)) sat_view_available = True # Check if image is directoty image_in_dir = os.path.isdir(config.image_dir) # If image directory, open list of images if image_in_dir: list_img_file = os.listdir(config.image_dir) list_img_file.sort(key=lambda f: int(''.join(filter(str.isdigit, f)))) # Else open the unique image else: image = cv2.imread(config.image_dir) hd_map_available = os.path.isfile(config.hd_map) if hd_map_available: hd_map = HDmap.from_csv(config.hd_map) cam_model_hdmap, image_hdmap = hd_map.create_view() image_hdmap = hd_map.display_on_image(image_hdmap, cam_model_hdmap) # Shrink det zone for complete det_zone_FNED_complete = None if config.shrink_zone < 1: det_zone_FNED_complete = det_zone_FNED.shrink_zone(config.shrink_zone) for traj in traj_list: if not traj.check_is_complete(det_zone_FNED_complete): print('Track: {} time_ms: {} not complete'.format( traj.get_id(), traj.get_start_trajoint().time_ms)) skip_value = 1 frame_index = 0 export_mode = False # If export mode is direclty asked if config.export: export_mode = True while True: ####################################################### ## Show Trajectories ####################################################### # Get Time ms frame_index = mathutil.clip(frame_index, 0, len(list_times_ms) - 1) time_ms = list_times_ms[frame_index] if image_in_dir: img_file_name = list_img_file[frame_index] image_current = cv2.imread( os.path.join(config.image_dir, img_file_name)) else: # Copy image image_current = image if not (image_current is None): print('Showing: frame_id: {} image: {}'.format( frame_index, img_file_name)) # Display traj image_current_traj, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model, image_current, traj_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label) image_current_traj, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model, image_current_traj, traj_person_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label) # Show image cv2.imshow('Trajectory visualizer', image_current_traj) if sat_view_available: if sat_view_enable: # Display traj image_sat_current, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model_sat, image_sat, traj_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label) image_sat_current, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model_sat, image_sat_current, traj_person_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label) # Show image cv2.imshow('Sat View merged', image_sat_current) if hd_map_available: # Display traj image_hdmap_current, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model_hdmap, image_hdmap, traj_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label, velocity_label=True) image_hdmap_current, _ = run_inspect_traj.display_traj_on_image( time_ms, cam_model_hdmap, image_hdmap_current, traj_person_list, det_zone_FNED_list=[det_zone_FNED], no_label=config.no_label, velocity_label=True) # Show image cv2.imshow('HD map view merged', image_hdmap_current) image_concat = EKF_utils.concatenate_images( image_current_traj, image_hdmap_current) cv2.imshow('View: Camera and HD map', image_concat) if export_mode: img_annoted_name = img_file_name.split( '.')[0] + '_annotated.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_annoted_name)) image_annoted_path = os.path.join(image_annoted_saving_dir, img_annoted_name) cv2.imwrite(image_annoted_path, image_current_traj) print('Saving: frame_id: {} image: {}'.format( frame_index, img_file_name)) image_raw_path = os.path.join(image_raw_saving_dir, img_file_name) cv2.imwrite(image_raw_path, image_current) if hd_map_available: img_hdmap_name = img_file_name.split('.')[0] + '_hdmap.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_hdmap_name)) image_hdmap_path = os.path.join(image_hd_map_saving_dir, img_hdmap_name) cv2.imwrite(image_hdmap_path, image_hdmap_current) img_concat_name = img_file_name.split( '.')[0] + '_concat.png' print('Saving: frame_id: {} image: {}'.format( frame_index, img_concat_name)) image_concat_path = os.path.join(image_concat_saving_dir, img_concat_name) cv2.imwrite(image_concat_path, image_concat) else: print('Not found: frame_id: {} image: {}'.format( frame_index, img_file_name)) ####################################################### ## Control keys ####################################################### if frame_index == (len(list_times_ms) - 1): print('Export view: Done') export_mode = False # Exit when it is done if in export directly mode if config.export: break wait_key = 0 if export_mode: frame_index += 1 wait_key = 1 key = cv2.waitKey(wait_key) & 0xFF if key == ord("n"): frame_index += skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("b"): frame_index += 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("p"): frame_index -= skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) elif key == ord("o"): frame_index -= 1000 * skip_value mathutil.clip(frame_index, 0, len(list_times_ms)) # Escape: Quit the program elif key == 27: break # Escape: Quit the program elif key == ord('z'): if sat_view_available: if sat_view_enable: cv2.destroyWindow('Sat View merged') sat_view_enable = False else: sat_view_enable = True elif key == ord("+"): skip_value += 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("-"): skip_value -= 1 skip_value = max(1, skip_value) print('Skip value: {}'.format(skip_value)) elif key == ord("e"): if not export_mode: export_mode = True frame_index = 0 print('Mode: Export mode started') else: export_mode = False print('Mode: Export mode stopped') elif key == 255: pass else: print('\nInstruction:\n- n: Next frame\ \n- b: Jump 1000 frame forward\ \n- p: Previous frame\ \n- b: Jump 1000 frame backward\ \n- +: Increase skip value\ \n- -: Decrease skip value\ \n- d: Open detection window\ \n- c: Open Agent type correction\ \n- m: Open merging file with sublime text\ \n- s: Enable saving form current frame\ \n- Click on Detection window: Enable/disable detections\ \n- f: Display only complete trajectories\ \n- i: Open ignore trajectory file\ \n- e: Export trajectory file\ \n- esc: Quit\ \n')
def main(): # Arg parser: parser = argparse.ArgumentParser(description='Visualize the trajectories') parser.add_argument( '-det_zone', dest="det_zone_path", type=str, help='Path to the detection zone yml', default= 'traj_ext/camera_calib/calib_file/brest/brest_area1_detection_zone.yml' ) parser.add_argument( '-shrink_zone', dest="shrink_zone", type=float, help='Detection zone shrink coefficient for complete trajectories', default=1) parser.add_argument( '-camera', dest="cam_model_path", type=str, help='Path of the camera model yml', default= 'traj_ext/camera_calib/calib_file/brest/brest_area1_street_cfg.yml') parser.add_argument( '-image', dest="image_path", type=str, help='Path of the image', default='traj_ext/camera_calib/calib_file/brest/brest_area1_street.jpg' ) args = parser.parse_args() # Read from yml det_zone_FNED = det_zone.DetZoneFNED.read_from_yml(args.det_zone_path) # Srhrink det_zone_FNED_shrinked = det_zone_FNED.shrink_zone(args.shrink_zone) # Get image image_current = cv2.imread(args.image_path) # Read cam model cam_model = CameraModel.read_from_yml(args.cam_model_path) save_flag = False while True: image_current = det_zone_FNED.display_on_image(image_current, cam_model, thickness=1) image_current = det_zone_FNED_shrinked.display_on_image(image_current, cam_model, thickness=1) cv2.imshow('Image', image_current) key = cv2.waitKey(0) & 0xFF if key == 13: save_flag = True break elif key == ord("q"): break if save_flag: # Get path: det_zone_FNED_shrinked_path = args.det_zone_path.replace( '.yml', '_shrinked_' + ('%i' % (args.shrink_zone * 100)) + '.yml') # Save to yml det_zone_FNED_shrinked.save_to_yml(det_zone_FNED_shrinked_path) # Create det_zone_image det_zone_im_shrinked = det_zone_FNED_shrinked.create_det_zone_image( cam_model) det_zone_im_shrinked_path = args.det_zone_path.replace( '.yml', '_shrinked_' + ('%i' % (args.shrink_zone * 100)) + '_im.yml') det_zone_im_shrinked.save_to_yml(det_zone_im_shrinked_path)