fulyvv/pose2sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

all finally works - last tests still to be run

Browse Source
This commit is contained in:
davidpagnon 2024-01-05 14:32:55 +01:00
parent 71648bf051
commit 778b880bad
4 changed files with 21 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Pose2Sim/Demo/S00_Demo_Session/Config.toml
View File

@ -131,7 +131,7 @@ reproj_error_threshold_triangulation = 15 # px
likelihood_threshold_triangulation = 0.3
min_cameras_for_triangulation = 2
interpolation = 'cubic' #linear, slinear, quadratic, cubic, or none
# 'none' if you don't want to interpolate missing points
# 'none' if you don't want to interpolate missing points
interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower

2
Pose2Sim/common.py
View File

@ -129,7 +129,7 @@ def weighted_triangulation(P_all,x_all,y_all,likelihood_all):
V = Vt.T
Q = np.array([V[0][3]/V[3][3], V[1][3]/V[3][3], V[2][3]/V[3][3], 1])
else:
Q = np.array([0.,0.,0.,1])
Q = np.array([np.nan,np.nan,np.nan,1])
return Q

2
Pose2Sim/personAssociation.py
View File

@ -187,7 +187,7 @@ def best_persons_and_cameras_combination(config, json_files_framef, personsIDs_c
error_comb_per_cam.append( euclidean_distance(q_file, q_calc) )
error_comb.append( np.mean(error_comb_per_cam) )
error_min = min(error_comb)
error_min = np.nanmin(error_comb)
persons_and_cameras_combination = combinations_with_cams_off[np.argmin(error_comb)]
if error_min < error_threshold_tracking:

46
Pose2Sim/triangulation.py
View File

@ -326,28 +326,27 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
x_files_swapped_filt[i][id_cams_off[i]] = np.nan
y_files_swapped_filt[i][id_cams_off[i]] = np.nan
likelihood_files_filt[i][id_cams_off[i]] = np.nan
# Excluded cameras index and count
id_cams_off_tot = [np.argwhere(np.isnan(x)).ravel() for x in likelihood_files_filt]
nb_cams_excluded_filt = [np.count_nonzero(np.nan_to_num(x)==0) for x in likelihood_files_filt] # count nans and zeros
nb_cams_off_tot = max(nb_cams_excluded_filt)
if nb_cams_off_tot > n_cams - min_cameras_for_triangulation:
break
if undistort_points:
calib_params_K_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) and not x_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_K_filt) ]
calib_params_dist_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) and not x_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_dist_filt) ]
calib_params_R_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) and not x_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_R_filt) ]
calib_params_T_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) and not x_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_T_filt) ]
projection_matrices_filt = [ [ p[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) and not x_files_filt[j][i]==0. ] for j, p in enumerate(projection_matrices_filt) ]
calib_params_K_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(likelihood_files_filt[j][i]) and not likelihood_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_K_filt) ]
calib_params_dist_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(likelihood_files_filt[j][i]) and not likelihood_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_dist_filt) ]
calib_params_R_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(likelihood_files_filt[j][i]) and not likelihood_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_R_filt) ]
calib_params_T_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(likelihood_files_filt[j][i]) and not likelihood_files_filt[j][i]==0. ] for j, c in enumerate(calib_params_T_filt) ]
projection_matrices_filt = [ [ p[i] for i in range(n_cams) if not np.isnan(likelihood_files_filt[j][i]) and not likelihood_files_filt[j][i]==0. ] for j, p in enumerate(projection_matrices_filt) ]
print('\nnb_cams_off', repr(nb_cams_off), 'nb_cams_excluded', repr(nb_cams_excluded_filt))
nb_cams_off_tot = nb_cams_excluded_filt[0]
id_cams_off_tot = [np.argwhere(np.isnan(x)).ravel() for x in x_files_filt]
print('x_files ', repr(x_files))
print('x_files_filt ', repr(x_files_filt))
print('id_cams_off_tot ', id_cams_off_tot)
x_files_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(xx) and not xx==0. ]) for x in x_files_filt ]
y_files_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(xx) and not xx==0. ]) for x in y_files_filt ]
x_files_swapped_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(xx) and not xx==0. ]) for x in x_files_swapped_filt ]
y_files_swapped_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(xx) and not xx==0. ]) for x in y_files_swapped_filt ]
x_files_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(likelihood_files_filt[i][ii]) and not likelihood_files_filt[i][ii]==0. ]) for i,x in enumerate(x_files_filt) ]
y_files_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(likelihood_files_filt[i][ii]) and not likelihood_files_filt[i][ii]==0. ]) for i,x in enumerate(y_files_filt) ]
x_files_swapped_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(likelihood_files_filt[i][ii]) and not likelihood_files_filt[i][ii]==0. ]) for i,x in enumerate(x_files_swapped_filt) ]
y_files_swapped_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(likelihood_files_filt[i][ii]) and not likelihood_files_filt[i][ii]==0. ]) for i,x in enumerate(y_files_swapped_filt) ]
likelihood_files_filt = [ np.array([ xx for ii, xx in enumerate(x) if not np.isnan(xx) and not xx==0. ]) for x in likelihood_files_filt ]
print('x_files_filt ', repr(x_files_filt))
# Triangulate 2D points
Q_filt = [weighted_triangulation(projection_matrices_filt[i], x_files_filt[i], y_files_filt[i], likelihood_files_filt[i]) for i in range(len(id_cams_off))]
@ -361,7 +360,6 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
coords_2D_kpt_calc_filt = [reprojection(projection_matrices_filt[i], Q_filt[i]) for i in range(len(id_cams_off))]
coords_2D_kpt_calc_filt = np.array(coords_2D_kpt_calc_filt, dtype=object)
x_calc_filt = coords_2D_kpt_calc_filt[:,0]
print('x_calc_filt ', x_calc_filt)
y_calc_filt = coords_2D_kpt_calc_filt[:,1]
# Reprojection error
@ -370,10 +368,9 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
q_file = [(x_files_filt[config_off_id][i], y_files_filt[config_off_id][i]) for i in range(len(x_files_filt[config_off_id]))]
q_calc = [(x_calc_filt[config_off_id][i], y_calc_filt[config_off_id][i]) for i in range(len(x_calc_filt[config_off_id]))]
error.append( np.mean( [euclidean_distance(q_file[i], q_calc[i]) for i in range(len(q_file))] ) )
print('error ', error)
# Choosing best triangulation (with min reprojection error)
error_min = min(error)
error_min = np.nanmin(error)
best_cams = np.argmin(error)
nb_cams_excluded = nb_cams_excluded_filt[best_cams]
@ -385,10 +382,8 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
n_cams_swapped = 1
error_off_swap_min = error_min
while error_off_swap_min > error_threshold_triangulation and n_cams_swapped < (n_cams - nb_cams_off_tot) / 2: # more than half of the cameras switched: may triangulate twice the same side
print('nb_cams_off ', nb_cams_off, 'n_cams_swapped ', n_cams_swapped, 'nb_cams_off_tot ', nb_cams_off_tot)
# Create subsets
id_cams_swapped = np.array(list(it.combinations(range(n_cams-nb_cams_off_tot), n_cams_swapped)))
print('id_cams_swapped ', id_cams_swapped)
x_files_filt_off_swap = np.array([[x] * len(id_cams_swapped) for x in x_files_filt])
y_files_filt_off_swap = np.array([[y] * len(id_cams_swapped) for y in y_files_filt])
for id_off in range(len(id_cams_off)): # for each configuration with nb_cams_off_tot removed
@ -418,19 +413,15 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
y_calc_off_swap = coords_2D_kpt_calc_off_swap[:,:,1]
# Reprojection error
print('x_files_filt_off_swap ', x_files_filt_off_swap)
error_off_swap = []
for id_off in range(len(id_cams_off)):
error_percam = []
for id_swapped, config_swapped in enumerate(id_cams_swapped):
# print(id_off,id_swapped,n_cams,nb_cams_off)
# print(repr(x_files_filt_off_swap))
q_file_off_swap = [(x_files_filt_off_swap[id_off,id_swapped,i], y_files_filt_off_swap[id_off,id_swapped,i]) for i in range(n_cams - nb_cams_off_tot)]
q_calc_off_swap = [(x_calc_off_swap[id_off,id_swapped,i], y_calc_off_swap[id_off,id_swapped,i]) for i in range(n_cams - nb_cams_off_tot)]
error_percam.append( np.mean( [euclidean_distance(q_file_off_swap[i], q_calc_off_swap[i]) for i in range(len(q_file_off_swap))] ) )
error_off_swap.append(error_percam)
error_off_swap = np.array(error_off_swap)
print('error_off_swap ', error_off_swap)
# Choosing best triangulation (with min reprojection error)
error_off_swap_min = np.min(error_off_swap)
@ -585,7 +576,6 @@ def triangulate_all(config):
for f in tqdm(range(*f_range)):
# Get x,y,likelihood values from files
json_tracked_files_f = [json_tracked_files[c][f] for c in range(n_cams)]
print(json_tracked_files_f)
x_files, y_files, likelihood_files = extract_files_frame_f(json_tracked_files_f, keypoints_ids)
# undistort points
@ -648,7 +638,7 @@ def triangulate_all(config):
# Interpolate missing values
if interpolation_kind != 'none':
Q_tot = Q_tot.apply(interpolate_zeros_nans, axis=0, args = [interp_gap_smaller_than, interpolation_kind])
Q_tot.replace(np.nan, 0, inplace=True)
# Q_tot.replace(np.nan, 0, inplace=True)
# Create TRC file
trc_path = make_trc(config, Q_tot, keypoints_names, f_range)