From 7b58454712318d113fe090426109f43831714f8e Mon Sep 17 00:00:00 2001 From: davidpagnon Date: Fri, 26 Jul 2024 16:50:51 +0200 Subject: [PATCH] Moving cameras supported (to be added to the Pose2Sim main pipeline) --- Pose2Sim/Utilities/reproj_from_trc_calib.py | 135 +++++++++++--------- 1 file changed, 76 insertions(+), 59 deletions(-) diff --git a/Pose2Sim/Utilities/reproj_from_trc_calib.py b/Pose2Sim/Utilities/reproj_from_trc_calib.py index 3dd6ee9..6963412 100644 --- a/Pose2Sim/Utilities/reproj_from_trc_calib.py +++ b/Pose2Sim/Utilities/reproj_from_trc_calib.py @@ -11,10 +11,12 @@ toml calibration file. The output 2D points can be chosen to follow the DeepLabCut (default) or - the OpenPose format. If OpenPose is chosen, the BODY_25B model is used, + the OpenPose format. If OpenPose is chosen, the HALPE_26 model is used, with ear and eye at coordinates (0,0) since they are not used by Pose2Sim. You can change the MODEL tree to a different one if you need to reproject - in OpenPose format with a different model than BODY_25B. + in OpenPose format with a different model than HALPLE_26. + + New: Moving cameras and zooming cameras are now supported. Usage: from Pose2Sim.Utilities import reproj_from_trc_calib; reproj_from_trc_calib.reproj_from_trc_calib_func(r'', r'', '', r'') @@ -48,33 +50,32 @@ __status__ = "Development" ## SKELETON -'''BODY_25B (full-body without hands, experimental, from OpenPose) -https://github.com/CMU-Perceptual-Computing-Lab/openpose_train/blob/master/experimental_models/README.md -Adjust it if you want to reproject in OpenPose format with a different model''' -nb_joints = 25 -MODEL = Node("CHip", id=None, children=[ +'''HALPE_26 (full-body without hands, from AlphaPose, MMPose, etc.) +https://github.com/MVIG-SJTU/AlphaPose/blob/master/docs/MODEL_ZOO.md +https://github.com/open-mmlab/mmpose/tree/main/projects/rtmpose''' +MODEL = Node("Hip", id=19, children=[ Node("RHip", id=12, children=[ Node("RKnee", id=14, children=[ Node("RAnkle", id=16, children=[ - Node("RBigToe", id=22, children=[ + Node("RBigToe", id=21, children=[ Node("RSmallToe", id=23), ]), - Node("RHeel", id=24), + Node("RHeel", id=25), ]), ]), ]), Node("LHip", id=11, children=[ Node("LKnee", id=13, children=[ Node("LAnkle", id=15, children=[ - Node("LBigToe", id=19, children=[ - Node("LSmallToe", id=20), + Node("LBigToe", id=20, children=[ + Node("LSmallToe", id=22), ]), - Node("LHeel", id=21), + Node("LHeel", id=24), ]), ]), ]), - Node("Neck", id=17, children=[ - Node("Head", id=18, children=[ + Node("Neck", id=18, children=[ + Node("Head", id=17, children=[ Node("Nose", id=0), ]), Node("RShoulder", id=6, children=[ @@ -90,32 +91,12 @@ MODEL = Node("CHip", id=None, children=[ ]), ]) -# nb_joints = 17 -# MODEL = Node("None", id=None, children=[ -# Node("Origin", id=0), -# Node("Board1", id=1), -# Node("Board2", id=2), -# Node("Board3", id=3), -# Node("Board4", id=4), -# Node("Furniture5", id=5), -# Node("Furniture6", id=6), -# Node("Furniture7", id=7), -# Node("Screen8", id=8), -# Node("Screen9", id=9), -# Node("Furniture10", id=10), -# Node("Furniture11", id=11), -# Node("Furniture12", id=12), -# Node("Furniture13", id=13), -# Node("Furniture14", id=14), -# Node("Furniture15", id=15), -# Node("Table16", id=16)]) - - ## FUNCTIONS def computeP(calib_file, undistort=False): ''' Compute projection matrices from toml calibration file. + Zooming or moving cameras are handled. INPUT: - calib_file: calibration .toml file. @@ -133,24 +114,47 @@ def computeP(calib_file, undistort=False): if cam != 'metadata': S = np.array(calib[cam]['size']) K = np.array(calib[cam]['matrix']) - if undistort: - dist = np.array(calib[cam]['distortions']) - optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0] - Kh = np.block([optim_K, np.zeros(3).reshape(3,1)]) - else: - Kh = np.block([K, np.zeros(3).reshape(3,1)]) - R, _ = cv2.Rodrigues(np.array(calib[cam]['rotation'])) + + if len(K.shape) == 2: # static camera + if undistort: + dist = np.array(calib[cam]['distortions']) + optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0] + Kh = np.block([optim_K, np.zeros(3).reshape(3,1)]) + else: + Kh = np.block([K, np.zeros(3).reshape(3,1)]) + elif len(K.shape) == 3: # zooming camera + if undistort: + dist = np.array(calib[cam]['distortions']) + optim_K = [cv2.getOptimalNewCameraMatrix(K[f], dist, [int(s) for s in S], 1, [int(s) for s in S])[0] for f in range(len(K))] + Kh = [np.block([optim_K[f], np.zeros(3).reshape(3,1)]) for f in range(len(K))] + else: + Kh = [np.block([K[f], np.zeros(3).reshape(3,1)]) for f in range(len(K))] + + R = np.array(calib[cam]['rotation']) T = np.array(calib[cam]['translation']) - H = np.block([[R,T.reshape(3,1)], [np.zeros(3), 1 ]]) - - P.append(Kh @ H) - - return P + if len(R.shape) == 1: # static camera + R_mat, _ = cv2.Rodrigues(np.array(calib[cam]['rotation'])) + H = np.block([[R_mat,T.reshape(3,1)], [np.zeros(3), 1 ]]) + elif len(R.shape) == 2: # moving camera + R_mat = [cv2.Rodrigues(R[f])[0] for f in range(len(R))] + H = [np.block([[R_mat[f],T[f].reshape(3,1)], [np.zeros(3), 1 ]]) for f in range(len(R))] + + if len(K.shape) == 2 and len(R.shape)==1: # static camera + P.append([Kh @ H]) + elif len(K.shape) == 3 and len(R.shape)==1: # zooming camera + P.append([Kh[f] @ H for f in range(len(K))]) + elif len(K.shape) == 2 and len(R.shape)==2: # moving camera + P.append([Kh @ H[f] for f in range(len(R))]) + elif len(K.shape) == 3 and len(R.shape)==2: # zooming and moving camera + P.append([Kh[f] @ H[f] for f in range(len(K))]) + + return np.array(P) def retrieve_calib_params(calib_file): ''' Compute projection matrices from toml calibration file. + Zooming or moving cameras are handled. INPUT: - calib_file: calibration .toml file. @@ -172,9 +176,15 @@ def retrieve_calib_params(calib_file): S.append(np.array(calib[cam]['size'])) K.append(np.array(calib[cam]['matrix'])) dist.append(np.array(calib[cam]['distortions'])) - optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0]) + + if len(K[c].shape) == 2: # static camera + optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0]) + elif len(K[c].shape) == 3: # zooming camera + optim_K.append([cv2.getOptimalNewCameraMatrix(K[c][f], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0] for f in range(len(K[c]))]) + R.append(np.array(calib[cam]['rotation'])) T.append(np.array(calib[cam]['translation'])) + calib_params = {'S': S, 'K': K, 'dist': dist, 'optim_K': optim_K, 'R': R, 'T': T} return calib_params @@ -248,10 +258,12 @@ def reproj_from_trc_calib_func(**args): toml calibration file. The output 2D points can be chosen to follow the DeepLabCut (default) or - the OpenPose format. If OpenPose is chosen, the BODY_25B model is used, + the OpenPose format. If OpenPose is chosen, the HALPE_26 model is used, with ear and eye at coordinates (0,0) since they are not used by Pose2Sim. You can change the MODEL tree to a different one if you need to reproject - in OpenPose format with a different model than BODY_25B. + in OpenPose format with a different model than HALPLE_26. + + New: Moving cameras and zooming cameras are now supported. Usage: from Pose2Sim.Utilities import reproj_from_trc_calib; reproj_from_trc_calib.reproj_from_trc_calib_func(input_trc_file = r'', input_calib_file = r'', openpose_output=True, deeplabcut_output=True, undistort_points=True, output_file_root = r'') @@ -268,6 +280,8 @@ def reproj_from_trc_calib_func(**args): output_file_root = args.get('output_file_root') if output_file_root == None: output_file_root = input_trc_file.replace('.trc', '_reproj') + if os.path.exists(output_file_root): + os.makedirs(output_file_root, exist_ok=True) if not openpose_output and not deeplabcut_output: raise ValueError('Output_format must be specified either "openpose_output" (-o) or "deeplabcut_output (-d)"') @@ -297,27 +311,29 @@ def reproj_from_trc_calib_func(**args): pass # header preparation + num_frames = min(P_all.shape[1], len(data_trc)) columns_iterables = [['DavidPagnon'], ['person0'], bodyparts, ['x','y']] columns_h5 = pd.MultiIndex.from_product(columns_iterables, names=['scorer', 'individuals', 'bodyparts', 'coords']) - rows_iterables = [['labeled_data'], [filename], [f'img_{i:03d}.png' for i in range(len(data_trc))]] + rows_iterables = [[os.path.join(os.path.splitext(input_trc_file)[0],f'img_{i:03d}.png') for i in range(num_frames)]] rows_h5 = pd.MultiIndex.from_product(rows_iterables) data_h5 = pd.DataFrame(np.nan, index=rows_h5, columns=columns_h5) # Reproject 3D points on all cameras data_proj = [deepcopy(data_h5) for cam in range(len(P_all))] # copy data_h5 as many times as there are cameras Q = data_trc_zup.iloc[:,2:] - for frame in range(len(Q)): + for frame in range(num_frames): coords = [[] for cam in range(len(P_all))] + P_all_frame = [P_all[cam][frame] for cam in range(len(P_all))] for keypoint in range(num_bodyparts): q = np.append(Q.iloc[frame,3*keypoint:3*keypoint+3], 1) if undistort_points: coords_2D_all = [cv2.projectPoints(np.array(q[:-1]), calib_params_R_filt[i], calib_params_T_filt[i], calib_params_K_filt[i], calib_params_dist_filt[i])[0] for i in range(len(P_all))] - x_all = [coords_2D_all[i][0,0,0] for i in range(len(P_all))] - y_all = [coords_2D_all[i][0,0,1] for i in range(len(P_all))] + x_all = [coords_2D_all[i][0,0,0] for i in range(len(P_all_frame))] + y_all = [coords_2D_all[i][0,0,1] for i in range(len(P_all_frame))] else: - x_all, y_all = reprojection(P_all, q) - [coords[cam].extend([x_all[cam], y_all[cam]]) for cam in range(len(P_all))] - for cam in range(len(P_all)): + x_all, y_all = reprojection(P_all_frame, q) + [coords[cam].extend([x_all[cam], y_all[cam]]) for cam in range(len(P_all_frame))] + for cam in range(len(P_all_frame)): data_proj[cam].iloc[frame,:] = coords[cam] # Save as h5 and csv if DeepLabCut format @@ -331,13 +347,14 @@ def reproj_from_trc_calib_func(**args): [data_proj[i].to_csv(csv_files[i], sep=',', index=True, lineterminator='\n') for i in range(len(P_all))] # Save as json if OpenPose format - if openpose_output: + elif openpose_output: # read model tree model = MODEL print('Keypoint hierarchy:') for pre, _, node in RenderTree(model): print(f'{pre}{node.name} id={node.id}') bodyparts_ids = [[node.id for _, _, node in RenderTree(model) if node.name==b][0] for b in bodyparts] + nb_joints = len(bodyparts_ids) #prepare json files json_dict = {'version':1.3, 'people':[]} json_dict['people'] = [{'person_id':[-1], @@ -382,4 +399,4 @@ if __name__ == '__main__': parser.add_argument('-O', '--output_file_root', required=False, help='output file root path, without extension') args = vars(parser.parse_args()) - reproj_from_trc_calib_func(**args) + reproj_from_trc_calib_func(**args) \ No newline at end of file