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)