def run_conflict_detection(config):
# Create output folder
output_dir = config.output_dir
output_dir = os.path.join(output_dir, 'detected_conflict')
os.makedirs(output_dir, exist_ok=True)
# 读取轨迹
traj_list = Trajectory.read_trajectory_panda_csv(config.traj)
# 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)
# 参数初始化,从0时刻至视频结束均检测冲突
frame_index = 0
# 所有时刻所有的冲突
conflict_list = []
# 读取每一帧画面的轨迹，进行冲突检测
while True:
#######################################################
## detect conflict
#######################################################
# Get Time ms
frame_index = mathutil.clip(frame_index, 0,
len(list_times_ms) -
1) # 剪辑返回的是帧序，保证帧序在0到最大之间
time_ms = list_times_ms[frame_index] # 读取当前的时刻，用于遍历所有存在轨迹的时间段
# 两两组合下所有两条轨迹组合遍历检测冲突
for i, traj_i in enumerate(traj_list):
# i 从 0 到 n-2 即 遍历 轨迹1 至轨迹 n - 1
if i >= 0 and i <= len(traj_list) - 2:
# 判断当前时刻 轨迹 i 若存在轨迹点，则继续
if not (traj_i.get_point_at_timestamp(time_ms) is None):
for j, traj_j in enumerate(traj_list):
if j >= i + 1 and j <= len(traj_list) - 1:
# 判断当前时刻 轨迹 j 若存在轨迹点， 则继续
if not (traj_j.get_point_at_timestamp(time_ms) is
None):
# 修改冲突检测的函数
conflict_point, TTC, conflict_type = two_vehicle_conflict_detect(
traj_i, traj_j, time_ms)
# 若存在冲突（TTC 不为 9999）, 保存当前冲突到 list
if TTC != 9999 and conflict_point.x != 0 and conflict_point.y != 0:
# 把当前的冲突数据 写入 Conflict 类
tmp_data = Conflict(time_ms, conflict_point.x, conflict_point.y,\
TTC, traj_i.get_id(), traj_j.get_id(), conflict_type)
# 将冲突写入数组
conflict_list.append(tmp_data)
# 滴滴发出声音，报警有冲突
winsound.Beep(500,
500) # 第一个参数是音调 ，第二个参数是持续时间
time.sleep(0.5)
winsound.Beep(500, 500)
print(time_ms,'ms时，轨迹',traj_i.get_id(),'与轨迹',traj_j.get_id(),\
'存在冲突！冲突类型为' + conflict_type + '\n\n\n')
# 完成所有时刻检测
if frame_index == (len(list_times_ms) - 1):
name_prefix = 'conflict_detect'
# 完成所有时刻冲突检测后保存冲突list到csv
Conflict.write_conflict_panda_csv(output_dir, name_prefix,
conflict_list, list_times_ms)
print('冲突检测完成！请查看文件')
break
# 当前时刻的冲突检测完成， 进行下一个时刻的检测
else:
frame_index += 1
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_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 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 main():
# Arg parser:
parser = argparse.ArgumentParser(description='Visualize the detections')
parser.add_argument('-image',
dest="image_path",
type=str,
help='Path of the image',
default='dataset/test/varna_20190125_153327_0_900/img')
parser.add_argument(
'-det',
dest="det_dir_path",
type=str,
help='Path to the detection folder',
default=
'dataset/test/varna_20190125_153327_0_900/output/test_output/det_crop')
parser.add_argument('-label',
dest="label_list",
nargs='*',
type=str,
help='List of label to show: boat airplane',
default=['traffic light'])
parser.add_argument('-label_replace',
dest="label_replace",
type=str,
help='Label used to replace labels',
default='car')
args = parser.parse_args()
print('\nShow detections: {}'.format(args.label_list))
print('Replace by: {}\n'.format(args.label_replace))
# Check if det data available:
det_data_available = os.path.isdir(args.det_dir_path)
print('Detection data: {}'.format(det_data_available))
if len(args.label_list) < 1:
print('[ERROR]: Label list empty: {}'.format(args.label_list))
return
# Check if image is directoty
image_in_dir = os.path.isdir(args.image_path)
# If image directory, open list of images
if image_in_dir:
list_img_file = os.listdir(args.image_path)
list_img_file.sort(key=lambda f: int(''.join(filter(str.isdigit, f))))
# Else open the unique image
else:
image = cv2.imread(args.image_path)
frame_index = 0
step = 1
while True:
#######################################################
## Show Trajectories
#######################################################
# Get Time ms
frame_index = mathutil.clip(frame_index, 0,
len(list_img_file) - 1)
if image_in_dir:
img_file_name = list_img_file[frame_index]
image_current = cv2.imread(
os.path.join(args.image_path, img_file_name))
else:
# Copy image
image_current = image
print('Showing: frame_id: {} image: {}'.format(frame_index,
img_file_name))
#######################################################
## Show Detections
#######################################################
label_detected = False
det_csv_path = ''
image_current_det = copy.copy(image_current)
det_object_list = []
if det_data_available:
# CSV name management
det_csv_name = img_file_name.split('.')[0] + '_det.csv'
det_csv_path = os.path.join(args.det_dir_path, 'csv')
det_csv_path = os.path.join(det_csv_path, det_csv_name)
det_object_list = DetObject.from_csv(det_csv_path,
expand_mask=True)
for det_object in det_object_list:
if det_object.label in args.label_list:
det_object.display_on_image(image_current_det)
label_detected = True
if label_detected or frame_index == 0 or frame_index == len(
list_img_file) - 1 or step == 0:
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, args.label_list,
args.label_replace
])
wait_key = 0
key = cv2.waitKey(wait_key) & 0xFF
if key == ord("q"):
break
elif key == ord("n"):
step = 1
elif key == ord("p"):
step = -1
frame_index += step
mathutil.clip(frame_index, 0, len(list_img_file))