├── .gitignore ├── ACM ├── __main__.py ├── anatomy.py ├── calibration.py ├── em.py ├── em_helper.py ├── em_run.py ├── export.py ├── fitting.py ├── gui │ ├── __init__.py │ └── viewer.py ├── helper.py ├── initialization.py ├── interp_3d.py ├── kalman.py ├── model.py ├── optimization.py ├── routines_math.py └── tools.py ├── FAQ.md ├── INPUTS.md ├── LICENSE ├── OUTPUTS.md ├── README.md ├── environment.yml ├── example_config └── example_dataset │ ├── configuration.py │ ├── labels_dlc_006900_184000.npy │ ├── labels_manual.npz │ ├── model.npy │ ├── multicalibration.npy │ └── origin_coord.npy └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | required_files/ 132 | datasets/ 133 | 134 | *.kate-swp 135 | 136 | *.ipynb 137 | *.swp 138 | 139 | .idea/ 140 | -------------------------------------------------------------------------------- /ACM/__main__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import argparse 3 | import os 4 | import sys 5 | from pprint import pprint 6 | import numpy as np 7 | from scipy.io import savemat 8 | 9 | def main(): 10 | # Parse inputs 11 | parser = argparse.ArgumentParser(description="ACM (Anatomically-constrained model) - a framework for videography based pose tracking of rodents") 12 | parser.add_argument('INPUT_PATH', type=str, help="Directory with job configuration") 13 | parser.add_argument('--viewer', required=False, help="Load viewer instead of tracking pose", action="store_true") 14 | parser.add_argument('--export', required=False, help="Exports result in alternative format", action="store_true") 15 | parser.add_argument('--makepose', required=False, help="Creates pose file save_dict", action="store_true") 16 | parser.add_argument('--calibration', required=False, help="Perform calibration only", action="store_true") 17 | parser.add_argument('--initialization', required=False, help="Perform initialization only (Requires calibration)", action="store_true") 18 | parser.add_argument('--poseinference', required=False, help="Perform poseinference only (Requires calibration and initialization)", action="store_true") 19 | args = parser.parse_args() 20 | input_path = os.path.expanduser(args.INPUT_PATH) 21 | 22 | # Load config 23 | # TODO change config system, e.g. pass around a dictionary instead of importing the config everywhere, requiring the sys.path.insert 24 | 25 | if args.viewer: 26 | sys.path.insert(0,input_path) 27 | print(f'Loading {input_path} ...') 28 | viewer() 29 | elif args.export: 30 | from .export import export 31 | export(input_path) 32 | elif args.makepose: 33 | config_path = input_path+'/../..' 34 | sys.path.insert(0,config_path) 35 | from ACM import tools 36 | print(f'Loading {config_path} ...') 37 | config = get_config_dict() 38 | save_dict = np.load(input_path+'/save_dict.npy',allow_pickle=True).item() 39 | x_ini = np.load(input_path+'/x_ini.npy',allow_pickle=True) 40 | pose = tools.propagate_latent_to_pose(config,save_dict,x_ini) 41 | 42 | posepath = input_path+'/pose' 43 | print(f'Saving pose to {posepath}') 44 | np.save(posepath+'.npy', pose) 45 | savemat(posepath+'.mat', pose) 46 | else: 47 | sys.path.insert(0,input_path) 48 | from ACM.export import export 49 | from ACM.tools import copy_config 50 | print(f'Loading {input_path} ...') 51 | check(args) 52 | config = get_config_dict() 53 | copy_config(config,input_path) 54 | track(args) 55 | export(config['folder_save']) 56 | 57 | def check(args): 58 | import configuration as cfg 59 | full_pipline = args.calibration==False and args.initialization==False and args.poseinference==False 60 | 61 | if args.calibration==True or full_pipline: 62 | check_directory(cfg.folder_calib,'Calibration') or sys.exit(1) 63 | if args.initialization==True or full_pipline: 64 | check_directory(cfg.folder_init,'Initialization') or sys.exit(1) 65 | if args.poseinference==True or full_pipline: 66 | check_directory(cfg.folder_save,'Result') or sys.exit(1) 67 | 68 | def track(args): 69 | import configuration as cfg 70 | 71 | from ACM import calibration 72 | from ACM import initialization 73 | from ACM import fitting 74 | from ACM import em_run 75 | 76 | full_pipline = args.calibration==False and args.initialization==False and args.poseinference==False 77 | 78 | # calibrate 79 | if args.calibration==True or full_pipline: 80 | calibration.main() 81 | 82 | # initialize 83 | if args.initialization==True or full_pipline: 84 | initialization.main() 85 | 86 | # run pose reconstruction 87 | if args.poseinference==True or full_pipline: 88 | if ((cfg.mode == 1) or (cfg.mode == 2)): 89 | # run deterministic models 90 | fitting.main() 91 | elif ((cfg.mode == 3) or (cfg.mode == 4)): 92 | # run probabilistic models 93 | em_run.main() 94 | 95 | def viewer(): 96 | config = get_config_dict() 97 | 98 | from ACM.gui import viewer 99 | viewer.start(config) 100 | 101 | def get_config_dict(): 102 | import configuration as cfg 103 | 104 | config = vars(cfg) 105 | for k in list(config.keys()): 106 | if k.startswith('__'): 107 | del config[k] 108 | 109 | return config 110 | 111 | def check_directory(path,dirtype): 112 | if os.path.isdir(path) : 113 | if len(os.listdir(path)) > 0: 114 | invalid_input = True 115 | while (invalid_input): 116 | print(f'{dirtype} folder {path} already exists. Do you want to overwrite the existing folder? [y/n]') 117 | input_user = input() 118 | if ((input_user == 'Y') or (input_user == 'y')): 119 | run_pose = True 120 | invalid_input = False 121 | elif ((input_user == 'N') or (input_user == 'n')): 122 | return False 123 | else: 124 | # create target save folder 125 | os.makedirs(path) 126 | 127 | return True 128 | 129 | if __name__ == "__main__": 130 | main() 131 | -------------------------------------------------------------------------------- /ACM/calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import sys 5 | import torch 6 | 7 | import configuration as cfg 8 | 9 | from . import anatomy 10 | from . import helper 11 | from . import interp_3d 12 | from . import model 13 | from . import optimization as opt 14 | from . import routines_math as rout_m 15 | 16 | def get_origin_coord(file_origin_coord, scale_factor): 17 | # load 18 | origin_coord = np.load(file_origin_coord, allow_pickle=True).item() 19 | # arena coordinate system 20 | origin = origin_coord['origin'] 21 | coord = origin_coord['coord'] 22 | # scaling (calibration board square size -> cm) 23 | origin = origin * scale_factor 24 | return origin, coord 25 | 26 | # ATTENTION: this is hard coded (assumes specific naming of the surface markers) 27 | def initialize_x(args, 28 | labels, 29 | arena_coord, arena_origin): 30 | # get numbers from args 31 | numbers = args['numbers'] 32 | nCameras = numbers['nCameras'] # int 33 | nBones = numbers['nBones'] # int 34 | nMarkers = numbers['nMarkers'] # int 35 | 36 | # calibration 37 | calibration = args['calibration'] 38 | A_entries = calibration['A_fit'].cpu().numpy() 39 | k = calibration['k_fit'].cpu().numpy() 40 | rX1 = calibration['rX1_fit'].cpu().numpy() 41 | RX1 = calibration['RX1_fit'].cpu().numpy() 42 | tX1 = calibration['tX1_fit'].cpu().numpy() 43 | A = np.zeros((nCameras, 3, 3), dtype=np.float64) 44 | 45 | if len(A_entries.shape) == 2: # Old style calibration 46 | for i_cam in range(nCameras): 47 | A[i_cam, 0, 0] = A_entries[i_cam, 0] 48 | A[i_cam, 0, 2] = A_entries[i_cam, 1] 49 | A[i_cam, 1, 1] = A_entries[i_cam, 2] 50 | A[i_cam, 1, 2] = A_entries[i_cam, 3] 51 | A[i_cam, 2, 2] = 1.0 52 | else: 53 | for i_cam in range(nCameras): 54 | A[i_cam, 0, 0] = A_entries[i_cam, 0, 0] 55 | A[i_cam, 0, 2] = A_entries[i_cam, 0, 2] 56 | A[i_cam, 1, 1] = A_entries[i_cam, 1, 1] 57 | A[i_cam, 1, 2] = A_entries[i_cam, 1, 2] 58 | A[i_cam, 2, 2] = A_entries[i_cam, 2, 2] 59 | 60 | # model 61 | joint_order = args['model']['joint_order'] # list 62 | joint_marker_order = args['model']['joint_marker_order'] # list 63 | skeleton_edges = args['model']['skeleton_edges'].cpu().numpy() 64 | skeleton_vertices = args['model']['skeleton_vertices'].cpu().numpy() 65 | skeleton_coords = args['model']['skeleton_coords'].cpu().numpy() 66 | skeleton_coords0 = args['model']['skeleton_coords0'].cpu().numpy() 67 | skeleton_vertices_links = args['model']['skeleton_vertices_links'].cpu().numpy() 68 | joint_marker_vec = args['model']['joint_marker_vectors'].cpu().numpy() 69 | joint_marker_index = args['model']['joint_marker_index'].cpu().numpy() 70 | # 71 | is_euler = args['model']['is_euler'].cpu().numpy() 72 | 73 | # initialize t and head direction 74 | labels3d = dict() 75 | for marker_index in range(nMarkers): 76 | marker_name = joint_marker_order[marker_index] 77 | string_split = marker_name.split('_') 78 | string = 'spot_' + '_'.join(string_split[1:-1]) 79 | if ((string == 'spot_head_002') or 80 | (string == 'spot_head_003')): 81 | labels_use = labels[:, marker_index] 82 | if (np.sum(labels_use[:, 2] != 0.0) >= 2): # check if label was detected in at least two cameras 83 | labels3d[string] = interp_3d.calc_3d_point(labels_use, A, k, rX1, tX1) 84 | if (('spot_head_003' in labels3d) and ('spot_head_002' in labels3d)): 85 | head_direc = labels3d['spot_head_002'] - labels3d['spot_head_003'] 86 | model_t = np.copy(labels3d['spot_head_003']) 87 | else: # use alternative initialization for t when the labels at the head are not visible 88 | avg_spine = np.zeros(3, dtype=np.float64) 89 | nSpine = 0 90 | avg_tail = np.zeros(3, dtype=np.float64) 91 | nTail = 0 92 | # 93 | calculate_3d_point = 0 94 | for marker_index in range(nMarkers): 95 | marker_name = joint_marker_order[marker_index] 96 | string_split = marker_name.split('_') 97 | if string_split[1] == 'spine': 98 | calculate_3d_point = 1 99 | elif (string_split[1] == 'tail'): 100 | calculate_3d_point = 2 101 | else: 102 | calculate_3d_point = 0 103 | if ((calculate_3d_point == 1) or (calculate_3d_point == 2)): 104 | string = 'spot_' + '_'.join(string_split[1:-1]) 105 | labels_use = labels[:, marker_index] 106 | if (np.sum(labels_use[:, 2] != 0.0) >= 2): # check if label was detected in at least two cameras 107 | labels3d[string] = interp_3d.calc_3d_point(labels_use, A, k, rX1, tX1) 108 | if (calculate_3d_point == 1): 109 | avg_spine += labels3d[string] 110 | nSpine += 1 111 | elif (calculate_3d_point == 2): 112 | avg_tail += labels3d[string] 113 | nTail += 1 114 | avg_spine /= nSpine 115 | avg_tail /= nTail 116 | # 117 | head_direc = avg_tail - avg_spine 118 | if ('spot_head_003' in labels3d): 119 | model_t = np.copy(labels3d['spot_head_003']) 120 | else: 121 | model_t = avg_spine - 0.5 * head_direc 122 | 123 | # r 124 | model_r = np.zeros((nBones, 3), dtype=np.float64) 125 | current_coords = np.tile(np.identity(3, dtype=np.float64), (nBones, 1, 1)) 126 | skeleton_coords_use = np.copy(skeleton_coords0) 127 | coord_emb = np.array([[0.0, 0.0, -1.0], 128 | [-1.0, 0.0, 0.0], 129 | [0.0, 1.0, 0.0]], 130 | dtype=np.float64) 131 | # head direction of the model after aligned to arena coordinate system 132 | current_head_direc = coord_emb[:, 2] # i.e. np.array([-1.0, 0.0, 0.0], dtype=np.float64) 133 | current_head_direc_xy = current_head_direc[:2] / np.sqrt(np.sum(current_head_direc[:2]**2)) 134 | # head direction calculated from labels 135 | head_direc_xy = head_direc[:2] / np.sqrt(np.sum(head_direc[:2]**2)) 136 | # signed angle between the current and target head direction in the xy-plane 137 | ang1 = np.arctan2(current_head_direc_xy[1], current_head_direc_xy[0]) 138 | ang2 = np.arctan2(head_direc_xy[1], head_direc_xy[0]) 139 | ang_xy = ang2 - ang1 140 | # resulting rotation matrix 141 | R_xy = rout_m.rodrigues2rotMat_single(np.array([0.0, 0.0, ang_xy], dtype=np.float64)) 142 | # 143 | target_skeleton_coords = np.copy(skeleton_coords_use) # use this for mean pose (according to bounds) 144 | target_skeleton_coords = np.einsum('ij,njk->nik', coord_emb, target_skeleton_coords) 145 | target_skeleton_coords = np.einsum('ij,njk->nik', R_xy, target_skeleton_coords) 146 | 147 | # this is the first global rotation 148 | # it roughly aligns the head direction 149 | index_bone = 0 150 | R = np.dot(target_skeleton_coords[index_bone], current_coords[index_bone].T) 151 | model_r[index_bone] = rout_m.rotMat2rodrigues_single(R) 152 | current_coords = np.einsum('nij,jk->nik', current_coords, R) 153 | 154 | # mean values for angles (calculated from joint angle limits) 155 | bounds = args['bounds_pose'].cpu().numpy() 156 | for index_bone in range(1, nBones): 157 | joint_index = skeleton_edges[index_bone, 0] 158 | joint_name = joint_order[joint_index] 159 | bounds_use = bounds[3*(1+index_bone):3*(1+index_bone)+3] 160 | index_set_zero1 = np.array([np.all(bounds_use[0] == 0.0), 161 | np.all(bounds_use[1] == 0.0), 162 | np.all(bounds_use[2] == 0.0)], dtype=bool) 163 | index_set_zero2 = np.array([np.all(np.isinf(bounds_use[0])), 164 | np.all(np.isinf(bounds_use[1])), 165 | np.all(np.isinf(bounds_use[2]))], dtype=bool) 166 | index_set_zero = np.logical_or(index_set_zero1, index_set_zero2) 167 | model_r[index_bone][~index_set_zero] = np.mean(bounds_use[~index_set_zero], 1) 168 | model_r[index_bone][index_set_zero] = 0.0 169 | 170 | # to avoid rodrigues vectors with zero elements 171 | rodrigues_mask = ~is_euler[1:] & np.all(model_r == 0.0, 1) 172 | noise = np.random.randn(*np.shape(model_r[rodrigues_mask])) * 2**-23 173 | model_r[rodrigues_mask] += noise 174 | 175 | # construct initial x 176 | x_ini = np.concatenate([model_t.ravel(), 177 | model_r.ravel()], 0) 178 | return x_ini 179 | 180 | 181 | def main(): 182 | # get arguments 183 | args = helper.get_arguments(cfg.file_origin_coord, cfg.file_calibration, cfg.file_model, cfg.file_labelsDLC, 184 | cfg.scale_factor, cfg.pcutoff) 185 | args['use_custom_clip'] = False 186 | 187 | # get relevant information from arguments 188 | nBones = args['numbers']['nBones'] 189 | nMarkers = args['numbers']['nMarkers'] 190 | nCameras = args['numbers']['nCameras'] 191 | joint_order = args['model']['joint_order'] # list 192 | joint_marker_order = args['model']['joint_marker_order'] # list 193 | skeleton_edges = args['model']['skeleton_edges'].cpu().numpy() 194 | bone_lengths_index = args['model']['bone_lengths_index'].cpu().numpy() 195 | joint_marker_index = args['model']['joint_marker_index'].cpu().numpy() 196 | # 197 | free_para_bones = args['free_para_bones'].cpu().numpy() 198 | free_para_markers = args['free_para_markers'].cpu().numpy() 199 | free_para_pose = args['free_para_pose'].cpu().numpy() 200 | nPara_bones = args['nPara_bones'] 201 | nPara_markers = args['nPara_markers'] 202 | nPara_pose = args['nPara_pose'] 203 | nFree_bones = args['nFree_bones'] 204 | nFree_markers = args['nFree_markers'] 205 | nFree_pose = args['nFree_pose'] 206 | 207 | print(cfg.file_labelsManual) 208 | # load frame list according to manual labels 209 | if (cfg.file_labelsManual[-3:] == 'npz'): 210 | labels_manual = np.load(cfg.file_labelsManual, allow_pickle=True)['arr_0'].item() 211 | elif (cfg.file_labelsManual[-3:] == 'npy'): 212 | labels_manual = np.load(cfg.file_labelsManual, allow_pickle=True).item() 213 | frame_list_manual = sorted(list(labels_manual.keys())) 214 | 215 | # get calibration frame list 216 | print(type(cfg.index_frames_calib)) 217 | print(cfg.index_frames_calib) 218 | if isinstance(cfg.index_frames_calib,str): 219 | print('a') 220 | frame_list_calib = list(labels_manual.keys()) 221 | else: 222 | print('b') 223 | frame_list_calib = np.array([], dtype=np.int64) 224 | for i in range(np.size(cfg.index_frames_calib, 0)): 225 | framesList_single = np.arange(cfg.index_frames_calib[i][0], 226 | cfg.index_frames_calib[i][1] + cfg.dFrames_calib, 227 | cfg.dFrames_calib, 228 | dtype=np.int64) 229 | frame_list_calib = np.concatenate([frame_list_calib, framesList_single], 0) 230 | 231 | nFrames = int(np.size(frame_list_calib)) 232 | 233 | # create correct free_para 234 | free_para = np.concatenate([free_para_bones, 235 | free_para_markers], 0) 236 | for i_frame in frame_list_calib: 237 | free_para = np.concatenate([free_para, 238 | free_para_pose], 0) 239 | 240 | # initialize x_pose 241 | # load arena coordinate system 242 | origin, coord = get_origin_coord(cfg.file_origin_coord, cfg.scale_factor) 243 | # 244 | labels_frame = np.zeros((nCameras, nMarkers, 3), dtype=np.float64) 245 | labels_use = labels_manual[frame_list_calib[0]] 246 | for i_marker in range(nMarkers): 247 | marker_name = joint_marker_order[i_marker] 248 | marker_name_split = marker_name.split('_') 249 | label_name = 'spot_' + '_'.join(marker_name_split[1:-1]) 250 | if label_name in labels_use: 251 | labels_frame[:, i_marker, :2] = labels_use[label_name] 252 | labels_frame[:, i_marker, 2] = 1.0 253 | x_pose = initialize_x(args, 254 | labels_frame, 255 | coord, origin)[None, :] 256 | for i_frame in frame_list_calib[1:]: 257 | labels_frame = np.zeros((nCameras, nMarkers, 3), dtype=np.float64) 258 | labels_use = labels_manual[frame_list_calib[0]] 259 | for i_marker in range(nMarkers): 260 | marker_name = joint_marker_order[i_marker] 261 | marker_name_split = marker_name.split('_') 262 | label_name = 'spot_' + '_'.join(marker_name_split[1:-1]) 263 | if label_name in labels_use: 264 | labels_frame[:, i_marker, :2] = labels_use[label_name] 265 | labels_frame[:, i_marker, 2] = 1.0 266 | x_pose_single = initialize_x(args, 267 | labels_frame, 268 | coord, origin)[None, :] 269 | x_pose = np.concatenate([x_pose, x_pose_single], 0) 270 | x_free_pose = x_pose[:, free_para_pose].ravel() 271 | x_pose = x_pose.ravel() 272 | 273 | # BOUNDS 274 | # bone_lengths 275 | bounds_free_bones = args['bounds_free_bones'] 276 | bounds_free_low_bones = model.do_normalization_bones(bounds_free_bones[:, 0]) 277 | bounds_free_high_bones = model.do_normalization_bones(bounds_free_bones[:, 1]) 278 | # joint_marker_vec 279 | bounds_free_markers = args['bounds_free_markers'] 280 | bounds_free_low_markers = model.do_normalization_markers(bounds_free_markers[:, 0]) 281 | bounds_free_high_markers = model.do_normalization_markers(bounds_free_markers[:, 1]) 282 | # pose 283 | bounds_free_pose = args['bounds_free_pose'] 284 | bounds_free_low_pose_single = model.do_normalization(bounds_free_pose[:, 0][None, :], args).numpy().ravel() 285 | bounds_free_high_pose_single = model.do_normalization(bounds_free_pose[:, 1][None, :], args).numpy().ravel() 286 | bounds_free_low_pose = np.tile(bounds_free_low_pose_single, nFrames) 287 | bounds_free_high_pose = np.tile(bounds_free_high_pose_single, nFrames) 288 | # all 289 | bounds_free_low = np.concatenate([bounds_free_low_bones, 290 | bounds_free_low_markers, 291 | bounds_free_low_pose], 0) 292 | bounds_free_high = np.concatenate([bounds_free_high_bones, 293 | bounds_free_high_markers, 294 | bounds_free_high_pose], 0) 295 | bounds_free = np.stack([bounds_free_low, bounds_free_high], 1) 296 | args['bounds_free'] = bounds_free 297 | 298 | # INITIALIZE X 299 | inital_bone_lenght = 0.0 300 | inital_marker_length = 0.0 301 | # initialize bone_lengths and joint_marker_vec 302 | x_bones = bounds_free_low_bones.numpy() + (bounds_free_high_bones.numpy() - bounds_free_low_bones.numpy()) * 0.5 303 | x_bones[np.isinf(x_bones)] = inital_bone_lenght 304 | 305 | x_free_bones = x_bones[free_para_bones] 306 | x_markers = np.full(nPara_markers, 0.0, dtype=np.float64) 307 | x_free_markers = np.zeros(nFree_markers ,dtype=np.float64) 308 | x_free_markers[(bounds_free_low_markers != 0.0) & (bounds_free_high_markers == 0.0)] = -inital_marker_length 309 | x_free_markers[(bounds_free_low_markers == 0.0) & (bounds_free_high_markers != 0.0)] = inital_marker_length 310 | x_free_markers[(bounds_free_low_markers != 0.0) & (bounds_free_high_markers != 0.0)] = 0.0 311 | x_free_markers[(bounds_free_low_markers == 0.0) & (bounds_free_high_markers == 0.0)] = 0.0 312 | x_markers[free_para_markers] = np.copy(x_free_markers) 313 | # 314 | x = np.concatenate([x_bones, 315 | x_markers, 316 | x_pose], 0) 317 | 318 | # update args regarding fixed tensors 319 | args['plot'] = False 320 | args['nFrames'] = nFrames 321 | 322 | # update args regarding x0 and labels 323 | # ARGS X 324 | args['x_torch'] = torch.from_numpy(np.concatenate([x_bones, 325 | x_markers, 326 | x_pose[:nPara_pose]], 0)) 327 | args['x_free_torch'] = torch.from_numpy(np.concatenate([x_free_bones, 328 | x_free_markers, 329 | x_free_pose], 0)) 330 | args['x_free_torch'].requires_grad = True 331 | # ARGS LABELS MANUAL 332 | args['labels_single_torch'] = torch.zeros((nFrames, nCameras, nMarkers, 3), dtype=model.float_type) 333 | args['labels_mask_single_torch'] = torch.zeros((nFrames, nCameras, nMarkers), dtype=torch.bool) 334 | for i in range(nFrames): 335 | index_frame = frame_list_calib[i] 336 | if index_frame in labels_manual: 337 | labels_manual_frame = labels_manual[index_frame] 338 | for marker_index in range(nMarkers): 339 | marker_name = joint_marker_order[marker_index] 340 | string = 'spot_' + '_'.join(marker_name.split('_')[1:-1]) 341 | if string in labels_manual_frame: 342 | mask = ~np.any(np.isnan(labels_manual[index_frame][string]), 1) 343 | args['labels_mask_single_torch'][i, :, marker_index] = torch.from_numpy(mask) 344 | args['labels_single_torch'][i, :, marker_index, :2][mask] = torch.from_numpy(labels_manual[index_frame][string][mask]) 345 | args['labels_single_torch'][i, :, marker_index, 2][mask] = 1.0 346 | 347 | # print ratio 348 | print('Ratio:') 349 | print('Number of free parameters:\t{:06d}'.format(int(np.sum(free_para)))) 350 | print('Number of measurement:\t\t{:06d}'.format(int(2 * torch.sum(args['labels_mask_single_torch'])))) 351 | 352 | 353 | # normalize x 354 | x_free = np.concatenate([model.do_normalization_bones(torch.from_numpy(x_free_bones[None, :])).numpy().ravel(), 355 | model.do_normalization_markers(torch.from_numpy(x_free_markers[None, :])).numpy().ravel(), 356 | model.do_normalization(torch.from_numpy(x_free_pose.reshape(nFrames, nFree_pose)), args).numpy().ravel()], 0) 357 | 358 | # OPTIMIZE 359 | # create optimization dictonary 360 | opt_options = dict() 361 | opt_options['disp'] = cfg.opt_options_calib__disp 362 | opt_options['maxiter'] = cfg.opt_options_calib__maxiter 363 | opt_options['maxcor'] = cfg.opt_options_calib__maxcor 364 | opt_options['ftol'] = cfg.opt_options_calib__ftol 365 | opt_options['gtol'] = cfg.opt_options_calib__gtol 366 | opt_options['maxfun'] = cfg.opt_options_calib__maxfun 367 | opt_options['iprint'] = cfg.opt_options_calib__iprint 368 | opt_options['maxls'] = cfg.opt_options_calib__maxls 369 | opt_dict = dict() 370 | opt_dict['opt_method'] = cfg.opt_method_calib 371 | opt_dict['opt_options'] = opt_options 372 | print('Calibrating') 373 | min_result = opt.optimize__scipy(x_free, args, 374 | opt_dict) 375 | # copy fitting result into correct arrary 376 | x_fit_free = np.copy(min_result.x) 377 | print('Finished calibrating') 378 | print() 379 | 380 | # reverse normalization of x 381 | x_fit_free = np.concatenate([model.undo_normalization_bones(torch.from_numpy(x_fit_free[:nFree_bones].reshape(1, nFree_bones))).numpy().ravel(), 382 | model.undo_normalization_markers(torch.from_numpy(x_fit_free[nFree_bones:nFree_bones+nFree_markers].reshape(1, nFree_markers))).numpy().ravel(), 383 | model.undo_normalization(torch.from_numpy(x_fit_free[nFree_bones+nFree_markers:]).reshape(nFrames, nFree_pose), args).numpy().ravel()], 0) 384 | 385 | # add free variables 386 | x_calib = np.copy(x) 387 | x_calib[free_para] = x_fit_free 388 | 389 | # save 390 | np.save(cfg.folder_calib + '/x_calib.npy', x_calib) 391 | 392 | if __name__ == "__main__": 393 | main() 394 | -------------------------------------------------------------------------------- /ACM/em.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import math 4 | import numpy as np 5 | import torch 6 | import os 7 | 8 | import configuration as cfg 9 | 10 | from . import kalman 11 | from . import model 12 | 13 | # IMPROVEMENT: put arrays of args_Qg in args 14 | # IMPROVEMENT: put arrays of the third row in args 15 | # IMPROVEMENT: also implement learning of A in slow mode 16 | def update_theta(mu_uks, var_uks, G_uks, 17 | args, args_Qg, 18 | x1_m_fx0, y1_m_hx1_2, measurement_expectation, 19 | var_f_1, var_g_1_L, A_1): 20 | args_kalman = args['args_kalman'] 21 | 22 | slow_mode = args_kalman['slow_mode'] 23 | nT = args_kalman['nT'] 24 | dim_z = args_kalman['dim_z'] 25 | dim_x = args_kalman['dim_x'] # for fast mode 26 | sigma_point_scheme = args_kalman['sigma_point_scheme'] 27 | w_c = args_kalman['w_c'] # for covariance terms 28 | w_c2 = args_kalman['w_c2'] # for covariance terms 29 | nSigmaPoints = args_kalman['nSigmaPoints'] 30 | nSigmaPoints2 = args_kalman['nSigmaPoints2'] 31 | sqrt_dimZ_p_lamb = args_kalman['sqrt_dimZ_p_lamb'] 32 | sqrt_dimZ_p_lamb2 = args_kalman['sqrt_dimZ_p_lamb2'] 33 | 34 | measure = args_kalman['measure'] 35 | measure_mask = args_kalman['measure_mask'] 36 | measure_mask_exclude = args_kalman['measure_mask_exclude'] 37 | args_model = args['args_model'] 38 | 39 | outer_z = args_kalman['outer_z'] 40 | var_pairwise = args_kalman['var_pairwise'] 41 | mu_pairwise = args_kalman['mu_pairwise'] 42 | sigma_points = args_kalman['sigma_points'] 43 | sigma_points_g = args_kalman['sigma_points_g'] 44 | sigma_points_pairwise = args_kalman['sigma_points_pairwise'] 45 | 46 | nMeasureT = args_Qg['nMeasureT'] 47 | 48 | # zero arrays to make sure no old values are still there (should not be necessary) 49 | x1_m_fx0.data.zero_() 50 | y1_m_hx1_2.data.zero_() 51 | measurement_expectation.data.zero_() 52 | var_f_1.data.zero_() 53 | var_g_1_L.data.zero_() 54 | if slow_mode: 55 | for t in range(1, nT+1): 56 | # sigma points 57 | if (sigma_point_scheme == 3): 58 | sigma_points.data.copy_(kalman.calc_sigma_points_p3(mu_uks[t], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[t])).data) 59 | elif (sigma_point_scheme == 5): 60 | sigma_points.data.copy_(kalman.calc_sigma_points_p5(mu_uks[t], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[t])).data) 61 | elif (sigma_point_scheme == 0): 62 | sigma_points.data.copy_(kalman.calc_sigma_points_rand(mu_uks[t], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[t]), 63 | nSigmaPoints).data) 64 | sigma_points_g.data.copy_((model.fcn_emission_free(sigma_points, args_model)[:, measure_mask_exclude]).data) 65 | # pairwise sigma points 66 | outer_z.data.copy_(torch.mm(G_uks[t-1], var_uks[t]).data) 67 | var_pairwise.data.copy_(torch.cat([torch.cat([var_uks[t], outer_z.transpose(1, 0)], 1), 68 | torch.cat([outer_z, var_uks[t-1]], 1)], 0).data) 69 | mu_pairwise.data.copy_(torch.cat([mu_uks[t], mu_uks[t-1]], 0).data) 70 | if (sigma_point_scheme == 3): 71 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_p3(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise)).data) 72 | elif (sigma_point_scheme == 5): 73 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_p5(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise)).data) 74 | elif (sigma_point_scheme == 0): 75 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_rand(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise), 76 | nSigmaPoints2).data) 77 | x1_m_fx0.data.copy_(kalman.substraction_save(sigma_points_pairwise[:, :dim_z], 78 | sigma_points_pairwise[:, dim_z:]).data) 79 | var_f_1.data.add_(torch.sum((w_c2[:, None, None] / float(nT)) * \ 80 | torch.einsum('mi,mj->mij', 81 | (x1_m_fx0, x1_m_fx0)), 0).data) 82 | var_f_1.data.copy_((0.5 * (var_f_1 + var_f_1.transpose(1, 0))).data) 83 | # var_g 84 | y1_m_hx1_2[t-1].data.copy_(torch.sum(w_c[:, None] * \ 85 | kalman.substraction_save(measure[t-1, None, :], 86 | sigma_points_g)**2, 0).data) 87 | y1_m_hx1_2[t-1, ~measure_mask[t-1]] = 0.0 88 | # 89 | measurement_expectation[t-1].data.copy_(sigma_points_g[0, :].data) 90 | var_g_1_L.data.copy_((torch.sum(y1_m_hx1_2, 0) * nMeasureT**-1).data) 91 | else: 92 | # sigma points 93 | if (sigma_point_scheme == 3): 94 | sigma_points.data.copy_(kalman.calc_sigma_points_p3(mu_uks[1:], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[1:])).data) 95 | elif (sigma_point_scheme == 5): 96 | sigma_points.data.copy_(kalman.calc_sigma_points_p5(mu_uks[1:], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[1:])).data) 97 | elif (sigma_point_scheme == 0): 98 | sigma_points.data.copy_(kalman.calc_sigma_points_rand(mu_uks[1:], sqrt_dimZ_p_lamb, kalman.cholesky_save(var_uks[1:]), 99 | nSigmaPoints).data) 100 | sigma_points_g.data.copy_(model.fcn_emission_free(sigma_points.reshape(nT * nSigmaPoints, dim_z), 101 | args_model)[:, measure_mask_exclude].reshape(nT, nSigmaPoints, dim_x).data) 102 | # pairwise sigma points 103 | outer_z.data.copy_(torch.einsum('tij,tjk->tik', (G_uks, var_uks[1:])).data) 104 | var_pairwise.data.copy_(torch.cat([torch.cat([var_uks[1:], outer_z.transpose(2, 1)], 2), 105 | torch.cat([outer_z, var_uks[:-1]], 2)], 1).data) 106 | mu_pairwise.data.copy_(torch.cat([mu_uks[1:], mu_uks[:-1]], 1).data) 107 | if (sigma_point_scheme == 3): 108 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_p3(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise)).data) 109 | elif (sigma_point_scheme == 5): 110 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_p5(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise)).data) 111 | elif (sigma_point_scheme == 0): 112 | sigma_points_pairwise.data.copy_(kalman.calc_sigma_points_rand(mu_pairwise, sqrt_dimZ_p_lamb2, kalman.cholesky_save(var_pairwise), 113 | nSigmaPoints2).data) 114 | # WHEN A IS NOT LEARNED 115 | # var_f 116 | x1_m_fx0.data.copy_(kalman.substraction_save(sigma_points_pairwise[:, :, :dim_z], 117 | sigma_points_pairwise[:, :, dim_z:]).data) 118 | var_f_1.data.copy_(torch.sum((w_c2[None, :, None, None] / float(nT)) * \ 119 | torch.einsum('tmi,tmj->tmij', 120 | (x1_m_fx0, x1_m_fx0)), (0, 1)).data) 121 | var_f_1.data.copy_((0.5 * (var_f_1 + var_f_1.transpose(1, 0))).data) 122 | 123 | # var_g 124 | y1_m_hx1_2.data.copy_(torch.sum(w_c[None, :, None] * \ 125 | kalman.substraction_save(measure[:, None, :], 126 | sigma_points_g)**2, 1).data) 127 | y1_m_hx1_2[~measure_mask] = 0.0 128 | # 129 | measurement_expectation.data.copy_(sigma_points_g[:, 0, :].data) 130 | # 131 | var_g_1_L.data.copy_((torch.sum(y1_m_hx1_2, 0) * nMeasureT**-1).data) 132 | return 133 | 134 | # IMPROVE: put arrays of args_Qg into args 135 | def run(mu0_in, var0_in, A_in, var_f_in, var_g_in, 136 | args): 137 | tol_out = float(cfg.tol) 138 | iter_out_max = int(cfg.iter_max) 139 | # 140 | nSave = int(50) 141 | # 142 | verbose = bool(True) 143 | 144 | use_cuda = args['use_cuda'] 145 | args_kalman = args['args_kalman'] 146 | nT = args_kalman['nT'] 147 | dim_z = args_kalman['dim_z'] 148 | dim_x = args_kalman['dim_x'] 149 | args_Qg = args_kalman['args_Qg'] 150 | # 151 | x1_m_fx0 = args_kalman['x1_m_fx0'] 152 | y1_m_hx1_2 = args_kalman['y1_m_hx1_2'] 153 | 154 | # theta0 155 | mu0_0 = mu0_in.clone() 156 | var0_0 = var0_in.clone() 157 | A_0 = A_in.clone() 158 | var_f_0 = var_f_in.clone() 159 | var_g_0 = var_g_in.clone() 160 | var_g_0_L = torch.diag(var_g_0).clone() 161 | # theta1 162 | mu0_1 = mu0_in.clone() 163 | var0_1 = var0_in.clone() 164 | A_1 = A_in.clone() 165 | var_f_1 = var_f_in.clone() 166 | var_g_1 = var_g_in.clone() 167 | var_g_1_L = torch.diag(var_g_0).clone() 168 | # 169 | measurement_expectation0 = args_kalman['measurement_expectation0'] 170 | measurement_expectation1 = args_kalman['measurement_expectation1'] 171 | # dTheta 172 | d_mu0 = mu0_in.clone() 173 | d_var0 = var0_in.clone() 174 | d_A = A_in.clone() 175 | d_var_f = var_f_in.clone() 176 | d_var_g = var_g_in.clone() 177 | d_norm_mu0 = mu0_in.clone() 178 | d_norm_var0 = var0_in.clone() 179 | d_norm_A = A_in.clone() 180 | d_norm_var_f = var_f_in.clone() 181 | d_norm_var_g = var_g_in.clone() 182 | # 183 | iter_out = int(0) 184 | cond = bool(False) 185 | convergence_status = int(0) 186 | while not(cond): 187 | # E-STEP 188 | mu_kalman, var_kalman, G_kalman = kalman.uks(mu0_0, var0_0, A_0, var_f_0, var_g_0, 189 | args) 190 | # M-STEP 191 | # mu0 192 | mu0_1.data.copy_(mu_kalman[0].data) 193 | # var0 194 | var0_1.data.copy_(var_kalman[0].data) 195 | # var_f & var_g & A 196 | update_theta(mu_kalman, var_kalman, G_kalman, 197 | args, args_Qg, 198 | x1_m_fx0, y1_m_hx1_2, measurement_expectation1, 199 | var_f_1, var_g_1_L, A_1) 200 | var_g_1.diagonal().data.copy_(var_g_1_L.data) # since var_g is assumed to be a diagonal matrix 201 | # REST 202 | # dTheata 203 | d_mu0.data.copy_(abs((mu0_1 - mu0_0)).data) 204 | d_var0.data.copy_(abs((var0_1 - var0_0)).data) 205 | d_A.data.copy_(abs((A_1 - A_0)).data) 206 | d_var_f.data.copy_(abs((var_f_1 - var_f_0)).data) 207 | d_var_g.data.copy_(abs((var_g_1 - var_g_0)).data) 208 | d_norm_mu0.data.copy_((d_mu0 / abs(mu0_0)).data) 209 | d_norm_var0.data.copy_((d_var0 / abs(var0_0)).data) 210 | d_norm_A.data.copy_((d_A / abs(A_0)).data) 211 | d_norm_var_f.data.copy_((d_var_f / abs(var_f_0)).data) 212 | d_norm_var_g.data.copy_((d_var_g / abs(var_g_0)).data) 213 | # differences in theta 214 | d_norm_mu0_mean = float(torch.mean(d_norm_mu0)) # dim_z 215 | d_norm_var0_mean = float(torch.mean(d_norm_var0.diag())) # dim_z 216 | d_norm_var_f_mean = float(torch.mean(d_norm_var_f.diag())) # dim_z 217 | d_norm_var_g_mean = float(torch.mean(d_norm_var_g.diag())) # dim_x 218 | theta_mean = float(torch.mean(torch.cat([d_norm_mu0, 219 | d_norm_var0.diag(), 220 | d_norm_var_f.diag(), 221 | d_norm_var_g.diag()], 0))) 222 | # 223 | if (theta_mean < tol_out): 224 | cond = True 225 | verbose = True 226 | convergence_status = 1 227 | if (iter_out >= iter_out_max): 228 | cond = True 229 | verbose = True 230 | convergence_status = 2 231 | 232 | # PRINT 233 | if (verbose): 234 | print('iteration:\t\t{:07d} / {:07d}'.format(iter_out, iter_out_max)) 235 | print('min./max. theta0:') 236 | print('min./max. mu0_t0:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_0[:3])), float(torch.max(mu0_0[:3])))) 237 | print('min./max. mu0_r0:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_0[3:6])), float(torch.max(mu0_0[3:6])))) 238 | print('min./max. mu0_r:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_0[6:])), float(torch.max(mu0_0[6:])))) 239 | print('var0:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(var0_0)), 240 | float(torch.max(var0_0)))) 241 | print('var0_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var0_0))), 242 | float(torch.max(torch.diag(var0_0))))) 243 | print('A:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(A_0)), 244 | float(torch.max(A_0)))) 245 | print('var_f:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(var_f_0)), 246 | float(torch.max(var_f_0)))) 247 | print('var_f_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var_f_0))), 248 | float(torch.max(torch.diag(var_f_0))))) 249 | print('var_g_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var_g_0))), 250 | float(torch.max(torch.diag(var_g_0))))) 251 | print('log det var0:\t\t{:0.8e}'.format(float(torch.logdet(var0_0)))) 252 | print('log det var_f:\t\t{:0.8e}'.format(float(torch.logdet(var_f_0)))) 253 | print('log det var_g:\t\t{:0.8e}'.format(float(torch.logdet(var_g_0)))) 254 | print('min./max. theta1:') 255 | print('min./max. mu0_t0:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_1[:3])), float(torch.max(mu0_1[:3])))) 256 | print('min./max. mu0_r0:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_1[3:6])), float(torch.max(mu0_1[3:6])))) 257 | print('min./max. mu0_r:\t{:0.8e} / {:0.8e}'.format(float(torch.min(mu0_1[6:])), float(torch.max(mu0_1[6:])))) 258 | print('var0:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(var0_1)), 259 | float(torch.max(var0_1)))) 260 | print('var0_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var0_1))), 261 | float(torch.max(torch.diag(var0_1))))) 262 | print('A:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(A_1)), 263 | float(torch.max(A_1)))) 264 | print('var_f:\t\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(var_f_1)), 265 | float(torch.max(var_f_1)))) 266 | print('var_f_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var_f_1))), 267 | float(torch.max(torch.diag(var_f_1))))) 268 | print('var_g_diag:\t\t{:0.8e} / {:0.8e}'.format(float(torch.min(torch.diag(var_g_1))), 269 | float(torch.max(torch.diag(var_g_1))))) 270 | print('log det var0:\t\t{:0.8e}'.format(float(torch.logdet(var0_1)))) 271 | print('log det var_f:\t\t{:0.8e}'.format(float(torch.logdet(var_f_1)))) 272 | print('log det var_g:\t\t{:0.8e}'.format(float(torch.logdet(var_g_1)))) 273 | print('|theta1 - theta0|:') 274 | print('mu0_t0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_mu0[:3])), 275 | float(torch.mean(d_mu0[:3])), 276 | float(torch.max(d_mu0[:3])), 277 | float(torch.median(d_mu0[:3])))) 278 | print('mu0_r0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_mu0[3:6])), 279 | float(torch.mean(d_mu0[3:6])), 280 | float(torch.max(d_mu0[3:6])), 281 | float(torch.median(d_mu0[3:6])))) 282 | print('mu0_r:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_mu0[6:])), 283 | float(torch.mean(d_mu0[6:])), 284 | float(torch.max(d_mu0[6:])), 285 | float(torch.median(d_mu0[6:])))) 286 | print('var0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_var0[~torch.isnan(d_var0)])), 287 | float(torch.mean(d_var0[~torch.isnan(d_var0)])), 288 | float(torch.max(d_var0[~torch.isnan(d_var0)])), 289 | float(torch.median(d_var0[~torch.isnan(d_var0)])))) 290 | print('var0_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_var0))), 291 | float(torch.mean(torch.diag(d_var0))), 292 | float(torch.max(torch.diag(d_var0))), 293 | float(torch.median(torch.diag(d_var0))))) 294 | print('A:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_A[~torch.isnan(d_A)])), 295 | float(torch.mean(d_A[~torch.isnan(d_A)])), 296 | float(torch.max(d_A[~torch.isnan(d_A)])), 297 | float(torch.median(d_A[~torch.isnan(d_A)])))) 298 | print('var_f:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_var_f[~torch.isnan(d_var_f)])), 299 | float(torch.mean(d_var_f[~torch.isnan(d_var_f)])), 300 | float(torch.max(d_var_f[~torch.isnan(d_var_f)])), 301 | float(torch.median(d_var_f[~torch.isnan(d_var_f)])))) 302 | print('var_f_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_var_f))), 303 | float(torch.mean(torch.diag(d_var_f))), 304 | float(torch.max(torch.diag(d_var_f))), 305 | float(torch.median(torch.diag(d_var_f))))) 306 | print('var_g_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_var_g))), 307 | float(torch.mean(torch.diag(d_var_g))), 308 | float(torch.max(torch.diag(d_var_g))), 309 | float(torch.median(torch.diag(d_var_g))))) 310 | print('norm. |theta1 - theta0|:') 311 | print('mu0_t0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_mu0[:3])), 312 | float(torch.mean(d_norm_mu0[:3])), 313 | float(torch.max(d_norm_mu0[:3])), 314 | float(torch.median(d_norm_mu0[:3])))) 315 | print('mu0_r0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_mu0[3:6])), 316 | float(torch.mean(d_norm_mu0[3:6])), 317 | float(torch.max(d_norm_mu0[3:6])), 318 | float(torch.median(d_norm_mu0[3:6])))) 319 | print('mu0_r:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_mu0[6:])), 320 | float(torch.mean(d_norm_mu0[6:])), 321 | float(torch.max(d_norm_mu0[6:])), 322 | float(torch.median(d_norm_mu0[6:])))) 323 | print('var0:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_var0[~torch.isnan(d_norm_var0)])), 324 | float(torch.mean(d_norm_var0[~torch.isnan(d_norm_var0)])), 325 | float(torch.max(d_norm_var0[~torch.isnan(d_norm_var0)])), 326 | float(torch.median(d_norm_var0[~torch.isnan(d_norm_var0)])))) 327 | print('var0_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_norm_var0))), 328 | float(torch.mean(torch.diag(d_norm_var0))), 329 | float(torch.max(torch.diag(d_norm_var0))), 330 | float(torch.median(torch.diag(d_norm_var0))))) 331 | print('A:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_A[~torch.isnan(d_norm_A)])), 332 | float(torch.mean(d_norm_A[~torch.isnan(d_norm_A)])), 333 | float(torch.max(d_norm_A[~torch.isnan(d_norm_A)])), 334 | float(torch.median(d_norm_A[~torch.isnan(d_norm_A)])))) 335 | print('var_f:\t\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(d_norm_var_f[~torch.isnan(d_norm_var_f)])), 336 | float(torch.mean(d_norm_var_f[~torch.isnan(d_norm_var_f)])), 337 | float(torch.max(d_norm_var_f[~torch.isnan(d_norm_var_f)])), 338 | float(torch.median(d_norm_var_f[~torch.isnan(d_norm_var_f)])))) 339 | print('var_f_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_norm_var_f))), 340 | float(torch.mean(torch.diag(d_norm_var_f))), 341 | float(torch.max(torch.diag(d_norm_var_f))), 342 | float(torch.median(torch.diag(d_norm_var_f))))) 343 | print('var_g_diag:\t\t{:0.8e} / {:0.8e} / {:0.8e}\t{:0.8e}'.format(float(torch.min(torch.diag(d_norm_var_g))), 344 | float(torch.mean(torch.diag(d_norm_var_g))), 345 | float(torch.max(torch.diag(d_norm_var_g))), 346 | float(torch.median(torch.diag(d_norm_var_g))))) 347 | print('summary statistics:') 348 | print('E(norm. |mu0_1 - mu0_0|):\t{:0.8e}'.format(d_norm_mu0_mean)) 349 | print('E(norm. |var0_1 - var0_0|):\t{:0.8e}'.format(d_norm_var0_mean)) 350 | print('E(norm. |var_f_1 - var_f_0|):\t{:0.8e}'.format(d_norm_var_f_mean)) 351 | print('E(norm. |var_g_1 - var_g_0|):\t{:0.8e}'.format(d_norm_var_g_mean)) 352 | print('E(norm. |theta_1 - theta_0|):\t{:0.8e} (goal: {:0.8e})'.format(theta_mean, tol_out)) 353 | print() 354 | # 355 | if (cond): 356 | if (convergence_status == 1): 357 | print('CONVERGENCE') 358 | elif (convergence_status == 2): 359 | print('MAX. NUMBER OF ITERATIONS REACHED') 360 | print('FINISHED POSE RECONSTRUCTION ({:s})'.format(cfg.folder_save)) 361 | print() 362 | 363 | # update arrays for next iteration 364 | if not(cond): 365 | iter_out += 1 366 | # parameters 367 | mu0_0.data.copy_(mu0_1.data) 368 | var0_0.data.copy_(var0_1.data) 369 | A_0.data.copy_(A_1.data) 370 | var_f_0.data.copy_(var_f_1.data) 371 | var_g_0.data.copy_(var_g_1.data) 372 | var_g_0_L.data.copy_(var_g_1_L.data) 373 | # other 374 | measurement_expectation0.data.copy_(measurement_expectation1.data) 375 | 376 | # save (change this to torch.save) 377 | if (((iter_out % nSave) == 0) or cond): 378 | save_dict = dict() 379 | if (cond): 380 | if (convergence_status == 1): 381 | save_dict['status'] = 1 382 | save_dict['message'] = 'CONVERGENCE' 383 | elif (convergence_status == 2): 384 | save_dict['status'] = 2 385 | save_dict['message'] = 'MAX. NUMBER OF ITERATIONS REACHED' 386 | # 387 | save_dict['mode'] = cfg.mode 388 | save_dict['mu0'] = mu0_1.detach().cpu().numpy() 389 | save_dict['var0'] = var0_1.detach().cpu().numpy() 390 | save_dict['A'] = A_1.detach().cpu().numpy() 391 | save_dict['var_f'] = var_f_1.detach().cpu().numpy() 392 | save_dict['var_g'] = var_g_1.detach().cpu().numpy() 393 | # 394 | save_dict['mu_uks'] = mu_kalman.detach().cpu().numpy() 395 | save_dict['var_uks'] = var_kalman.detach().cpu().numpy() 396 | # 397 | save_dict['cond'] = cond 398 | save_dict['iter_out'] = iter_out 399 | # 400 | np.save(os.path.join(cfg.folder_save,'save_dict.npy'), save_dict) 401 | return mu0_1, var0_1, A_1, var_f_1, var_g_1 402 | -------------------------------------------------------------------------------- /ACM/em_helper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import torch 5 | import time 6 | 7 | import configuration as cfg 8 | import em 9 | import helper 10 | import kalman 11 | import model 12 | 13 | 14 | def gen_args_Qg(args): 15 | args_kalman = args['args_kalman'] 16 | nT = args_kalman['nT'] 17 | dim_x = args_kalman['dim_x'] 18 | nSigmaPoints = args_kalman['nSigmaPoints'] 19 | w_m = args_kalman['w_m'] 20 | measure = args_kalman['measure'].cpu().numpy() 21 | measure_mask = args_kalman['measure_mask'].cpu().numpy().astype(bool) 22 | # 23 | not_measure_mask = ~measure_mask 24 | # 25 | measure_use = np.tile(measure, nSigmaPoints) 26 | measure_use = np.reshape(measure_use, (nT, nSigmaPoints, dim_x)) 27 | # 28 | nMeasure = np.sum(measure_mask).astype(np.float64) 29 | nMeasure_times_log2pi = nMeasure * np.log(2.0 * np.pi) 30 | nMeasure_t = np.sum(measure_mask, 0).astype(np.float64) 31 | # 32 | trace_diag_elements = np.zeros((nT, dim_x), dtype=np.float64) 33 | Qg = np.zeros(1, dtype=np.float64) 34 | grad = np.zeros(dim_x, dtype=np.float64) 35 | # 36 | # 37 | args_Qg = dict() 38 | args_Qg['w_m'] = w_m 39 | args_Qg['not_measure_mask'] = not_measure_mask 40 | args_Qg['measure_use'] = measure_use 41 | args_Qg['nMeasure_times_log2pi'] = nMeasure_times_log2pi 42 | args_Qg['nMeasure_t'] = nMeasure_t 43 | # 44 | args_Qg['trace_diag_elements'] = trace_diag_elements 45 | args_Qg['Qg'] = Qg 46 | args_Qg['grad'] = grad 47 | return args_Qg 48 | 49 | def print_args(args, s): 50 | buffer = 30 51 | for key in np.sort(list(args.keys())): 52 | key_use = s + key + ':' + ' ' * (buffer - len(key)) 53 | key_type = type(args[key]) 54 | if (key_type == type(dict())): 55 | print('{:s}:{:s}'.format(key_use, str(key_type))) 56 | s_use = s + '\t' 57 | print_args(args[key], s_use) 58 | elif(key_type == type(torch.Tensor([]))): 59 | print('{:s}:{:s} (is_cuda = {:s})'.format(key_use, str(key_type), str(args[key].is_cuda))) 60 | else: 61 | print('{:s}:'.format(key_use, str(key_type))) 62 | return 63 | 64 | def convert_dtype(args_in): 65 | args_out = dict() 66 | for key in np.sort(list(args_in.keys())): 67 | key_type = type(args_in[key]) 68 | if (key_type == type(dict())): 69 | args_out[key] = convert_dtype(args_in[key]) 70 | elif (key_type == type(torch.Tensor([]))): 71 | args_out[key] = args_in[key].half() 72 | # elif (key_type == type(float())): 73 | # args_out[key] = torch.float16(args_in[key]) 74 | # elif (key_type == type(int())): 75 | # args_out[key] = torch.int16(args_in[key]) 76 | else: 77 | args_out[key] = args_in[key] 78 | return args_out 79 | 80 | def make_args_torch(args): 81 | args_torch = dict() 82 | for key in np.sort(list(args.keys())): 83 | key_type = type(args[key]) 84 | if (key_type == type(dict())): 85 | args_torch[key] = make_args_gpu(args[key]) 86 | elif (key_type == type(np.array([]))): 87 | arg_use = args[key] 88 | if (args[key].dtype == 'bool'): 89 | arg_use = arg_use.astype(np.uint8) 90 | args_torch[key] = torch.from_numpy(arg_use) 91 | elif (key_type == type(torch.Tensor([]))): 92 | args_torch[key] = args[key].detach() 93 | elif (key_type == type(np.float64())): 94 | args_torch[key] = float(args[key]) 95 | elif (key_type == type(np.int64())): 96 | args_torch[key] = int(args[key]) 97 | else: 98 | args_torch[key] = args[key] 99 | return args_torch 100 | 101 | def make_args_gpu(args): 102 | args_gpu = dict() 103 | for key in np.sort(list(args.keys())): 104 | key_type = type(args[key]) 105 | if (key_type == type(dict())): 106 | args_gpu[key] = make_args_gpu(args[key]) 107 | elif (key_type == type(np.array([]))): 108 | arg_use = args[key] 109 | if (args[key].dtype == 'bool'): 110 | arg_use = arg_use.astype(np.uint8) 111 | args_gpu[key] = torch.from_numpy(arg_use).cuda() 112 | elif (key_type == type(torch.Tensor([]))): 113 | args_gpu[key] = args[key].detach().cuda() 114 | elif (key_type == type(np.float64())): 115 | args_gpu[key] = float(args[key]) 116 | elif (key_type == type(np.int64())): 117 | args_gpu[key] = int(args[key]) 118 | else: 119 | args_gpu[key] = args[key] 120 | return args_gpu 121 | 122 | def convert_to_torch(arg_in): 123 | key_type = type(arg_in) 124 | if (key_type == type(np.array([]))): 125 | arg_out = arg_in 126 | if (arg_out.dtype == 'bool'): 127 | arg_out = arg_use.astype(np.uint8) 128 | arg_out = torch.from_numpy(arg_use) 129 | elif (key_type == type(torch.Tensor([]))): 130 | arg_out = arg_in.detach() 131 | elif (key_type == type(np.float64())): 132 | arg_out = float(arg_in) 133 | elif (key_type == type(np.int64())): 134 | arg_out = int(arg_in) 135 | else: 136 | arg_out = arg_in 137 | return arg_out 138 | 139 | def convert_to_gpu(arg_in): 140 | key_type = type(arg_in) 141 | if (key_type == type(np.array([]))): 142 | arg_out = arg_in 143 | if (arg_out.dtype == 'bool'): 144 | arg_out = arg_use.astype(np.uint8) 145 | arg_out = torch.from_numpy(arg_use).cuda() 146 | elif (key_type == type(torch.Tensor([]))): 147 | arg_out = arg_in.detach().cuda() 148 | elif (key_type == type(np.float64())): 149 | arg_out = float(arg_in) 150 | elif (key_type == type(np.int64())): 151 | arg_out = int(arg_in) 152 | else: 153 | arg_out = arg_in 154 | return arg_out -------------------------------------------------------------------------------- /ACM/export.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import importlib 4 | import numpy as np 5 | import os 6 | from scipy import io 7 | import sys 8 | import torch 9 | 10 | def export(result_path): 11 | folder_save = result_path 12 | project_path = os.path.abspath(result_path+"/../..") 13 | 14 | sys.path.append(project_path) 15 | print(project_path) 16 | import configuration as cfg 17 | from ACM import anatomy 18 | from ACM import helper 19 | from ACM import model 20 | from ACM import routines_math as rout_m 21 | sys.path.pop(sys.path.index(project_path)) 22 | 23 | if not hasattr(cfg,'animal_is_large'): 24 | cfg.animal_is_large = False 25 | 26 | list_is_large_animal = [cfg.animal_is_large] 27 | 28 | importlib.reload(anatomy) 29 | 30 | # get arguments 31 | file_origin_coord = cfg.file_origin_coord 32 | file_calibration = cfg.file_calibration 33 | file_model = cfg.file_model 34 | file_labelsDLC = cfg.file_labelsDLC 35 | 36 | args_model = helper.get_arguments(file_origin_coord, file_calibration, file_model, file_labelsDLC, 37 | cfg.scale_factor, cfg.pcutoff) 38 | 39 | args_model['use_custom_clip'] = True 40 | nBones = args_model['numbers']['nBones'] 41 | nMarkers = args_model['numbers']['nMarkers'] 42 | # 43 | joint_order = args_model['model']['joint_order'] 44 | skel_edges = args_model['model']['skeleton_edges'].cpu().numpy() 45 | skel_coords0 = args_model['model']['skeleton_coords0'].cpu().numpy() 46 | bone_lengths_index = args_model['model']['bone_lengths_index'].cpu().numpy() 47 | 48 | # get save_dict 49 | save_dict = np.load(result_path+'/save_dict.npy', allow_pickle=True).item() 50 | if ('mu_uks' in save_dict): 51 | mu_uks_norm = np.copy(save_dict['mu_uks'][1:]) 52 | else: 53 | mu_uks_norm = np.copy(save_dict['mu_fit'][1:]) 54 | mu_uks = model.undo_normalization(torch.from_numpy(mu_uks_norm), args_model).numpy() # reverse normalization 55 | 56 | # get x_ini 57 | x_ini = np.load(result_path+'/x_ini.npy', allow_pickle=True) 58 | 59 | # free parameters 60 | free_para_pose = args_model['free_para_pose'].cpu().numpy() 61 | free_para_bones = np.zeros(nBones, dtype=bool) 62 | free_para_markers = np.zeros(nMarkers*3, dtype=bool) 63 | free_para = np.concatenate([free_para_bones, 64 | free_para_markers, 65 | free_para_pose], 0) 66 | 67 | # full x 68 | nT_use = np.size(mu_uks_norm, 0) 69 | x = np.tile(x_ini, nT_use).reshape(nT_use, len(x_ini)) 70 | x[:, free_para] = mu_uks 71 | 72 | # requested parameters 73 | nPara_bones = args_model['nPara_bones'] 74 | nPara_markers = args_model['nPara_markers'] 75 | nPara_skel = nPara_bones + nPara_markers 76 | # 77 | bone_lengths = x_ini[:nPara_bones] # normalization in x_ini already reversed 78 | bone_lengths = bone_lengths[bone_lengths_index] # fill bone lengths of 'right' bones with the values from the 'left' bones 79 | # 80 | t0 = x[:, nPara_skel:nPara_skel+3].reshape(nT_use, 3) 81 | r = x[:, nPara_skel+3:].reshape(nT_use, nBones, 3) 82 | R = np.zeros((nT_use, nBones, 3, 3), dtype=np.float64) 83 | for i in range(nT_use): 84 | for j in range(nBones): 85 | R[i, j] = rout_m.rodrigues2rotMat_single(r[i, j]) 86 | 87 | # dict to be saved 88 | data_dict = dict() 89 | data_dict['joint_names'] = joint_order 90 | data_dict['edges'] = skel_edges 91 | data_dict['coords0'] = skel_coords0 92 | data_dict['bone_lengths'] = bone_lengths 93 | data_dict['t'] = t0 94 | data_dict['R'] = R 95 | data_dict['index_frame_start'] = cfg.index_frame_start 96 | data_dict['origin'] = np.load(file_origin_coord,allow_pickle=True).item() 97 | data_dict['folder_ccv'] = cfg.folder_video 98 | 99 | # save 100 | np.savez(folder_save+'/motiondata.npz', data_dict) 101 | io.savemat(folder_save+'/motiondata.mat', data_dict) 102 | 103 | if __name__ == '__main__': 104 | export(sys.argv[1]) 105 | -------------------------------------------------------------------------------- /ACM/fitting.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | import numpy as np 5 | import torch 6 | from scipy.io import savemat 7 | 8 | import configuration as cfg 9 | 10 | from . import calibration as calib 11 | from . import helper 12 | from . import model 13 | from . import optimization as opt 14 | 15 | def main(): 16 | # get arguments 17 | args = helper.get_arguments(cfg.file_origin_coord, cfg.file_calibration, cfg.file_model, cfg.file_labelsDLC, 18 | cfg.scale_factor, cfg.pcutoff) 19 | args['use_custom_clip'] = False 20 | 21 | # get relevant information from arguments 22 | nBones = args['numbers']['nBones'] 23 | nMarkers = args['numbers']['nMarkers'] 24 | nCameras = args['numbers']['nCameras'] 25 | joint_order = args['model']['joint_order'] # list 26 | joint_marker_order = args['model']['joint_marker_order'] # list 27 | skeleton_edges = args['model']['skeleton_edges'].cpu().numpy() 28 | bone_lengths_index = args['model']['bone_lengths_index'].cpu().numpy() 29 | joint_marker_index = args['model']['joint_marker_index'].cpu().numpy() 30 | # 31 | free_para_bones = args['free_para_bones'].cpu().numpy() 32 | free_para_markers = args['free_para_markers'].cpu().numpy() 33 | free_para_pose = args['free_para_pose'].cpu().numpy() 34 | nPara_bones = args['nPara_bones'] 35 | nPara_markers = args['nPara_markers'] 36 | nPara_pose = args['nPara_pose'] 37 | nFree_pose = args['nFree_pose'] 38 | 39 | # remove all free parameters that do not modify the pose 40 | free_para_bones = np.zeros_like(free_para_bones, dtype=bool) 41 | free_para_markers = np.zeros_like(free_para_markers, dtype=bool) 42 | nFree_bones = int(0) 43 | nFree_markers = int(0) 44 | args['free_para_bones'] = torch.from_numpy(free_para_bones) 45 | args['free_para_markers'] = torch.from_numpy(free_para_markers) 46 | args['nFree_bones'] = nFree_bones 47 | args['nFree_markers'] = nFree_markers 48 | 49 | # get index of initialization frame 50 | labelsDLC = np.load(cfg.file_labelsDLC, allow_pickle=True).item() 51 | frame_list = labelsDLC['frame_list'] 52 | labels_dlc = labelsDLC['labels_all'] 53 | 54 | # create correct free_para 55 | free_para = np.concatenate([free_para_bones, 56 | free_para_markers, 57 | free_para_pose], 0) 58 | # load x_ini 59 | x_ini = np.load(cfg.folder_save + '/x_ini.npy', allow_pickle=True) 60 | x_ini_free = x_ini[free_para] 61 | 62 | # update args regarding fixed tensors 63 | args['plot'] = False 64 | args['nFrames'] = int(1) 65 | 66 | # update args regarding x0 and labels 67 | # ARGS X 68 | args['x_torch'] = torch.from_numpy(x_ini) 69 | args['x_free_torch'] = torch.from_numpy(x_ini[free_para]) 70 | args['x_free_torch'].requires_grad = True 71 | # ARGS LABELS DLC 72 | labels_dlc = np.load(cfg.file_labelsDLC, allow_pickle=True).item() 73 | i_frame_single = np.where(cfg.index_frame_ini == labels_dlc['frame_list'])[0][0] 74 | args['labels_single_torch'] = args['labels'][i_frame_single][None, :].clone() 75 | args['labels_mask_single_torch'] = args['labels_mask'][i_frame_single][None, :].clone() 76 | 77 | # BOUNDS 78 | # pose 79 | bounds_free_pose = args['bounds_free_pose'] 80 | bounds_free_low_pose = model.do_normalization(bounds_free_pose[:, 0][None, :], args).numpy().ravel() 81 | bounds_free_high_pose = model.do_normalization(bounds_free_pose[:, 1][None, :], args).numpy().ravel() 82 | bounds_free = np.stack([bounds_free_low_pose, bounds_free_high_pose], 1) 83 | args['bounds_free'] = bounds_free 84 | 85 | # normalize x 86 | x_ini_free_norm = model.do_normalization(torch.from_numpy(x_ini_free.reshape(1, nFree_pose)), args).numpy().ravel() 87 | 88 | # OPTIMIZE 89 | # create optimization dictonary 90 | opt_options = dict() 91 | opt_options['disp'] = cfg.opt_options_fit__disp 92 | opt_options['maxiter'] = cfg.opt_options_fit__maxiter 93 | opt_options['maxcor'] = cfg.opt_options_fit__maxcor 94 | opt_options['ftol'] = cfg.opt_options_fit__ftol 95 | opt_options['gtol'] = cfg.opt_options_fit__gtol 96 | opt_options['maxfun'] = cfg.opt_options_fit__maxfun 97 | opt_options['iprint'] = cfg.opt_options_fit__iprint 98 | opt_options['maxls'] = cfg.opt_options_fit__maxls 99 | opt_dict = dict() 100 | opt_dict['opt_method'] = cfg.opt_method_fit 101 | opt_dict['opt_options'] = opt_options 102 | 103 | # optimize 104 | x_fit_free_norm__all = np.zeros((cfg.nT+1, nFree_pose), dtype=np.float64) 105 | x_fit_free_norm__all[0, :] = np.nan # corresponds to mu0 106 | x_previous = np.copy(x_ini) 107 | x_previous_free_norm = np.copy(x_ini_free_norm) 108 | x_fit_free_norm__all__counter = 1 109 | for i in np.arange(cfg.nT): 110 | # PRINT 111 | print('Optimizing frame {:09d} / {:09d}'.format(cfg.index_frame_ini + i * cfg.dt, cfg.index_frame_ini + cfg.nT * cfg.dt - 1)) 112 | 113 | # update args regarding x0 and labels 114 | # ARGS X 115 | args['x_torch'].data.copy_(torch.from_numpy(x_previous).data) 116 | args['x_free_torch'].data.copy_(torch.from_numpy(x_previous[free_para]).data) 117 | # ARGS LABELS DLC 118 | i_frame_single = np.where((cfg.index_frame_ini + i * cfg.dt) == labels_dlc['frame_list'])[0][0] 119 | args['labels_single_torch'].data.copy_(args['labels'][i_frame_single][None, :].data) 120 | args['labels_mask_single_torch'].data.copy_(args['labels_mask'][i_frame_single][None, :].data) 121 | 122 | # optimize 123 | min_result = opt.optimize__scipy(x_previous_free_norm, args, 124 | opt_dict) 125 | x_fit_free_norm = np.copy(min_result.x) 126 | x_fit_free = model.undo_normalization(torch.from_numpy(x_fit_free_norm).reshape(1, nFree_pose), args).numpy().ravel() 127 | 128 | x_fit_free_norm__all[i+1] = np.copy(x_fit_free_norm) 129 | 130 | x_previous_free_norm = np.copy(x_fit_free_norm) 131 | x_previous[free_para] = np.copy(x_fit_free) 132 | 133 | # save 134 | save_dict = dict() 135 | save_dict['mu_fit'] = x_fit_free_norm__all 136 | np.save(os.path.join(cfg.folder_save,'save_dict.npy'), save_dict) 137 | 138 | # to get 3D joint & marker locations 139 | args['plot'] = True 140 | marker_proj, marker_pos, skel_pos = model.fcn_emission_free(torch.from_numpy(x_fit_free_norm__all[1:]), args) 141 | marker_proj = marker_proj.detach().cpu().numpy().reshape(cfg.nT, nCameras, nMarkers, 2) 142 | marker_pos = marker_pos.detach().cpu().numpy() 143 | skel_pos = skel_pos.detach().cpu().numpy() 144 | pose = dict() 145 | pose['marker_positions_2d'] = marker_proj 146 | pose['marker_positions_3d'] = marker_pos 147 | pose['joint_positions_3d'] = skel_pos 148 | 149 | posepath = os.path.join(cfg.folder_save,'pose') 150 | print(f'Saving pose to {posepath}') 151 | np.save(posepath+'.npy', pose) 152 | savemat(posepath+'.mat', pose) 153 | 154 | if __name__ == "__main__": 155 | main() -------------------------------------------------------------------------------- /ACM/gui/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/ACM/gui/__init__.py -------------------------------------------------------------------------------- /ACM/gui/viewer.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | from PyQt5.QtWidgets import QApplication, QMainWindow, QFileDialog, QFrame, QGridLayout, \ 3 | QSlider, QComboBox, QLineEdit, QCheckBox, QPushButton, QScrollArea, QAction 4 | from PyQt5.QtCore import Qt 5 | 6 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg 7 | from matplotlib.figure import Figure 8 | 9 | import re 10 | 11 | import imageio 12 | from ccvtools import rawio 13 | 14 | import numpy as np 15 | import torch 16 | 17 | from ..model import map_m 18 | from ..helper import get_calibration 19 | 20 | from pprint import pprint 21 | 22 | 23 | class Viewer(QMainWindow): 24 | app = [] 25 | config = dict() 26 | vidreader = [] 27 | results = [] 28 | calib = [] 29 | frames = [] 30 | 31 | labels_dlc = None 32 | labels_manual = None 33 | 34 | camidx = 0 35 | frameidx = 0 36 | resultidx = 0 37 | 38 | frame_main = [] 39 | 40 | frame_slider = [] 41 | frame_lineedit = [] 42 | frame_buttonback = [] 43 | frameforward_button = [] 44 | frameskip_lineedit = [] 45 | cam_combobox = [] 46 | result_combobox = [] 47 | marker_checkboxes = [] 48 | 49 | menu = dict() 50 | 51 | plot_components = {'dlc': [], 'manual': []} 52 | 53 | frame_cache = [-1, -1, []] 54 | 55 | clear = 4 56 | 57 | fig = [] 58 | canvas = [] 59 | ax = [] 60 | 61 | globalnotify = True 62 | 63 | def __init__(self, app, config): 64 | super().__init__() 65 | 66 | self.app = app 67 | 68 | self.config = config 69 | 70 | if 'videos' not in self.config: 71 | self.config['videos'] = self.read_videos() 72 | 73 | for f in self.config['videos']: 74 | self.vidreader.append(imageio.get_reader(f)) 75 | 76 | self.calib = get_calibration(self.get_config()['file_origin_coord'], self.get_config()['file_calibration'], 77 | self.get_config()['scale_factor']) 78 | 79 | self.results = os.listdir(os.path.join(self.get_config()['folder_project'], 'results')) 80 | self.results.insert(0, '') 81 | 82 | try: 83 | self.frames = np.asarray(list(range(max([vr.count_frames() for vr in self.vidreader])))) 84 | except AttributeError: 85 | self.frames = np.asarray(list(range(max([len(vr) for vr in self.vidreader])))) 86 | 87 | self.make_menu() 88 | self.make_gui() 89 | 90 | print("Setting defaults") 91 | self.set_camidx(0, showframe=False) 92 | self.set_frameidx(0, showframe=False) 93 | self.set_resultidx(0, showframe=False) # len(self.results)-1 94 | print("Done") 95 | self.show_frame() 96 | 97 | def get_config(self): 98 | return self.config 99 | 100 | def get_result(self, resultidx=None): 101 | if resultidx is None: 102 | resultidx = self.resultidx 103 | 104 | if resultidx < len(self.results) and resultidx>0: 105 | print(self.get_config()['folder_project']) 106 | print(self.results[resultidx]) 107 | resultpath = os.path.join(self.get_config()['folder_project'], 'results', self.results[resultidx]) 108 | 109 | with open(resultpath + "/configuration/configuration.py") as cf: 110 | for line in cf: 111 | x = re.search(r'index_frame_start *= *(\d*)', line) 112 | if x is not None: 113 | break 114 | if x is None: 115 | raise RuntimeError("Did not find index_frame_start") 116 | else: 117 | index_frame_start = int(x.group(1)) 118 | 119 | with open(resultpath + "/configuration/configuration.py") as cf: 120 | for line in cf: 121 | x = re.search(r'index_frame_end *= *(\d*)', line) 122 | if x is not None: 123 | break 124 | if x is None: 125 | raise RuntimeError("Did not find index_frame_end") 126 | else: 127 | index_frame_end = int(x.group(1)) 128 | 129 | with open(resultpath + "/configuration/configuration.py") as cf: 130 | for line in cf: 131 | x = re.search(r'dt *= *(\d*)', line) 132 | if x is not None: 133 | break 134 | if x is None: 135 | raise RuntimeError("Did not find dt") 136 | else: 137 | dt = int(x.group(1)) 138 | 139 | frames = np.asarray(list(range(index_frame_start, index_frame_end, dt))) 140 | 141 | posefile = os.path.join(resultpath, 'pose.npy') 142 | if os.path.isfile(posefile): 143 | model = np.load(os.path.join(self.get_config()['folder_project'], "model.npy"), 144 | allow_pickle=True).item() 145 | pose = np.load(posefile, allow_pickle=True).item() 146 | 147 | return pose, model, frames 148 | else: 149 | print("Result not found") 150 | 151 | def get_vidreader(self, vididx): 152 | return self.vidreader[vididx] 153 | 154 | def make_menu(self): 155 | self.menu['menu'] = self.menuBar() 156 | 157 | self.menu['menu_file'] = self.menu['menu'].addMenu("&Export") 158 | 159 | saveframe_action = QAction("&Save frame", self) 160 | saveframe_action.triggered.connect(self.save_frame) 161 | self.menu['menu_file'].addAction(saveframe_action) 162 | 163 | def make_gui(self): 164 | self.setGeometry(0, 0, 1280, 900) 165 | self.setWindowTitle('ACM viewer') 166 | 167 | self.frame_main = QFrame() 168 | layout_grid = QGridLayout() 169 | layout_grid.setSpacing(0) 170 | layout_grid.setRowStretch(0, 9) 171 | layout_grid.setRowStretch(1, 1) 172 | layout_grid.setColumnStretch(0, 5) 173 | layout_grid.setColumnStretch(1, 1) 174 | self.frame_main.setLayout(layout_grid) 175 | 176 | frame_plot = QFrame() 177 | layout_grid.addWidget(frame_plot, 0, 0) 178 | layout_grid_plot = QGridLayout() 179 | layout_grid_plot.setSpacing(0) 180 | frame_plot.setLayout(layout_grid_plot) 181 | 182 | frame_labels = QScrollArea() # TODO: Make this properly scollable 183 | layout_grid.addWidget(frame_labels, 0, 1) 184 | layout_grid_labels = QGridLayout() 185 | layout_grid_labels.setSpacing(0) 186 | frame_labels.setLayout(layout_grid_labels) 187 | 188 | frame_controls = QFrame() 189 | layout_grid.addWidget(frame_controls, 1, 0, 1, 2) 190 | layout_grid_control = QGridLayout() 191 | layout_grid_control.setSpacing(10) 192 | layout_grid_control.setRowStretch(0, 1) 193 | layout_grid_control.setRowStretch(1, 1) 194 | layout_grid_control.setColumnStretch(0, 1) 195 | layout_grid_control.setColumnStretch(1, 2) 196 | layout_grid_control.setColumnStretch(2, 2) 197 | layout_grid_control.setColumnStretch(3, 1) 198 | layout_grid_control.setColumnStretch(4, 0) 199 | layout_grid_control.setColumnStretch(5, 0) 200 | frame_controls.setLayout(layout_grid_control) 201 | 202 | model = np.load(self.get_config()['file_model'], allow_pickle=True).item() 203 | for i, mlabel in enumerate(model['joint_marker_order']): 204 | self.marker_checkboxes.append(QCheckBox()) 205 | self.marker_checkboxes[-1].setChecked(True) 206 | self.marker_checkboxes[-1].setText(mlabel) 207 | self.marker_checkboxes[-1].toggled.connect(lambda: self.show_frame(actions=["dlc", "draw"])) 208 | layout_grid_labels.addWidget(self.marker_checkboxes[-1], i, 0) 209 | 210 | self.frame_slider = QSlider(Qt.Horizontal) 211 | self.frame_slider.setMinimum(self.frames[0]) 212 | self.frame_slider.setMaximum(self.frames[-1]) 213 | self.frame_slider.setTickInterval(1) 214 | self.frame_slider.sliderReleased.connect(lambda: self.set_frameidx(self.frame_slider.value())) 215 | layout_grid_control.addWidget(self.frame_slider, 0, 0, 1, 3) 216 | 217 | self.frame_lineedit = QLineEdit() 218 | self.frame_lineedit.editingFinished.connect(lambda: self.set_frameidx(int(self.frame_lineedit.text()))) 219 | # TODO add QtValidator 220 | layout_grid_control.addWidget(self.frame_lineedit, 0, 3, 1, 1) 221 | 222 | self.frame_buttonback = QPushButton() 223 | self.frame_buttonback.setText('<') 224 | self.frame_buttonback.setMinimumWidth(1) 225 | self.frame_buttonback.clicked.connect( 226 | lambda: 227 | self.set_frameidx(int(self.frame_lineedit.text()) - int(self.frameskip_lineedit.text()))) 228 | # TODO add QtValidator 229 | layout_grid_control.addWidget(self.frame_buttonback, 0, 4, 1, 1) 230 | 231 | self.frameforward_button = QPushButton() 232 | self.frameforward_button.setText('>') 233 | self.frameforward_button.setMinimumWidth(1) 234 | self.frameforward_button.clicked.connect( 235 | lambda: self.set_frameidx(int(self.frame_lineedit.text()) + int(self.frameskip_lineedit.text()))) 236 | # TODO add QtValidator 237 | layout_grid_control.addWidget(self.frameforward_button, 0, 5, 1, 1) 238 | 239 | self.frameskip_lineedit = QLineEdit() 240 | self.frameskip_lineedit.setText('1') 241 | layout_grid_control.addWidget(self.frameskip_lineedit, 1, 4, 1, 2) 242 | 243 | self.cam_combobox = QComboBox() 244 | self.cam_combobox.addItems([f'Camera {n}' for n in range(len(self.vidreader))]) 245 | self.cam_combobox.currentIndexChanged.connect(lambda n: self.set_camidx(n)) 246 | layout_grid_control.addWidget(self.cam_combobox, 1, 0, 1, 2) 247 | 248 | print(self.get_config()['folder_save']) 249 | self.result_combobox = QComboBox() 250 | self.result_combobox.addItems(self.results) 251 | self.result_combobox.currentIndexChanged.connect(lambda n: self.set_resultidx(n)) 252 | layout_grid_control.addWidget(self.result_combobox, 1, 2, 1, 2) 253 | 254 | self.fig = Figure(tight_layout=True) 255 | self.canvas = FigureCanvasQTAgg(self.fig) 256 | self.canvas.setParent(frame_plot) 257 | self.ax = self.fig.add_subplot(1, 1, 1) 258 | layout_grid_plot.addWidget(self.canvas, 0, 0) 259 | 260 | self.frame_main.setLayout(layout_grid) 261 | self.setCentralWidget(self.frame_main) 262 | self.setFocus() 263 | 264 | self.show() 265 | 266 | def set_camidx(self, camidx, notify=True, showframe=True): 267 | if notify and self.globalnotify: 268 | self.globalnotify = False 269 | self.cam_combobox.setCurrentIndex(camidx) 270 | self.globalnotify = True 271 | 272 | self.camidx = camidx 273 | 274 | if showframe and self.globalnotify: 275 | self.show_frame() 276 | 277 | def set_frameidx(self, frameidx, notify=True, showframe=True): 278 | frameidx = max(frameidx, self.frames[0]) 279 | frameidx = min(frameidx, self.frames[-1]) 280 | 281 | if notify and self.globalnotify: 282 | self.globalnotify = False 283 | print(f"setting slider to {frameidx}") 284 | self.frame_slider.setValue(frameidx) 285 | self.frame_lineedit.setText(str(frameidx)) 286 | self.globalnotify = True 287 | 288 | self.frameidx = frameidx 289 | 290 | if showframe and self.globalnotify: 291 | self.show_frame() 292 | 293 | def set_resultidx(self, resultidx, notify=True, showframe=True): 294 | if notify and self.globalnotify: 295 | self.globalnotify = False 296 | self.result_combobox.setCurrentIndex(resultidx) 297 | self.globalnotify = True 298 | 299 | self.resultidx = resultidx 300 | 301 | if showframe and self.globalnotify: 302 | self.show_frame() 303 | 304 | def get_dlc_labels(self): 305 | if self.labels_dlc is None: 306 | self.labels_dlc = np.load(self.get_config()['file_labelsDLC'], allow_pickle=True).item() 307 | 308 | return self.labels_dlc 309 | 310 | def get_manual_labels(self): 311 | if self.labels_manual is None: 312 | self.labels_manual = np.load(self.get_config()['file_labelsManual'], allow_pickle=True)['arr_0'].item() 313 | 314 | return self.labels_manual 315 | 316 | def show_frame(self, camidx=None, frameidx=None, resultidx=None, 317 | actions=("clear", "imshow", "dlc", "manual", "joints", "draw")): 318 | # TODO implement clearing of respective parts in each section 319 | if camidx is None: 320 | camidx = self.camidx 321 | if frameidx is None: 322 | frameidx = self.frameidx 323 | if resultidx is None: 324 | resultidx = self.resultidx 325 | 326 | if self.frame_cache[0] == camidx and self.frame_cache[1] == frameidx: 327 | frame = self.frame_cache[2] 328 | else: 329 | frame = self.get_vidreader(camidx).get_data(frameidx) 330 | if len(frame.shape) > 2: 331 | frame = frame[:, :, 0] # self.get_config()['folder_project'] 332 | self.frame_cache = [camidx, frameidx, frame] 333 | 334 | # calib = np.load(self.get_config()['file_calibration'], allow_pickle=True).item() 335 | 336 | if "clear" in actions: 337 | self.ax.clear() 338 | self.ax.set_xticklabels('') 339 | self.ax.set_yticklabels('') 340 | self.ax.set_xticks([]) 341 | self.ax.set_yticks([]) 342 | 343 | self.ax.imshow(frame, 344 | aspect=1, 345 | cmap='gray', 346 | vmin=0, 347 | vmax=255) 348 | 349 | # Plot dlc labels if applicable 350 | if "dlc" in actions: 351 | labels_dlc = self.get_dlc_labels() 352 | for c in self.plot_components["dlc"]: 353 | c.remove() 354 | checked_markers = np.asarray([mc.isChecked() for mc in self.marker_checkboxes]) 355 | used_markers = np.squeeze( 356 | labels_dlc['labels_all'][labels_dlc['frame_list'] == frameidx, camidx, :, 2] >= 0.9) 357 | show_mask = used_markers & checked_markers 358 | print(f'DLC used/unused labels: {np.sum(used_markers)}/{np.sum(~used_markers)}') 359 | frame_mask = labels_dlc['frame_list'] == frameidx 360 | if np.sum(frame_mask) == 1: 361 | self.plot_components["dlc"] = self.ax.plot( 362 | labels_dlc['labels_all'][frame_mask, camidx, show_mask, 0], 363 | labels_dlc['labels_all'][frame_mask, camidx, show_mask, 1], 364 | 'bo') 365 | elif np.sum(frame_mask) > 1: 366 | print("Frame found multiple times in DLC data ...") 367 | 368 | # Plot manual labels if applicable 369 | if "manual" in actions: 370 | labels_man = self.get_manual_labels() 371 | for c in self.plot_components["manual"]: 372 | c.remove() 373 | self.plot_components["manual"] = [] 374 | if frameidx in labels_man: 375 | for k in labels_man[frameidx]: 376 | self.plot_components["manual"].append( 377 | self.ax.plot(labels_man[frameidx][k][camidx, 0], labels_man[frameidx][k][camidx, 1], 'g+')[0]) 378 | 379 | if "joints" in actions and resultidx>0: 380 | # Calculate and plot joint positions 381 | try: 382 | pose, model, res_frames = self.get_result(resultidx) 383 | if res_frames[0] <= frameidx <= res_frames[-1]: 384 | closestposeidx = np.argmin(np.abs(res_frames - frameidx)) 385 | joint_positions_3d = pose['joint_positions_3d'][[closestposeidx], :, :] 386 | joint_positions_2d = np.asarray(map_m(self.calib['RX1_fit'], 387 | self.calib['tX1_fit'], 388 | self.calib['A_fit'], 389 | self.calib['k_fit'], 390 | torch.from_numpy(joint_positions_3d))) 391 | 392 | for edge in model['skeleton_edges']: 393 | self.ax.plot(joint_positions_2d[:, camidx, edge, 0][0], 394 | joint_positions_2d[:, camidx, edge, 1][0], 'r') 395 | 396 | self.ax.plot(joint_positions_2d[:, camidx, :, 0], joint_positions_2d[:, camidx, :, 1], 'r+') 397 | else: 398 | print(f"{frameidx} not in result range {res_frames[0]}-{res_frames[-1]}") 399 | except RuntimeError as e: 400 | print(f"Requested pose data not found") 401 | raise e 402 | 403 | if "draw" in actions: 404 | self.canvas.draw() 405 | 406 | def read_videos(self): 407 | vid_dlg = QFileDialog() 408 | dialog_options = vid_dlg.Options() 409 | # dialogOptions |= vidDlg.DontUseNativeDialog 410 | video_files, _ = QFileDialog.getOpenFileNames(vid_dlg, 411 | "Choose video files", 412 | self.config['folder_project'], 413 | "video files (*.*)", 414 | options=dialog_options) 415 | 416 | if len(video_files) == 0: # TODO check match with calibration 417 | print("Select files matching your multicam calibration!", file=sys.stderr) 418 | # self.app.quit() # TODO find proper way here 419 | sys.exit(1) 420 | 421 | video_files = sorted(video_files) 422 | 423 | return video_files 424 | 425 | def save_frame(self): 426 | figpath = os.path.join(self.get_config()['folder_project'], 'results', self.results[self.resultidx], 'exports') 427 | print(f"Saving frame {self.frameidx} to {figpath}... ", end='') 428 | os.makedirs(figpath, exist_ok=True) 429 | self.fig.savefig(os.path.join(figpath, f'frame_{self.frameidx}_{self.results[self.resultidx]}.svg'), 430 | bbox_inches='tight', 431 | dpi=600, 432 | transparent=True, 433 | format='svg', 434 | pad_inches=0) 435 | print('Success') 436 | 437 | 438 | def start(config): 439 | app = QApplication([]) 440 | Viewer(app, config) 441 | sys.exit(app.exec_()) 442 | -------------------------------------------------------------------------------- /ACM/helper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import copy 4 | import numpy as np 5 | import os 6 | import sys 7 | import torch 8 | import warnings 9 | 10 | import configuration as cfg 11 | 12 | from . import anatomy 13 | from . import routines_math as rout_m 14 | 15 | 16 | def get_calibration(file_origin_coord, file_calibration, 17 | scale_factor): 18 | calibration = np.load(file_calibration, allow_pickle=True).item() 19 | 20 | # This is mostly for backwards compatibility. Newer calibrations from calibcam 1.2.0 onwards 21 | # should always have a scale factor of 1. 22 | if 'scale_factor' in calibration: 23 | if scale_factor is None: 24 | scale_factor = calibration['scale_factor'] 25 | elif not scale_factor == calibration['scale_factor']: 26 | warnings.warn("!!!!!!! scale_factor in calibration does not match scale_factor in configuration !!!!!!!") 27 | 28 | n_cameras = calibration['nCameras'] 29 | A = calibration['A_fit'] 30 | k = calibration['k_fit'] 31 | rX1 = calibration['rX1_fit'] 32 | RX1 = calibration['RX1_fit'] 33 | tX1 = calibration['tX1_fit'] 34 | 35 | if os.path.isfile(file_origin_coord): 36 | origin_coord_arena = np.load(file_origin_coord, allow_pickle=True).item() 37 | origin_arena = origin_coord_arena['origin'] 38 | coord_arena = origin_coord_arena['coord'] 39 | else: 40 | origin_arena = np.zeros(3, dtype=np.float64) 41 | coord_arena = np.identity(3, dtype=np.float64) 42 | print('WARNING: File that defines the origin does not exist') 43 | 44 | # change reference coordinate system to coordinate system of arena 45 | tX1 = tX1 + np.einsum('nij,j->ni', RX1, origin_arena) 46 | RX1 = np.einsum('nij,jk->nik', RX1, coord_arena) 47 | for i_cam in range(n_cameras): 48 | rX1[i_cam] = rout_m.rotMat2rodrigues_single(RX1[i_cam]) 49 | 50 | # scaling (calibration board square size -> cm) 51 | tX1 = tX1 * scale_factor 52 | 53 | calibration_torch = dict() 54 | # calibration_torch['nCameras'] = int(nCameras) 55 | if len(A.shape) == 2: # Old style calibration 56 | calibration_torch['A_fit'] = torch.from_numpy(A) 57 | else: 58 | calibration_torch['A_fit'] = torch.from_numpy(np.array([ 59 | A[:, 0, 0], 60 | A[:, 0, 2], 61 | A[:, 1, 1], 62 | A[:, 1, 2], 63 | ]).T) 64 | calibration_torch['k_fit'] = torch.from_numpy(k) 65 | calibration_torch['rX1_fit'] = torch.from_numpy(rX1) 66 | calibration_torch['RX1_fit'] = torch.from_numpy(RX1) 67 | calibration_torch['tX1_fit'] = torch.from_numpy(tX1) 68 | calibration_torch['scale_factor'] = scale_factor 69 | return calibration_torch 70 | 71 | 72 | def get_model3d(file_model): 73 | # load 74 | model3d = np.load(file_model, allow_pickle=True).item() 75 | joint_order = model3d['joint_order'] 76 | joint_marker_order = model3d['joint_marker_order'] 77 | 78 | skeleton_edges = model3d['skeleton_edges'] 79 | skeleton_vertices = model3d['skeleton_vertices'] 80 | skeleton_coords = model3d['skeleton_coords'] 81 | joint_order = model3d['joint_order'] 82 | 83 | skel_verts_new = np.zeros_like(model3d['skeleton_vertices'], dtype=np.float64)[None, :, :] 84 | joint_marker_vec_new = np.zeros_like(model3d['joint_marker_vectors'], dtype=np.float64)[None, :, :] 85 | 86 | 87 | nBones = np.shape(skeleton_edges)[0] 88 | nMarkers = np.shape(joint_marker_order)[0] 89 | 90 | joint_marker_vec_index = anatomy.get_joint_marker_vec_index(nMarkers, joint_marker_order) 91 | bone_lengths_index = anatomy.get_bone_lengths_index(nBones, skeleton_edges, joint_order) 92 | bounds, _, _, is_euler = anatomy.get_bounds_pose(nBones, skeleton_edges, joint_order) 93 | I_bone = np.tile(np.identity(3, dtype=np.float64), (nBones, 1, 1)) 94 | 95 | skeleton_coords0 = anatomy.get_skeleton_coords0(skeleton_edges, skeleton_vertices, skeleton_coords, joint_order, bounds) 96 | 97 | # reparameterization 98 | if (cfg.use_reparameterization): 99 | skeleton_vertices_links = model3d['skeleton_vertices_links'] 100 | skeleton_coords_index = model3d['skeleton_coords_index'] 101 | skeleton_coords0 = anatomy.reparameterize_coords0(skeleton_coords0, bounds, 102 | skeleton_vertices_links, skeleton_coords_index, is_euler) 103 | 104 | model3d_torch = dict() 105 | # 106 | model3d_torch['skeleton_vertices'] = torch.from_numpy(model3d['skeleton_vertices']) 107 | model3d_torch['skeleton_edges'] = torch.from_numpy(model3d['skeleton_edges']) 108 | model3d_torch['bone_lengths'] = torch.from_numpy(model3d['bone_lengths']) 109 | model3d_torch['skeleton_vertices_links'] = torch.from_numpy(model3d['skeleton_vertices_links']) 110 | model3d_torch['joint_marker_vectors'] = torch.from_numpy(model3d['joint_marker_vectors']) 111 | model3d_torch['skeleton_coords_index'] = torch.from_numpy(model3d['skeleton_coords_index']) 112 | model3d_torch['joint_marker_index'] = torch.from_numpy(model3d['joint_marker_index']) 113 | model3d_torch['skeleton_coords'] = torch.from_numpy(model3d['skeleton_coords']) 114 | # 115 | model3d_torch['skeleton_coords0'] = torch.from_numpy(skeleton_coords0) 116 | model3d_torch['I_bone'] = torch.from_numpy(I_bone) 117 | model3d_torch['is_euler'] = torch.from_numpy(is_euler) 118 | model3d_torch['skeleton_vertices_new'] = torch.from_numpy(skel_verts_new) 119 | model3d_torch['joint_marker_vectors_new'] = torch.from_numpy(joint_marker_vec_new) 120 | # for plotting 121 | model3d_torch['surface_triangles'] = torch.from_numpy(model3d['surface_triangles']) 122 | model3d_torch['surface_vertices'] = torch.from_numpy(model3d['surface_vertices']) 123 | model3d_torch['surface_vertices_weights'] = torch.from_numpy(model3d['surface_vertices_weights']) 124 | # for calculating other things (i.e. nor needed within model.py) 125 | model3d_torch['joint_order'] = list(joint_order) 126 | model3d_torch['joint_marker_order'] = list(joint_marker_order) 127 | # 128 | model3d_torch['joint_marker_vec_index'] = torch.from_numpy(joint_marker_vec_index) 129 | model3d_torch['bone_lengths_index'] = torch.from_numpy(bone_lengths_index) 130 | return model3d_torch 131 | 132 | 133 | def get_labelsDLC(file_labelsDLC, pcutoff, 134 | joint_marker_order, nCameras, nMarkers): 135 | # load labels 136 | labelsDLC = np.load(file_labelsDLC, allow_pickle=True).item() 137 | nFrames = np.size(labelsDLC['frame_list'], 0) 138 | labels_mask = (labelsDLC['labels_all'][:, :, :, 2] > pcutoff) 139 | labels = np.zeros((nFrames, nCameras, nMarkers, 3), 140 | dtype=np.float64) 141 | labels[labels_mask] = labelsDLC['labels_all'][labels_mask] # shoule be possible to just copy all respective label values 142 | return labels, labels_mask 143 | 144 | 145 | def get_arguments(file_origin_coord, file_calibration, file_model, file_labelsDLC, 146 | scale_factor, pcutoff=0.9): 147 | # calibration 148 | calibration = get_calibration(file_origin_coord, file_calibration, 149 | scale_factor) 150 | nCameras = int(np.size(calibration['A_fit'], 0)) 151 | # model3d 152 | model3d = get_model3d(file_model) 153 | joint_order = model3d['joint_order'] # list 154 | joint_marker_order = model3d['joint_marker_order'] # list 155 | skeleton_edges = model3d['skeleton_edges'].cpu().numpy() 156 | joint_marker_vectors = model3d['joint_marker_vectors'].cpu().numpy() 157 | joint_marker_index = model3d['joint_marker_index'].cpu().numpy() 158 | 159 | # numbers 160 | nBones = int(np.size(skeleton_edges, 0)) 161 | # nJoints = int(np.size(model3d['skeleton_vertices'], 0)) 162 | nMarkers = int(np.size(joint_marker_vectors, 0)) 163 | numbers = dict() 164 | numbers['nCameras'] = int(nCameras) 165 | numbers['nBones'] = int(nBones) 166 | # numbers['nJoints'] = int(nJoints) 167 | numbers['nMarkers'] = int(nMarkers) 168 | # numbers['nLabels'] = int(nLabels) 169 | # numbers['nFrames'] = int(nFrames) 170 | 171 | # labels 172 | labels, labels_mask = \ 173 | get_labelsDLC(file_labelsDLC, pcutoff, 174 | joint_marker_order, nCameras, nMarkers) 175 | # nLabels = int(np.size(labelsDLC['labels_all'], 2)) 176 | # nFrames = (np.size(labelsDLC['frame_list'], 0)) 177 | 178 | 179 | 180 | # generate weights for objective function 181 | weights = np.ones(nMarkers, dtype=np.float64) 182 | # for i_joint in np.unique(joint_marker_index): 183 | # mask = (joint_marker_index == i_joint) 184 | # weights[mask] = 1.0 / np.sum(mask, dtype=np.float64) 185 | 186 | # bounds & free parameters 187 | bounds_bones, bounds_free_bones, free_para_bones = anatomy.get_bounds_bones(nBones, skeleton_edges, joint_order) 188 | bounds_markers, bounds_free_markers, free_para_markers = anatomy.get_bounds_markers(nMarkers, joint_marker_order) 189 | bounds_pose, bounds_free_pose, free_para_pose, is_euler = anatomy.get_bounds_pose(nBones, skeleton_edges, joint_order) 190 | # reparameterization 191 | if (cfg.use_reparameterization): 192 | bounds_pose, bounds_free_pose = anatomy.reparameterize_bounds(bounds_pose, nBones, free_para_pose) 193 | 194 | # mean/range of pose parameters 195 | bounds_free_pose_range = (bounds_free_pose[:, 1] - bounds_free_pose[:, 0]) / 2.0 196 | # to avoid warnings: 197 | bounds_free_pose_0 = np.zeros_like(bounds_free_pose[:, 0], dtype=np.float64) 198 | mask_inf = np.isinf(bounds_free_pose_range) 199 | bounds_free_pose_0[mask_inf] = 0.0 200 | bounds_free_pose_0[~mask_inf] = bounds_free_pose[~mask_inf, 0] + bounds_free_pose_range[~mask_inf] 201 | 202 | # initialize args 203 | args = dict() 204 | # numbers 205 | args['numbers'] = numbers 206 | # calibration 207 | args['calibration'] = calibration 208 | # model3d 209 | args['model'] = model3d 210 | # bounds 211 | args['free_para_bones'] = torch.from_numpy(free_para_bones) 212 | args['bounds_bones'] = torch.from_numpy(bounds_bones) 213 | args['bounds_free_bones'] = torch.from_numpy(bounds_free_bones) 214 | args['nPara_bones'] = int(np.size(free_para_bones)) 215 | args['nFree_bones'] = int(np.sum(free_para_bones)) 216 | # 217 | args['free_para_markers'] = torch.from_numpy(free_para_markers) 218 | args['bounds_markers'] = torch.from_numpy(bounds_markers) 219 | args['bounds_free_markers'] = torch.from_numpy(bounds_free_markers) 220 | args['nPara_markers'] = int(np.size(free_para_markers)) 221 | args['nFree_markers'] = int(np.sum(free_para_markers)) 222 | # 223 | args['free_para_pose'] = torch.from_numpy(free_para_pose) 224 | args['bounds_pose'] = torch.from_numpy(bounds_pose) 225 | args['nPara_pose'] = int(np.size(free_para_pose)) 226 | args['nFree_pose'] = int(np.sum(free_para_pose)) 227 | # 228 | args['bounds_free_pose'] = torch.from_numpy(bounds_free_pose) 229 | args['bounds_free_pose_range'] = torch.from_numpy(bounds_free_pose_range) 230 | args['bounds_free_pose_0'] = torch.from_numpy(bounds_free_pose_0) 231 | args['is_euler'] = torch.from_numpy(is_euler) 232 | # weights 233 | args['weights'] = torch.from_numpy(weights) 234 | # labels 235 | args['labels'] = torch.from_numpy(labels) 236 | args['labels_mask'] = torch.from_numpy(labels_mask) 237 | # # ATTENTION: labels_batch and labels_mask_batch need to be populated with their respective entries later on! 238 | # # ATTENTION: Do not initialize the arrays with np.nan since that can lead to errors in the gradient calculation in pytorch (even when using masks) 239 | # labels_batch = np.zeros((nBatch, nCameras, nMarkers, 3), dtype=np.float64) 240 | # labels_mask_batch = np.zeros((nBatch, nCameras, nMarkers), dtype=bool) 241 | return args 242 | 243 | # put this into the functions in optimization.py 244 | def update_args(x_torch, i_frame, args): 245 | free_para = args['free_para'] 246 | 247 | args['x_torch'] = x_torch[None, :].clone() 248 | # args['x_torch'].requires_grad = True 249 | args['x_free_torch'] = x_torch[free_para][None, :].clone() 250 | args['x_free_torch'].requires_grad = True 251 | args['labels_single_torch'] = args['labels'][i_frame].clone() 252 | args['labels_mask_single_torch'] = args['labels_mask'][i_frame].clone() 253 | return 254 | -------------------------------------------------------------------------------- /ACM/initialization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | import sys 5 | import torch 6 | 7 | import configuration as cfg 8 | 9 | from . import calibration as calib 10 | from . import helper 11 | from . import model 12 | from . import optimization as opt 13 | 14 | def main(): 15 | # get arguments 16 | args = helper.get_arguments(cfg.file_origin_coord, cfg.file_calibration, cfg.file_model, cfg.file_labelsDLC, 17 | cfg.scale_factor, cfg.pcutoff) 18 | args['use_custom_clip'] = False 19 | 20 | # get relevant information from arguments 21 | nBones = args['numbers']['nBones'] 22 | nMarkers = args['numbers']['nMarkers'] 23 | nCameras = args['numbers']['nCameras'] 24 | joint_order = args['model']['joint_order'] # list 25 | joint_marker_order = args['model']['joint_marker_order'] # list 26 | skeleton_edges = args['model']['skeleton_edges'].cpu().numpy() 27 | bone_lengths_index = args['model']['bone_lengths_index'].cpu().numpy() 28 | joint_marker_index = args['model']['joint_marker_index'].cpu().numpy() 29 | # 30 | free_para_bones = args['free_para_bones'].cpu().numpy() 31 | free_para_markers = args['free_para_markers'].cpu().numpy() 32 | free_para_pose = args['free_para_pose'].cpu().numpy() 33 | nPara_bones = args['nPara_bones'] 34 | nPara_markers = args['nPara_markers'] 35 | nPara_pose = args['nPara_pose'] 36 | nFree_pose = args['nFree_pose'] 37 | 38 | # remove all free parameters that do not modify the pose 39 | free_para_bones = np.zeros_like(free_para_bones, dtype=bool) 40 | free_para_markers = np.zeros_like(free_para_markers, dtype=bool) 41 | nFree_bones = int(0) 42 | nFree_markers = int(0) 43 | args['free_para_bones'] = torch.from_numpy(free_para_bones) 44 | args['free_para_markers'] = torch.from_numpy(free_para_markers) 45 | args['nFree_bones'] = nFree_bones 46 | args['nFree_markers'] = nFree_markers 47 | 48 | # get index of initialization frame 49 | labelsDLC = np.load(cfg.file_labelsDLC, allow_pickle=True).item() 50 | frame_list = labelsDLC['frame_list'] 51 | labels_dlc = labelsDLC['labels_all'] 52 | 53 | # initialize x_pose 54 | # load calibrated model and initalize the pose 55 | x_calib = np.load(cfg.folder_calib + '/x_calib.npy', allow_pickle=True) 56 | x_bones = x_calib[:nPara_bones] 57 | x_markers = x_calib[nPara_bones:nPara_bones+nPara_markers] 58 | # load arena coordinate system 59 | origin, coord = calib.get_origin_coord(cfg.file_origin_coord, cfg.scale_factor) 60 | # 61 | i_frame = np.where(frame_list == cfg.index_frame_ini)[0][0] 62 | labels_frame = np.copy(labels_dlc[i_frame]) 63 | labels_frame_mask = (labels_frame[:, :, 2] < cfg.pcutoff) 64 | labels_frame[labels_frame_mask] = 0.0 65 | x_pose = calib.initialize_x(args, 66 | labels_frame, 67 | coord, origin) 68 | # 69 | x = np.concatenate([x_bones, 70 | x_markers, 71 | x_pose], 0) 72 | # create correct free_para 73 | free_para = np.concatenate([free_para_bones, 74 | free_para_markers, 75 | free_para_pose], 0) 76 | 77 | # update args regarding fixed tensors 78 | args['plot'] = False 79 | args['nFrames'] = int(1) 80 | 81 | # update args regarding x0 and labels 82 | # ARGS X 83 | args['x_torch'] = torch.from_numpy(np.concatenate([x_bones, 84 | x_markers, 85 | x_pose], 0)) 86 | args['x_free_torch'] = torch.from_numpy(x_pose[free_para_pose]) 87 | args['x_free_torch'].requires_grad = True 88 | # ARGS LABELS DLC 89 | labels_dlc = np.load(cfg.file_labelsDLC, allow_pickle=True).item() 90 | i_frame_single = np.where(cfg.index_frame_ini == labels_dlc['frame_list'])[0][0] 91 | args['labels_single_torch'] = args['labels'][i_frame_single][None, :].clone() 92 | args['labels_mask_single_torch'] = args['labels_mask'][i_frame_single][None, :].clone() 93 | 94 | # BOUNDS 95 | # pose 96 | if ((cfg.mode == 1) or (cfg.mode == 2)): 97 | bounds_free_pose = args['bounds_free_pose'] 98 | bounds_free_low_pose = bounds_free_pose[:, 0] 99 | bounds_free_high_pose = bounds_free_pose[:, 1] 100 | elif ((cfg.mode == 3) or (cfg.mode == 4)): 101 | # so that pose-encoding parameters do not get initialized with basically infinity when EM algorithm is used 102 | mu_ini_fac = 0.9 103 | bounds_free_low_pose = args['bounds_free_pose_0'] - args['bounds_free_pose_range'] * mu_ini_fac 104 | bounds_free_high_pose = args['bounds_free_pose_0'] + args['bounds_free_pose_range'] * mu_ini_fac 105 | bounds_free_low_pose = model.do_normalization(bounds_free_low_pose[None, :], args).numpy().ravel() 106 | bounds_free_high_pose = model.do_normalization(bounds_free_high_pose[None, :], args).numpy().ravel() 107 | bounds_free = np.stack([bounds_free_low_pose, bounds_free_high_pose], 1) 108 | args['bounds_free'] = bounds_free 109 | 110 | # normalize x 111 | x_free = model.do_normalization(torch.from_numpy(x_pose[free_para_pose].reshape(1, nFree_pose)), args).numpy().ravel() 112 | 113 | # OPTIMIZE 114 | # create optimization dictonary 115 | opt_options = dict() 116 | opt_options['disp'] = cfg.opt_options_ini__disp 117 | opt_options['maxiter'] = cfg.opt_options_ini__maxiter 118 | opt_options['maxcor'] = cfg.opt_options_ini__maxcor 119 | opt_options['ftol'] = cfg.opt_options_ini__ftol 120 | opt_options['gtol'] = cfg.opt_options_ini__gtol 121 | opt_options['maxfun'] = cfg.opt_options_ini__maxfun 122 | opt_options['iprint'] = cfg.opt_options_ini__iprint 123 | opt_options['maxls'] = cfg.opt_options_ini__maxls 124 | opt_dict = dict() 125 | opt_dict['opt_method'] = cfg.opt_method_ini 126 | opt_dict['opt_options'] = opt_options 127 | print('Initializing') 128 | min_result = opt.optimize__scipy(x_free, args, 129 | opt_dict) 130 | print('Finished initializing') 131 | print() 132 | 133 | # copy fitting result into correct arrary 134 | x_fit_free = np.copy(min_result.x) 135 | 136 | # reverse normalization of x 137 | x_fit_free = model.undo_normalization(torch.from_numpy(x_fit_free).reshape(1, nFree_pose), args).numpy().ravel() 138 | 139 | # add free variables 140 | x_ini = np.copy(x) 141 | x_ini[free_para] = x_fit_free 142 | 143 | # save 144 | np.save(cfg.folder_init + '/x_ini.npy', x_ini) 145 | 146 | if __name__ == "__main__": 147 | main() 148 | -------------------------------------------------------------------------------- /ACM/interp_3d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.optimize import minimize 3 | 4 | from . import routines_math as rout_m 5 | 6 | def find_closest_3d_point(m, n): 7 | def obj_func(x): 8 | estimated_points = x[:-3, None] * m + n 9 | res = 0.5 * np.sum((estimated_points - x[None, -3:])**2) 10 | 11 | jac = np.zeros(len(x), dtype=np.float64) 12 | jac[:-3] = np.sum(m**2, 1) * x[:-3] + \ 13 | np.sum(m * n, 1) - \ 14 | np.sum(m * x[None, -3:], 1) 15 | jac[-3:] = (np.float64(len(x)) - 3.0) * x[-3:] - np.sum(estimated_points, 0) 16 | return res, jac 17 | 18 | nPoints = np.size(m, 0) 19 | x0 = np.zeros(nPoints + 3) 20 | tolerance = np.finfo(np.float32).eps 21 | min_result = minimize(obj_func, 22 | x0, 23 | method='l-bfgs-b', 24 | jac=True, 25 | tol=tolerance, 26 | options={'disp':False, 27 | 'maxcor':20, 28 | 'maxfun':15000, 29 | 'maxiter':15000, 30 | 'maxls':40}) 31 | if not(min_result.success): 32 | print('WARNING: 3D point interpolation did not converge') 33 | print('\tnPoints\t', nPoints) 34 | print('\tsuccess:\t', min_result.success) 35 | print('\tstatus:\t', min_result.status) 36 | print('\tmessage:\t',min_result.message) 37 | print('\tnit:\t', min_result.nit) 38 | return min_result.x 39 | 40 | # look at: Rational Radial Distortion Models with Analytical Undistortion Formulae, Lili Ma et al. 41 | # source: https://arxiv.org/pdf/cs/0307047.pdf 42 | # only works for k = [k1, k2, 0, 0, 0] 43 | def calc_udst(m_dst, k): 44 | assert np.all(k[2:] == 0.0), 'ERROR: Undistortion only valid for up to two radial distortion coefficients.' 45 | x_2 = m_dst[:, 0] 46 | y_2 = m_dst[:, 1] 47 | # use r directly instead of c 48 | nPoints = np.size(m_dst, 0) 49 | p = np.zeros(6, dtype=np.float64) 50 | p[4] = 1.0 51 | sol = np.zeros(3, dtype=np.float64) 52 | x_1 = np.zeros(nPoints, dtype=np.float64) 53 | y_1 = np.zeros(nPoints, dtype=np.float64) 54 | for i_point in range(nPoints): 55 | cond = (np.abs(x_2[i_point]) > np.abs(y_2[i_point])) 56 | if (cond): 57 | c = y_2[i_point] / x_2[i_point] 58 | p[5] = -x_2[i_point] 59 | else: 60 | c = x_2[i_point] / y_2[i_point] 61 | p[5] = -y_2[i_point] 62 | p[2] = k[0] * (1 + c**2) 63 | p[0] = k[1] * (1 + c**2)**2 64 | sol = np.real(np.roots(p)) 65 | sol_abs = np.abs(sol) 66 | if (cond): 67 | x_1[i_point] = sol[sol_abs == np.min(sol_abs)][0] 68 | y_1[i_point] = c * x_1[i_point] 69 | else: 70 | y_1[i_point] = sol[sol_abs == np.min(sol_abs)][0] 71 | x_1[i_point] = c * y_1[i_point] 72 | m_udst = np.concatenate([[x_1], [y_1], [m_dst[:, 2]]], 0).T 73 | return m_udst 74 | 75 | def calc_3d_point(points_2d, A, k, rX1, tX1): 76 | mask_nan = ~np.any(np.isnan(points_2d), 1) 77 | mask_zero = ~np.any((points_2d == 0.0), 1) 78 | mask = np.logical_and(mask_nan, mask_zero) 79 | nValidPoints = np.sum(mask) 80 | if (nValidPoints < 2): 81 | print("WARNING: Less than 2 valid 2D locations for 3D point interpolation.") 82 | n = np.zeros((nValidPoints, 3)) 83 | m = np.zeros((nValidPoints, 3)) 84 | nCameras = np.size(points_2d, 0) 85 | 86 | index = 0 87 | for i_cam in range(nCameras): 88 | if (mask[i_cam]): 89 | point = np.array([[(points_2d[i_cam, 0] - A[i_cam][0, 2]) / A[i_cam][0, 0], 90 | (points_2d[i_cam, 1] - A[i_cam][1, 2]) / A[i_cam][1, 1], 91 | 1.0]], dtype=np.float64) 92 | point = calc_udst(point, k[i_cam]).T 93 | line = point * np.linspace(0, 1, 2) 94 | RX1 = rout_m.rodrigues2rotMat_single(rX1[i_cam]) 95 | line = np.dot(RX1.T, line - tX1[i_cam].reshape(3, 1)) 96 | n[index] = line[:, 0] 97 | m[index] = line[:, 1] - line[:, 0] 98 | index = index + 1 99 | x = find_closest_3d_point(m, n) 100 | return x[-3:] 101 | -------------------------------------------------------------------------------- /ACM/kalman.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import torch 4 | 5 | from . import model 6 | 7 | def addition_save(term1, term2): 8 | return term1 + term2 9 | 10 | def substraction_save(term1, term2): 11 | return term1 - term2 12 | 13 | def cholesky_save(M_in): 14 | len_MShape = len(M_in.size()) 15 | if (len_MShape == 2): # no batch 16 | try: 17 | L = torch.cholesky(0.5 * (M_in + M_in.transpose(1, 0))) 18 | except: 19 | eps = torch.diag_embed(torch.ones_like(M_in[0], dtype=model.float_type)) 20 | expo = -52 21 | cholesky_is_invalid = True 22 | while (cholesky_is_invalid): 23 | try: 24 | L = torch.cholesky(0.5 * (M_in + M_in.transpose(1, 0)) + eps * 2.0**expo) 25 | cholesky_is_invalid = False 26 | except: 27 | expo += 1 28 | print('WARNING: cholesky_save (no batch) expo:\t{:03d}'.format(int(expo))) 29 | elif (len_MShape == 3): # batch 30 | try: 31 | L = torch.cholesky(0.5 * (M_in + M_in.transpose(2, 1))) 32 | except: 33 | eps = torch.diag_embed(torch.ones_like(M_in[0, 0], dtype=model.float_type)) 34 | expo = -52 35 | cholesky_is_invalid = True 36 | while (cholesky_is_invalid): 37 | try: 38 | L = torch.cholesky(0.5 * (M_in + M_in.transpose(2, 1)) + eps[None, :, :] * 2.0**expo) 39 | cholesky_is_invalid = False 40 | except: 41 | expo += 1 42 | print('WARNING: cholesky_save (batch) expo:\t{:03d}'.format(int(expo))) 43 | else: 44 | print('ERROR: Invalid array dimension in kalman.cholesky_save') 45 | return L 46 | 47 | 48 | def calc_sigma_points_rand(m, w, L, 49 | n): 50 | distribution = torch.distributions.MultivariateNormal(loc=torch.zeros_like(m, dtype=model.float_type), 51 | scale_tril=L) 52 | 53 | len_mShape = len(m.size()) 54 | nSamples = int((n-1)/2) 55 | if (len_mShape == 1): 56 | samples = distribution.sample((nSamples,)) 57 | sigma_points_rand = torch.cat([m[None, :], 58 | addition_save(m[None, :], samples), 59 | addition_save(m[None, :], -samples)], 0) 60 | elif (len_mShape == 2): 61 | samples = distribution.sample((nSamples,)).permute(1, 0, 2) 62 | sigma_points_rand = torch.cat([m[:, None, :], 63 | kalma.addition_save(m[:, None, :], samples), 64 | kalma.addition_save(m[:, None, :], -samples)], 1) 65 | else: 66 | print('ERROR: Invalid array dimension in kalman.calc_sigma_points_rand') 67 | raise 68 | return sigma_points_rand 69 | 70 | def calc_sigma_points_p3(m, w, L): 71 | len_mShape = len(m.size()) 72 | if (len_mShape == 1): 73 | w_times_L_T = w * L.transpose(1, 0) 74 | sigma_points_p3 = torch.cat([m[None, :], 75 | addition_save(m[None, :], w_times_L_T), 76 | addition_save(m[None, :], -w_times_L_T)], 0) 77 | elif (len_mShape == 2): 78 | w_times_L_T = w * L.transpose(2, 1) 79 | sigma_points_p3 = torch.cat([m[:, None, :], 80 | addition_save(m[:, None, :], w_times_L_T), 81 | addition_save(m[:, None, :], -w_times_L_T)], 1) 82 | else: 83 | print('ERROR: Invalid array dimension in kalman.calc_sigma_points_p3') 84 | raise 85 | return sigma_points_p3 86 | 87 | def calc_sigma_points_p5(m, w, L): 88 | # 3 89 | sigma_points_p3 = calc_sigma_points_p3(m, w, L) 90 | 91 | # 5 92 | mShape = list(m.size()) 93 | if (len(mShape) == 1): 94 | dim_z = mShape[0] 95 | dim0 = (dim_z*(dim_z-1)) 96 | 97 | L_T = L.transpose(1, 0) 98 | 99 | # can be calculated outside already 100 | L5_mask = torch.ones((dim_z, dim_z, dim_z), dtype=torch.bool).cuda() 101 | L5_mask_index = torch.arange(dim_z, dtype=torch.int64).cuda() 102 | L5_mask[L5_mask_index, L5_mask_index, :] = 0 103 | 104 | L5_1 = addition_save(L_T[None, :, :], L_T[:, None, :])[L5_mask].reshape(dim0, dim_z) 105 | L5_2 = addition_save(L_T[None, :, :], -L_T[:, None, :])[L5_mask].reshape(dim0, dim_z) 106 | L5_3 = addition_save(-L_T[None, :, :], L_T[:, None, :])[L5_mask].reshape(dim0, dim_z) 107 | L5_4 = addition_save(-L_T[None, :, :], -L_T[:, None, :])[L5_mask].reshape(dim0, dim_z) 108 | 109 | L5 = torch.unique(torch.cat([L5_1, L5_2, L5_3, L5_4], 0).float(), 110 | dim=0).double() 111 | 112 | sigma_points_p5 = torch.cat([sigma_points_p3, 113 | addition_save(m[None, :], w * L5)], 0) 114 | elif (len(mShape) == 2): 115 | nT = mShape[0] 116 | dim_z = mShape[1] 117 | 118 | L_T = L.transpose(2, 1) 119 | 120 | L1 = L_T.repeat(dim_z, 1, 1, 1) 121 | L1 = L1.reshape(dim_z, nT, dim_z, dim_z) 122 | L1 = L1.transpose(1, 0) 123 | L2 = L_T.repeat(1, 1, 1, dim_z) 124 | L2 = L2.reshape(nT, dim_z, dim_z, dim_z) 125 | 126 | L5_1 = addition_save(L1, L2) 127 | L5_2 = addition_save(L1, -L2) 128 | L5_3 = addition_save(-L1, L2) 129 | L5_4 = addition_save(-L1, -L2) 130 | 131 | # can be calculated outside already 132 | L5_mask = torch.ones_like(L1[0], dtype=torch.bool) 133 | L5_mask_mask = torch.ones_like(L_T[0, 0], dtype=torch.bool) 134 | L5_mask_mask = torch.diag_embed(L5_mask_mask) 135 | L5_mask_mask = L5_mask_mask.repeat(1, 1, dim_z) 136 | L5_mask_mask = L5_mask_mask.reshape(dim_z, dim_z, dim_z) 137 | L5_mask_mask = L5_mask_mask.transpose(2, 1) 138 | L5_mask[L5_mask_mask] = 0 139 | L5_mask = L5_mask.repeat(nT, 1, 1, 1) 140 | L5_mask = L5_mask.reshape(nT, dim_z, dim_z, dim_z) 141 | 142 | dim0 = (dim_z*(dim_z-1)) 143 | 144 | L5_1 = L5_1[L5_mask].reshape(nT, dim0, dim_z) 145 | L5_2 = L5_2[L5_mask].reshape(nT, dim0, dim_z) 146 | L5_3 = L5_3[L5_mask].reshape(nT, dim0, dim_z) 147 | L5_4 = L5_4[L5_mask].reshape(nT, dim0, dim_z) 148 | L5 = torch.cat([L5_1, L5_2, L5_3, L5_4], 1) 149 | 150 | L5 = torch.unique(L5.float(), dim=1).double() 151 | 152 | nSigmaPointsAdd = 2*dim_z*(dim_z-1) 153 | m_use = m.repeat(1, 1, nSigmaPointsAdd) 154 | m_use = m_use.reshape(nT, nSigmaPointsAdd, dim_z) 155 | 156 | sigma_points_p5 = torch.cat([sigma_points_p3, 157 | m_use + w * L5], 1) 158 | else: 159 | print('ERROR: Invalid array dimension in kalman.calc_sigma_points_p5') 160 | raise 161 | return sigma_points_p5 162 | 163 | def uks(mu0, var0, A, var_f, var_g, 164 | args): 165 | nT = args['args_kalman']['nT'] 166 | G_kalman = args['args_kalman']['G_kalman'] 167 | 168 | mu_kalman, var_kalman = ukf(mu0, var0, A, var_f, var_g, 169 | args) 170 | 171 | # should not be necessary since arrays will be overwritten anyway 172 | G_kalman.data.zero_() 173 | 174 | for t in range(nT-1, -1, -1): 175 | uks_step(mu_kalman[t], var_kalman[t], G_kalman[t], 176 | mu_kalman[t+1], var_kalman[t+1], 177 | A, var_f, var_g, 178 | args) 179 | return mu_kalman, var_kalman, G_kalman 180 | 181 | # 2013__Saerkkae__Bayesian_Filtering_And_Smoothing 182 | # page 148f., algorithm 9.3 183 | def uks_step(mu_ukf_t, var_ukf_t, G_uks, 184 | mu_uks_tp1, var_uks_tp1, 185 | A, var_f, var_g, 186 | args): 187 | dim_z = args['args_kalman']['dim_z'] 188 | sigma_point_scheme = args['args_kalman']['sigma_point_scheme'] 189 | w_m = args['args_kalman']['w_m'] 190 | w_c = args['args_kalman']['w_c'] 191 | sqrt_dimZ_p_lamb = args['args_kalman']['sqrt_dimZ_p_lamb'] 192 | args_model = args['args_model'] 193 | 194 | kalman_sigmaPoints = args['args_kalman']['sigmaPoints_kalman'] 195 | var_2 = args['args_kalman']['var_2_uks'] 196 | var_31 = args['args_kalman']['var_31_uks'] 197 | var_32 = args['args_kalman']['var_32_uks'] 198 | var_33 = args['args_kalman']['var_33_uks'] 199 | 200 | # should not be necessary since arrays will be overwritten anyway 201 | var_2.data.zero_() 202 | var_31.data.zero_() 203 | var_32.data.zero_() 204 | var_33.data.zero_() 205 | 206 | # variable shapes: 207 | # var_1: nSigmaPoints, dim_z 208 | # var_2: nSigmaPoints, dim_z 209 | # var_31: dim_z 210 | # var_32: dim_z, dim_z 211 | # var_33: dim_z, dim_z 212 | # var_41: dim_z, dim_z (G) 213 | # var_42: dim_z (mu) 214 | # var_43: dim_z, dim_z (var) 215 | 216 | # 1. 217 | if (sigma_point_scheme == 3): 218 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p3(mu_ukf_t, sqrt_dimZ_p_lamb, cholesky_save(var_ukf_t)).data) 219 | elif (sigma_point_scheme == 5): 220 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p5(mu_ukf_t, sqrt_dimZ_p_lamb, cholesky_save(var_ukf_t)).data) 221 | elif (sigma_point_scheme == 0): 222 | kalman_sigmaPoints.data.copy_(calc_sigma_points_rand(mu_ukf_t, sqrt_dimZ_p_lamb, cholesky_save(var_ukf_t), 223 | args['args_kalman']['nSigmaPoints']).data) 224 | 225 | # 2. 226 | var_2.data.copy_(model.fcn_transition_free(kalman_sigmaPoints, A, args_model).data) 227 | 228 | # 3.1 229 | var_31.data.copy_(torch.sum(w_m[:, None] * var_2, 0).data) 230 | 231 | # var_2 is treated as dummy to save intermediate calculations 232 | var_2.data.copy_(substraction_save(var_2, var_31).data) 233 | 234 | # 3.2 235 | var_32.data.copy_(addition_save(torch.sum(w_c[:, None, None] * \ 236 | torch.einsum('ni,nj->nij', (var_2, var_2)), 0), 237 | var_f).data) 238 | var_32.data.copy_((0.5 * (var_32 + var_32.transpose(1, 0))).data) 239 | 240 | # 3.3 [cross-covariance -> not symmetric] 241 | var_33.data.copy_(torch.sum(w_c[:, None, None] * \ 242 | torch.einsum('ni,nj->nij', 243 | (substraction_save(kalman_sigmaPoints, mu_ukf_t), 244 | var_2)), 0).data) 245 | 246 | # 4.1 247 | G_uks.data.copy_(torch.mm(var_33, torch.inverse(var_32)).data) 248 | 249 | # 4.2 250 | mu_ukf_t.data.add_(torch.mv(G_uks, 251 | substraction_save(mu_uks_tp1, var_31)).data) 252 | 253 | # 4.3 [P + G (P_s - P_m) G_T = P + (G P_s - D) G_T since G = D P_m^-1] 254 | var_ukf_t.data.add_(torch.mm(substraction_save(torch.mm(G_uks, var_uks_tp1), var_33), 255 | G_uks.transpose(1, 0)).data) 256 | var_ukf_t.data.copy_((0.5 * (var_ukf_t + var_ukf_t.transpose(1, 0))).data) 257 | return 258 | 259 | def ukf(mu0, var0, A, var_f, var_g, 260 | args): 261 | nT = args['args_kalman']['nT'] 262 | dim_z = args['args_kalman']['dim_z'] 263 | measure = args['args_kalman']['measure'] 264 | measure_mask = args['args_kalman']['measure_mask'] 265 | measure_mask_exclude = args['args_kalman']['measure_mask_exclude'] 266 | 267 | mu_kalman = args['args_kalman']['mu_kalman'] 268 | var_kalman = args['args_kalman']['var_kalman'] 269 | 270 | # should not be necessary since arrays will be overwritten anyway 271 | mu_kalman.data.zero_() 272 | var_kalman.data.zero_() 273 | 274 | mu_kalman[0].data.copy_(mu0.data) 275 | var_kalman[0].data.copy_(var0.data) 276 | for t in range(nT): 277 | ukf_step(mu_kalman[t], var_kalman[t], 278 | measure[t], 279 | measure_mask[t], 280 | measure_mask_exclude, 281 | A, var_f, var_g, 282 | args, 283 | mu_kalman[t+1], var_kalman[t+1]) 284 | return mu_kalman, var_kalman 285 | 286 | # 2013__Saerkkae__Bayesian_Filtering_And_Smoothing 287 | # page 86f., algorithm 5.14 288 | # 2016__Shumway__Time_Series_Analysis_and_its_Applications 289 | # page 310f., 6.4. Missing Data Modifications 290 | def ukf_step(mu_t, var_t, 291 | x_t, 292 | x_t_mask, 293 | x_t_mask_exclude, 294 | A, var_f, var_g, 295 | args, 296 | mu_ukf, var_ukf): 297 | dim_z = args['args_kalman']['dim_z'] 298 | sigma_point_scheme = args['args_kalman']['sigma_point_scheme'] 299 | w_m = args['args_kalman']['w_m'] 300 | w_c = args['args_kalman']['w_c'] 301 | sqrt_dimZ_p_lamb = args['args_kalman']['sqrt_dimZ_p_lamb'] 302 | args_model = args['args_model'] 303 | 304 | kalman_sigmaPoints = args['args_kalman']['sigmaPoints_kalman'] 305 | var_p2 = args['args_kalman']['var_p2_ukf'] 306 | var_p31 = args['args_kalman']['var_p31_ukf'] 307 | var_p32 = args['args_kalman']['var_p32_ukf'] 308 | var_u2 = args['args_kalman']['var_u2_ukf'] 309 | var_u31 = args['args_kalman']['var_u31_ukf'] 310 | var_u32 = args['args_kalman']['var_u32_ukf'] 311 | var_u33 = args['args_kalman']['var_u33_ukf'] 312 | var_u41 = args['args_kalman']['var_u41_ukf'] 313 | 314 | # should not be necessary since arrays will be overwritten anyway 315 | var_p2.data.zero_() 316 | var_p31.data.zero_() 317 | var_p32.data.zero_() 318 | var_u2.data.zero_() 319 | var_u31.data.zero_() 320 | var_u32.data.zero_() 321 | var_u33.data.zero_() 322 | var_u41.data.zero_() 323 | 324 | # variable shapes: 325 | # var_p1: nSigmaPoints, dim_z 326 | # var_p2: nSigmaPoints, dim_z 327 | # var_p31: dim_z 328 | # var_p32: dim_z, dim_z 329 | # var_u1: nSigmaPoints, dim_z 330 | # var_u2: nSigmaPoints, dim_x 331 | # var_u31: dim_x 332 | # var_u32: dim_x, dim_x 333 | # var_u33: dim_z, dim_x 334 | # var_u41: dim_z, dim_x 335 | # var_u42: dim_z (mu) 336 | # var_u43: dim_z, dim_z (var) 337 | 338 | # PREDICT 339 | # p.1 340 | if (sigma_point_scheme == 3): 341 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p3(mu_t, sqrt_dimZ_p_lamb, cholesky_save(var_t)).data) 342 | elif (sigma_point_scheme == 5): 343 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p5(mu_t, sqrt_dimZ_p_lamb, cholesky_save(var_t)).data) 344 | elif (sigma_point_scheme == 0): 345 | kalman_sigmaPoints.data.copy_(calc_sigma_points_rand(mu_t, sqrt_dimZ_p_lamb, cholesky_save(var_t), 346 | args['args_kalman']['nSigmaPoints']).data) 347 | 348 | # p.2 349 | var_p2.data.copy_(model.fcn_transition_free(kalman_sigmaPoints, A, args_model).data) 350 | 351 | # p.3.1 352 | var_p31.data.copy_(torch.sum(w_m[:, None] * var_p2, 0).data) 353 | 354 | # var_p2 is treated as dummy to save intermediate calculations 355 | var_p2.data.copy_(substraction_save(var_p2, var_p31).data) 356 | 357 | # p.3.2 358 | var_p32.data.copy_(addition_save(torch.sum(w_c[:, None, None] * \ 359 | torch.einsum('ni,nj->nij', (var_p2, var_p2)), 0), 360 | var_f).data) 361 | var_p32.data.copy_((0.5 * (var_p32 + var_p32.transpose(1, 0))).data) 362 | 363 | # UPDATE 364 | # u.1 365 | if (sigma_point_scheme == 3): 366 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p3(var_p31, sqrt_dimZ_p_lamb, cholesky_save(var_p32)).data) 367 | elif (sigma_point_scheme == 5): 368 | kalman_sigmaPoints.data.copy_(calc_sigma_points_p5(var_p31, sqrt_dimZ_p_lamb, cholesky_save(var_p32)).data) 369 | elif (sigma_point_scheme == 0): 370 | kalman_sigmaPoints.data.copy_(calc_sigma_points_rand(var_p31, sqrt_dimZ_p_lamb, cholesky_save(var_p32), 371 | args['args_kalman']['nSigmaPoints']).data) 372 | 373 | # u.2 374 | var_u2.data.copy_(model.fcn_emission_free(kalman_sigmaPoints, args_model)[:, x_t_mask_exclude].data) 375 | 376 | # u.3.1 377 | var_u31.data.copy_(torch.sum(w_m[:, None] * var_u2, 0).data) 378 | 379 | # var_u2 is treated as dummy to save intermediate calculations 380 | var_u2.data.copy_(substraction_save(var_u2, var_u31).data) 381 | 382 | # u.3.2 (i.e. S) 383 | var_u32.data.copy_(addition_save(torch.sum(w_c[:, None, None] * \ 384 | torch.einsum('ni,nj->nij', (var_u2, var_u2)), 0), 385 | var_g).data) 386 | var_u32[~x_t_mask, :] = 0.0 # according to 6.24 (sigma used in 6.22) in Shumway et al. (i.e. after substition of A, c.f. 6.77 & 6.78) 387 | var_u32[:, ~x_t_mask] = 0.0 # according to 6.24 (sigma used in 6.22) in Shumway et al. (i.e. after substition of A, c.f. 6.77 & 6.78) 388 | var_u32[~x_t_mask, ~x_t_mask] = 1.0 # according to 6.24 (sigma used in 6.22) in Shumway et al. (i.e. after substition of A, c.f. 6.77 & 6.78) 389 | var_u32.data.copy_((0.5 * (var_u32 + var_u32.transpose(1, 0))).data) 390 | 391 | # u.3.3 (i.e. C) [cross-covariance -> not symmetric] 392 | var_u33.data.copy_(torch.sum(w_c[:, None, None] * \ 393 | torch.einsum('ni,nj->nij', 394 | (substraction_save(kalman_sigmaPoints, var_p31), 395 | var_u2)), 0).data) 396 | var_u33[:, ~x_t_mask] = 0.0 # according to 6.22 (assuming C <=> P^t-1_t A^'_t) in Shumway et al. (i.e. after substition of A, c.f. 6.77 & 6.78) 397 | 398 | # u.4.1 (i.e. K) [Kalman gain] 399 | var_u32.data.copy_(torch.inverse(var_u32).data) 400 | var_u32.data.copy_((0.5 * (var_u32 + var_u32.transpose(1, 0))).data) 401 | var_u41.data.copy_(torch.mm(var_u33, var_u32).data) 402 | 403 | # var_u31 is treated as dummy to save intermediate calculations 404 | var_u31.data.copy_(substraction_save(x_t, var_u31).data) 405 | var_u31[~x_t_mask] = 0.0 # according to 6.23 (epsilon used in 6.20) in Shumway et al. (i.e. after substition of y and A, c.f. 6.77 & 6.78) [use estimated position as measurement when no measurement is available] 406 | 407 | # u.4.2 408 | mu_ukf.data.copy_(addition_save(var_p31, 409 | torch.mv(var_u41, var_u31)).data) 410 | 411 | # u.4.3 [P - K S K^T = P - C K^T = P - K C^T since K = C S^-1 and S = S^T => S^-1 = S^-T] 412 | var_ukf.data.copy_(substraction_save(var_p32, 413 | torch.mm(var_u41, 414 | var_u33.transpose(1, 0))).data) 415 | var_ukf.data.copy_((0.5 * (var_ukf + var_ukf.transpose(1, 0))).data) 416 | return 417 | -------------------------------------------------------------------------------- /ACM/model.py: -------------------------------------------------------------------------------- 1 | import math 2 | import torch 3 | 4 | import configuration as cfg 5 | 6 | float_type = torch.float64 7 | num_tol = cfg.num_tol 8 | 9 | custom_clipping_constant = (0.25 * math.pi)**0.5 * cfg.slope 10 | 11 | def do_custom_clip(z_in): 12 | return torch.erf(custom_clipping_constant * z_in) # [-1.0, 1.0] 13 | 14 | def undo_custom_clip(z_in): 15 | return torch.erfinv(z_in) / custom_clipping_constant # [-inf, inf] 16 | 17 | def do_normalization_r(r, args): 18 | return undo_custom_clip((r - args['bounds_free_pose_0'][None, 6:]) / args['bounds_free_pose_range'][None, 6:]) 19 | 20 | def undo_normalization_r(r_norm, args): 21 | return do_custom_clip(r_norm) * args['bounds_free_pose_range'][None, 6:] + args['bounds_free_pose_0'][None, 6:] 22 | 23 | def do_normalization_r0(r0): 24 | return r0 / cfg.normalize_r0 25 | 26 | def undo_normalization_r0(r0_norm): 27 | return r0_norm * cfg.normalize_r0 28 | 29 | def do_normalization_t0(t0): 30 | return t0 / cfg.normalize_t0 31 | 32 | def undo_normalization_t0(t0_norm): 33 | return t0_norm * cfg.normalize_t0 34 | 35 | def do_normalization(x_free, args_torch): 36 | if (args_torch['use_custom_clip']): 37 | x_free_norm = torch.cat([do_normalization_t0(x_free[:, :3]), 38 | do_normalization_r0(x_free[:, 3:6]), 39 | do_normalization_r(x_free[:, 6:], args_torch)], 1) 40 | else: 41 | x_free_norm = torch.cat([do_normalization_t0(x_free[:, :3]), 42 | do_normalization_r0(x_free[:, 3:])], 1) 43 | return x_free_norm 44 | 45 | def undo_normalization(x_free_norm, args_torch): 46 | if (args_torch['use_custom_clip']): 47 | x_free = torch.cat([undo_normalization_t0(x_free_norm[:, :3]), 48 | undo_normalization_r0(x_free_norm[:, 3:6]), 49 | undo_normalization_r(x_free_norm[:, 6:], args_torch)], 1) 50 | else: 51 | x_free = torch.cat([undo_normalization_t0(x_free_norm[:, :3]), 52 | undo_normalization_r0(x_free_norm[:, 3:])], 1) 53 | return x_free 54 | 55 | def do_normalization_bones(bone_lengths): 56 | return bone_lengths / cfg.normalize_bone_lengths 57 | 58 | def undo_normalization_bones(bone_lengths_norm): 59 | return bone_lengths_norm * cfg.normalize_bone_lengths 60 | 61 | def do_normalization_markers(joint_marker_vec): 62 | return joint_marker_vec / cfg.normalize_joint_marker_vec 63 | 64 | def undo_normalization_markers(joint_marker_vec_norm): 65 | return joint_marker_vec_norm * cfg.normalize_joint_marker_vec 66 | 67 | # implementation according to: 2009__Moll__Ball_Joints_for_Marker-less_Human_Motion_Capture 68 | def rodrigues2rotMat(r): # nSigmaPoints, 3 69 | sqrt_arg = torch.sum(r**2, 1) 70 | theta = torch.sqrt(sqrt_arg) 71 | omega = r / theta[:, None] 72 | zero_entries = torch.zeros_like(theta, dtype=float_type) 73 | omega_hat = torch.stack([torch.stack([zero_entries, -omega[:, 2], omega[:, 1]], 1), 74 | torch.stack([omega[:, 2], zero_entries, -omega[:, 0]], 1), 75 | torch.stack([-omega[:, 1], omega[:, 0], zero_entries], 1)], 1) 76 | 77 | rotMat = torch.diag_embed(torch.ones_like(r, dtype=float_type)) + \ 78 | torch.sin(theta)[:, None, None] * omega_hat + \ 79 | (1.0 - torch.cos(theta))[:, None, None] * torch.einsum('nij,njk->nik', (omega_hat, omega_hat)) 80 | 81 | mask = torch.all(abs(r) <= num_tol, 1) 82 | if torch.any(mask): 83 | rotMat[mask] = torch.diag_embed(torch.ones_like(r[mask], dtype=float_type)) 84 | return rotMat # nSigmaPoints, 3, 3 85 | 86 | def map_m(RX1, tX1, A, k, 87 | M): # nSigmaPoints, nMarkers, 3 88 | # RX1 * m + tX1 89 | m_cam = torch.einsum('cij,mnj->mcni', (RX1, M)) + tX1[None, :, None, :] # nSigmaPoints, nCameras, nMarkers, 3 90 | # m / m[2] 91 | m = m_cam[:, :, :, :2] / m_cam[:, :, :, 2][:, :, :, None] # nSigmaPoints, nCameras, nMarkers, 2 92 | # distort & A * m 93 | r_2 = m[:, :, :, 0]**2 + m[:, :, :, 1]**2 94 | sum_term = 1.0 + \ 95 | k[None, :, None, 0] * r_2 + \ 96 | k[None, :, None, 1] * r_2**2 + \ 97 | k[None, :, None, 4] * r_2**3 98 | x_times_y_times_2 = m[:, :, :, 0] * m[:, :, :, 1] * 2.0 99 | m = torch.stack([A[None, :, None, 1] + A[None, :, None, 0] * \ 100 | (m[:, :, :, 0] * sum_term + \ 101 | k[None, :, None, 2] * x_times_y_times_2 + \ 102 | k[None, :, None, 3] * (r_2 + 2.0 * m[:, :, :, 0]**2)), 103 | A[None, :, None, 3] + A[None, :, None, 2] * \ 104 | (m[:, :, :, 1] * sum_term + \ 105 | k[None, :, None, 2] * (r_2 + 2.0 * m[:, :, :, 1]**2) + \ 106 | k[None, :, None, 3] * x_times_y_times_2)], 3) 107 | return m # nSigmaPoints, nCameras, nMarkers, 2 108 | 109 | # IMPROVEMENT: bone_lengths & joint_marker_vec should have the correct size (i.e. < nBones/nMarkers due to symmetry) 110 | def adjust_joint_marker_pos2(model_torch, 111 | bone_lengths, joint_marker_vec, 112 | model_t0_torch, model_r0_torch, model_r_torch, 113 | nBones, 114 | adjust_surface=False): 115 | skel_verts = model_torch['skeleton_vertices'] 116 | skel_edges = model_torch['skeleton_edges'] 117 | skel_verts_links = model_torch['skeleton_vertices_links'] 118 | skel_coords_index = model_torch['skeleton_coords_index'] 119 | joint_marker_index = model_torch['joint_marker_index'] 120 | # skel_coords = model_torch['skeleton_coords'] 121 | skel_coords0 = model_torch['skeleton_coords0'] 122 | bone_lengths_index = model_torch['bone_lengths_index'] 123 | joint_marker_vec_index = model_torch['joint_marker_vec_index'] 124 | I_bone = model_torch['I_bone'] 125 | is_euler = model_torch['is_euler'][2:] # [0] is global translation, [1] is global rotation 126 | # 127 | nSigmaPoints = model_t0_torch.size()[0] 128 | skel_verts_new = model_torch['skeleton_vertices_new'].repeat(nSigmaPoints, 1, 1) 129 | joint_marker_vec_new = model_torch['joint_marker_vectors_new'].repeat(nSigmaPoints, 1, 1) 130 | 131 | bone_lengths_sym = bone_lengths[:, bone_lengths_index] 132 | joint_marker_vec_sym = joint_marker_vec[:, abs(joint_marker_vec_index) - 1] 133 | mask_marker_sym = (joint_marker_vec_index < 0) 134 | joint_marker_vec_sym[:, mask_marker_sym, 0] = -joint_marker_vec_sym[:, mask_marker_sym, 0] 135 | 136 | # BONE COORDINATE SYSTEMS 137 | # always use rodrigues parameterization for first rotation to avoid gimbal lock 138 | R_T = rodrigues2rotMat(model_r0_torch).transpose(1, 2) 139 | skel_coords_new = torch.einsum('sij,bjk->sbik', (R_T, I_bone)) 140 | # the for loop goes through the skeleton in a directed order 141 | # rotates all skeleton coordinate systems that are affected by the rotation of bone i_bone 142 | for i_bone in range(nBones-1): 143 | if (is_euler[i_bone]): 144 | cos_x = torch.cos(model_r_torch[:, i_bone, 0]) 145 | sin_x = torch.sin(model_r_torch[:, i_bone, 0]) 146 | cos_y = torch.cos(model_r_torch[:, i_bone, 1]) 147 | sin_y = torch.sin(model_r_torch[:, i_bone, 1]) 148 | cos_z = torch.cos(model_r_torch[:, i_bone, 2]) 149 | sin_z = torch.sin(model_r_torch[:, i_bone, 2]) 150 | R_T = torch.stack([torch.stack([cos_y*cos_z, 151 | sin_x*sin_y*cos_z + cos_x*sin_z, 152 | -cos_x*sin_y*cos_z + sin_x*sin_z], 1), 153 | torch.stack([-cos_y*sin_z, 154 | -sin_x*sin_y*sin_z + cos_x*cos_z, 155 | cos_x*sin_y*sin_z + sin_x*cos_z], 1), 156 | torch.stack([sin_y, 157 | -sin_x*cos_y, 158 | cos_x*cos_y], 1)], 1) 159 | else: 160 | R_T = rodrigues2rotMat(model_r_torch[:, i_bone]).transpose(1, 2) 161 | skel_coords_new[:, skel_verts_links[i_bone+1]] = torch.einsum('sij,sbjk->sbik', (R_T, skel_coords_new[:, skel_verts_links[i_bone+1]])) 162 | skel_coords_new = skel_coords_new.transpose(2, 3) # to apply rotations starting from leaf joints 163 | skel_coords_new = torch.einsum('sbij,bjk->sbik', (skel_coords_new, skel_coords0)) 164 | 165 | # SKELETON & MARKER 166 | # this moves all the other markers and the skeleton 167 | for i_bone in range(nBones): 168 | index_bone_start = skel_edges[i_bone, 0] 169 | index_bone_end = skel_edges[i_bone, 1] 170 | # SKELETON 171 | skel_verts_new[:, index_bone_end] = skel_verts_new[:, index_bone_start] + (skel_coords_new[:, i_bone, :, 2] * bone_lengths_sym[:, i_bone][:, None]) 172 | # MARKER 173 | mask_markers = (joint_marker_index == index_bone_end) 174 | if torch.any(mask_markers): 175 | joint_marker_vec_new[:, mask_markers] = torch.einsum('sij,sbj->sbi', (skel_coords_new[:, i_bone], joint_marker_vec_sym[:, mask_markers])) 176 | # translation: the position of first vertex in the skeleton graph becomes equal to the translation vector (first vertex position is always (0, 0, 0)) 177 | skel_verts_new = skel_verts_new + model_t0_torch[:, None, :] 178 | 179 | # add joint position to get final marker positions 180 | marker_pos_new = skel_verts_new[:, joint_marker_index] + joint_marker_vec_new 181 | 182 | # SURFACE (optional, not used during optimization) 183 | if (adjust_surface): # should raise an error for nSigmaPoints != 1 (i.e. does not work then) 184 | # if ('surface_vertices' in model_torch.keys()): 185 | # surf_verts = model_torch['surface_vertices'] 186 | # surf_verts_weights = model_torch['surface_vertices_weights'] 187 | # surf_verts_new = torch.zeros_like(surf_verts, dtype=float_type) 188 | # for i_bone in range(nBones): 189 | # index_bone_start = skel_edges[i_bone, 0] 190 | # index_bone_end = skel_edges[i_bone, 1] 191 | # skel_coords_new_use = skel_coords_new[0, i_bone] 192 | # mask_surf = (surf_verts_weights[:, index_bone_end] != 0.0) 193 | # skin_pos_norm = surf_verts[mask_surf] - skel_verts[index_bone_start] 194 | # skin_pos_new = torch.einsum('ij,bj->bi', (skel_coords_new_use, skin_pos_norm)) + skel_verts_new[0, index_bone_start] 195 | # surf_verts_new[mask_surf] = surf_verts_new[mask_surf] + surf_verts_weights[mask_surf, index_bone_end][:, None] * skin_pos_new 196 | # else: 197 | # skel_coords_new = float('nan') 198 | # surf_verts_new = float('nan') 199 | surf_verts_new = float('nan') 200 | return skel_coords_new, skel_verts_new, surf_verts_new, marker_pos_new 201 | else: 202 | return marker_pos_new # nSigmaPoints, nMarkers, xy 203 | 204 | def fcn_emission(x_torch, args_torch): 205 | nPara_skel = args_torch['nPara_bones'] + args_torch['nPara_markers'] 206 | 207 | nSigmaPoints = x_torch.size()[0] 208 | model_bone_lengths = x_torch[:, :args_torch['nPara_bones']].reshape(nSigmaPoints, args_torch['nPara_bones']) 209 | joint_marker_vec = x_torch[:, args_torch['nPara_bones']:nPara_skel].reshape(nSigmaPoints, 210 | args_torch['numbers']['nMarkers'], 3) 211 | model_t0_torch = x_torch[:, nPara_skel:nPara_skel+3].reshape(nSigmaPoints, 3) 212 | model_r0_torch = x_torch[:, nPara_skel+3:nPara_skel+6].reshape(nSigmaPoints, 3) 213 | model_r_torch = x_torch[:, nPara_skel+6:].reshape(nSigmaPoints, args_torch['numbers']['nBones']-1, 3) 214 | 215 | if (args_torch['plot']): 216 | _, skel_pos_torch, _, marker_pos_torch = \ 217 | adjust_joint_marker_pos2(args_torch['model'], 218 | model_bone_lengths, joint_marker_vec, 219 | model_t0_torch, model_r0_torch, model_r_torch, 220 | args_torch['numbers']['nBones'], 221 | True) 222 | else: 223 | marker_pos_torch = adjust_joint_marker_pos2(args_torch['model'], 224 | model_bone_lengths, joint_marker_vec, 225 | model_t0_torch, model_r0_torch, model_r_torch, 226 | args_torch['numbers']['nBones']) 227 | marker_proj_torch = map_m(args_torch['calibration']['RX1_fit'], 228 | args_torch['calibration']['tX1_fit'], 229 | args_torch['calibration']['A_fit'], 230 | args_torch['calibration']['k_fit'], 231 | marker_pos_torch).reshape(nSigmaPoints, 232 | args_torch['numbers']['nCameras']*args_torch['numbers']['nMarkers']*2) 233 | 234 | if (args_torch['plot']): 235 | return marker_proj_torch, marker_pos_torch, skel_pos_torch 236 | else: 237 | return marker_proj_torch 238 | 239 | def fcn_emission_free(x_free_torch, args_torch): 240 | free_para_bones = args_torch['free_para_bones'] 241 | free_para_markers = args_torch['free_para_markers'] 242 | free_para_pose = args_torch['free_para_pose'] 243 | nPara_bones = args_torch['nPara_bones'] 244 | nPara_markers = args_torch['nPara_markers'] 245 | nFree_bones = args_torch['nFree_bones'] 246 | nFree_markers = args_torch['nFree_markers'] 247 | 248 | nSigmaPoints = x_free_torch.size()[0] 249 | x_torch = args_torch['x_torch'].repeat(nSigmaPoints, 1) 250 | 251 | nPara_skel = nPara_bones + nPara_markers 252 | nFree_skel = nFree_bones + nFree_markers 253 | 254 | x_bones_torch = x_torch[:, :nPara_bones] 255 | x_bones_torch[:, free_para_bones] = undo_normalization_bones(x_free_torch[:, :nFree_bones]) 256 | x_markers_torch = x_torch[:, nPara_bones:nPara_skel] 257 | x_markers_torch[:, free_para_markers] = undo_normalization_markers(x_free_torch[:, nFree_bones:nFree_skel]) 258 | x_pose_torch = x_torch[:, nPara_skel:] 259 | x_pose_torch[:, free_para_pose] = undo_normalization(x_free_torch[:, nFree_skel:], args_torch) 260 | x_torch = torch.cat([x_bones_torch, 261 | x_markers_torch, 262 | x_pose_torch], 1) 263 | 264 | if (args_torch['plot']): 265 | marker_proj_torch, marker_pos_torch, skel_pos_torch = fcn_emission(x_torch, args_torch) 266 | marker_proj_torch[:, 0::2] = marker_proj_torch[:, 0::2] / (cfg.normalize_camera_sensor_x * 0.5) - 1.0 267 | marker_proj_torch[:, 1::2] = marker_proj_torch[:, 1::2] / (cfg.normalize_camera_sensor_y * 0.5) - 1.0 268 | return marker_proj_torch, marker_pos_torch, skel_pos_torch 269 | else: 270 | marker_proj_torch = fcn_emission(x_torch, args_torch) 271 | marker_proj_torch[:, 0::2] = marker_proj_torch[:, 0::2] / (cfg.normalize_camera_sensor_x * 0.5) - 1.0 272 | marker_proj_torch[:, 1::2] = marker_proj_torch[:, 1::2] / (cfg.normalize_camera_sensor_y * 0.5) - 1.0 273 | return marker_proj_torch 274 | 275 | # WARNING: ONLY USE THIS WHEN MOVEMENT MODEL IS FIXED TO THE IDENTITY (I.E. A = I)! 276 | def fcn_transition_free(z_tm1, M_transition, args_torch): 277 | return z_tm1 278 | 279 | def obj_fcn(x_free_torch, args_torch): 280 | nFrames = args_torch['nFrames'] 281 | nFree_bones = args_torch['nFree_bones'] 282 | nFree_markers = args_torch['nFree_markers'] 283 | nFree_pose = args_torch['nFree_pose'] 284 | 285 | x_free_skel_torch = x_free_torch[:nFree_bones+nFree_markers].repeat(nFrames, 1) 286 | x_free_pose_torch = x_free_torch[nFree_bones+nFree_markers:].reshape(nFrames, nFree_pose) 287 | x_free_use_torch = torch.cat([x_free_skel_torch, x_free_pose_torch], 1) 288 | marker_proj_torch = fcn_emission_free(x_free_use_torch, args_torch).reshape(nFrames, 289 | args_torch['numbers']['nCameras'], 290 | args_torch['numbers']['nMarkers'], 291 | 2) 292 | # reverse normalization 293 | diff_x_torch = ((marker_proj_torch[:, :, :, 0] + 1.0) * (cfg.normalize_camera_sensor_x * 0.5) - \ 294 | args_torch['labels_single_torch'][:, :, :, 0]) 295 | diff_y_torch = ((marker_proj_torch[:, :, :, 1] + 1.0) * (cfg.normalize_camera_sensor_y * 0.5) - \ 296 | args_torch['labels_single_torch'][:, :, :, 1]) 297 | 298 | # reduce influence of markers connected to the same joint via weights 299 | dist_torch = ((args_torch['weights'][None, None, :] * diff_x_torch)**2 + \ 300 | (args_torch['weights'][None, None, :] * diff_y_torch)**2) 301 | dist_torch[~args_torch['labels_mask_single_torch']] = 0.0 # set distances of undetected labels to zero 302 | 303 | # normalize by the number of used labels (i.e. get avg. distance per frame per camera per label per xy-position) 304 | res_torch = torch.sum(dist_torch) / torch.sum(args_torch['labels_mask_single_torch'], dtype=float_type) 305 | return res_torch -------------------------------------------------------------------------------- /ACM/optimization.py: -------------------------------------------------------------------------------- 1 | from scipy.optimize import minimize 2 | import time 3 | import torch 4 | 5 | from . import model 6 | 7 | def obj_fcn__wrap(x_free, args): 8 | x_free_torch = args['x_free_torch'] 9 | 10 | x_free_torch.data.copy_(torch.from_numpy(x_free).data) 11 | loss_torch = model.obj_fcn(x_free_torch, args) 12 | loss = loss_torch.item() 13 | 14 | loss_torch.backward() 15 | grad_free = x_free_torch.grad.detach().cpu().clone().numpy() 16 | x_free_torch.grad.data.zero_() 17 | 18 | return loss, grad_free 19 | 20 | def optimize__scipy(x_free, args, 21 | opt_dict): 22 | time_start = time.time() 23 | min_result = minimize(obj_fcn__wrap, 24 | x_free, 25 | args=args, 26 | method=opt_dict['opt_method'], 27 | jac=True, 28 | hess=None, 29 | hessp=None, 30 | bounds=args['bounds_free'], 31 | constraints=(), 32 | tol=None, 33 | callback=None, 34 | options=opt_dict['opt_options']) 35 | time_end = time.time() 36 | print('iterations:\t{:06d}'.format(min_result.nit)) 37 | print('residual:\t{:0.8e}'.format(min_result.fun)) 38 | print('success:\t{}'.format(min_result.success)) 39 | print('message:\t{}'.format(min_result.message)) 40 | print('time needed:\t{:0.3f} seconds'.format(time_end - time_start)) 41 | return min_result 42 | -------------------------------------------------------------------------------- /ACM/routines_math.py: -------------------------------------------------------------------------------- 1 | import configuration as cfg 2 | import numpy as np 3 | 4 | def rodrigues2rotMat_single(r): 5 | if np.all(abs(r) <= cfg.num_tol): 6 | rotMat = np.identity(3, dtype=np.float64) 7 | else: 8 | sqrt_arg = np.sum(r**2) 9 | theta = np.sqrt(sqrt_arg) 10 | u = r / theta 11 | # row 1 12 | rotMat_00 = np.cos(theta) + u[0]**2 * (1.0 - np.cos(theta)) 13 | rotMat_01 = u[0] * u[1] * (1.0 - np.cos(theta)) - u[2] * np.sin(theta) 14 | rotMat_02 = u[0] * u[2] * (1.0 - np.cos(theta)) + u[1] * np.sin(theta) 15 | # row 2 16 | rotMat_10 = u[0] * u[1] * (1.0 - np.cos(theta)) + u[2] * np.sin(theta) 17 | rotMat_11 = np.cos(theta) + u[1]**2 * (1 - np.cos(theta)) 18 | rotMat_12 = u[1] * u[2] * (1.0 - np.cos(theta)) - u[0] * np.sin(theta) 19 | # row 3 20 | rotMat_20 = u[0] * u[2] * (1.0 - np.cos(theta)) - u[1] * np.sin(theta) 21 | rotMat_21 = u[1] * u[2] * (1.0 - np.cos(theta)) + u[0] * np.sin(theta) 22 | rotMat_22 = np.cos(theta) + u[2]**2 * (1.0 - np.cos(theta)) 23 | # output 24 | rotMat = np.array([[rotMat_00, rotMat_01, rotMat_02], 25 | [rotMat_10, rotMat_11, rotMat_12], 26 | [rotMat_20, rotMat_21, rotMat_22]], 27 | dtype=np.float64) 28 | return rotMat 29 | 30 | def rotMat2rodrigues_single(R): 31 | if (abs(np.trace(R) - 3.0) <= cfg.num_tol): 32 | r = np.zeros(3, dtype=np.float64) 33 | else: 34 | theta_norm = np.arccos((np.trace(R) - 1.0) / 2.0) 35 | r = theta_norm / (2.0 * np.sin(theta_norm)) * \ 36 | np.array([R[2,1] - R[1,2], 37 | R[0,2] - R[2,0], 38 | R[1,0] - R[0,1]], 39 | dtype=np.float64) 40 | return r 41 | -------------------------------------------------------------------------------- /ACM/tools.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import torch 4 | from scipy.io import savemat 5 | import shutil 6 | 7 | #from . import em 8 | from . import helper 9 | #from . import kalman 10 | from . import model 11 | 12 | 13 | def propagate_latent_to_pose(config,save_dict,x_ini): 14 | 15 | mu_ini = x_ini 16 | if ('mu_uks' in save_dict): 17 | mu_uks = save_dict['mu_uks'][1:] 18 | var_uks = save_dict['var_uks'][1:] 19 | print(save_dict['message']) 20 | else: 21 | mu_uks = save_dict['mu'][1:] 22 | nPara = np.size(mu_uks, 1) 23 | var_dummy = np.identity(nPara, dtype=np.float64) * 2**-52 24 | var_uks = np.tile(var_dummy.ravel(), config['nT']).reshape(config['nT'], nPara, nPara) 25 | 26 | nT = np.size(mu_uks, 0) 27 | 28 | file_origin_coord = config['file_origin_coord'] 29 | file_calibration = config['file_calibration'] 30 | file_model = config['file_model'] 31 | file_labelsDLC = config['file_labelsDLC'] 32 | 33 | args = helper.get_arguments(file_origin_coord, file_calibration, file_model, file_labelsDLC, 34 | config['scale_factor'], config['pcutoff']) 35 | if ((config['mode'] == 1) or (config['mode'] == 2)): 36 | args['use_custom_clip'] = False 37 | elif ((config['mode'] == 3) or (config['mode'] == 4)): 38 | args['use_custom_clip'] = True 39 | args['plot'] = True 40 | del(args['model']['surface_vertices']) 41 | joint_order = args['model']['joint_order'] 42 | joint_marker_order = args['model']['joint_marker_order'] 43 | skeleton_edges = args['model']['skeleton_edges'].cpu().numpy() 44 | nCameras = args['numbers']['nCameras'] 45 | nMarkers = args['numbers']['nMarkers'] 46 | nBones = args['numbers']['nBones'] 47 | 48 | free_para_bones = args['free_para_bones'].cpu().numpy() 49 | free_para_markers = args['free_para_markers'].cpu().numpy() 50 | free_para_pose = args['free_para_pose'].cpu().numpy() 51 | free_para_bones = np.zeros_like(free_para_bones, dtype=bool) 52 | free_para_markers = np.zeros_like(free_para_markers, dtype=bool) 53 | nFree_bones = int(0) 54 | nFree_markers = int(0) 55 | free_para = np.concatenate([free_para_bones, 56 | free_para_markers, 57 | free_para_pose], 0) 58 | args['x_torch'] = torch.from_numpy(mu_ini).type(model.float_type) 59 | args['x_free_torch'] = torch.from_numpy(mu_ini[free_para]).type(model.float_type) 60 | args['free_para_bones'] = torch.from_numpy(free_para_bones) 61 | args['free_para_markers'] = torch.from_numpy(free_para_markers) 62 | args['nFree_bones'] = nFree_bones 63 | args['nFree_markers'] = nFree_markers 64 | args['x_torch'] = torch.from_numpy(mu_ini).type(model.float_type) 65 | args['x_free_torch'] = torch.from_numpy(mu_ini[free_para]).type(model.float_type) 66 | # 67 | z_all = torch.from_numpy(mu_uks) 68 | 69 | marker_proj, marker_pos, skel3d_all = model.fcn_emission_free(z_all, args) 70 | 71 | pose = { 72 | 'marker_positions_2d': marker_proj.detach().cpu().numpy().reshape(nT, nCameras, nMarkers, 2), 73 | 'marker_positions_3d': marker_pos.detach().cpu().numpy(), 74 | 'joint_positions_3d': skel3d_all.cpu().numpy(), 75 | } 76 | 77 | return pose 78 | 79 | def copy_config(config,input_path): 80 | configdocdir = config["folder_save"]+'/configuration/' 81 | os.makedirs(configdocdir,exist_ok=True) 82 | 83 | shutil.copy(input_path+'/configuration.py',configdocdir) 84 | shutil.copyfile(config['file_origin_coord'],configdocdir+"/file_origin_coord.npy") 85 | shutil.copyfile(config['file_calibration'],configdocdir+"/file_calibration.npy") 86 | shutil.copyfile(config['file_model'],configdocdir+"/file_model.npy") 87 | shutil.copyfile(config['file_labelsDLC'],configdocdir+"/file_labelsDLC.npy") 88 | shutil.copyfile(config['file_labelsManual'],configdocdir+"/file_labelsManual.npz") 89 | -------------------------------------------------------------------------------- /FAQ.md: -------------------------------------------------------------------------------- 1 | # ACM FAQ 2 | 3 | ## Is it possible to use an alternative marker configuration? 4 | 5 | Although the marker positions shown in our publication were carefully selected for their connection to their respective joint, it is in principle possible to choose alternative marker configuration. For this, the following steps have to be performed: 6 | 7 | 1. Prepare a modified models.py, where 8 | joint_marker_order gives a list of the names of the new set of markers and 9 | joint_marker_index links each of these new markers to a joint. 10 | 11 | *Note that it is important to be consistent with the naming of the new joints & markers (i.e. for joints: 'joint\_\*' & for surface markers: 'spot\_\*' in labeling GUI, otherwise 'marker\_\*\_start'). Particularly, function 'initialize\_x' in calibration.py currently assumes that the model contains markers with the names 'spot\_head\_002' and 'spot\_head\_003' or 'spot_spine\_\*' and/or 'spot_tail\_\*' (this function is used to get very rough initial pose alignments in calibration.py and initialization.py).* 12 | 2. Modify anatomy.py for new constraints according to the methods section 13 | "Constraining surface marker positions based on body symmetry" 14 | 15 | *This concerns the functions 'get_bounds_markers', 'get_bounds_pose' and 'get_skeleton_coords0'* -------------------------------------------------------------------------------- /INPUTS.md: -------------------------------------------------------------------------------- 1 | ### File structure inputs 2 | 3 | #### configuration.py 4 | 5 | ###### General 6 | For running the example dataset, only the start and end frame indices `index_frame_start` and `index_frame_end` as well as the desired output location `folder_save` need to be specified. 7 | 8 | ###### Calibration 9 | In the calibration step the anatomy of the animal is learned, based on manually annotated labels. The most important parameters are: 10 | ``` 11 | # the body weight of the animal in gram 12 | body_weight = 72.0 13 | # the number of frames to skip between time points in the sequence used for learning the animal's anatomy 14 | dFrames_calib = 200 15 | # the start and end frame index of the sequence used for learning the animal's anatomy 16 | index_frames_calib = list([[6900, 44300],]) 17 | ``` 18 | Thus, `dFrames_calib=50` and `index_frames_calib=list([[0,100],[1000,1100]])` will result in learning the anatomy using frames with indices 0, 50, 100, 1000, 1050 and 1100. These frames need to be manually annotated in `labels_manual.npz`. 19 | 20 | ###### Initialization 21 | In the initialization step the animal's pose in the first time point of the sequence, which sould be reconstructed, is learned via gradient descent optimization. 22 | 23 | ###### Pose reconstruction 24 | In the final step the model parameters are learned, i.e. poses are reconstructed. The most important parameters are: 25 | ``` 26 | # determines which constraints will be enforced when poses are reconstructed (values: 1 - 4) 27 | # 1: no constraints (deterministic model) 28 | # 2: only anatomical constraints (deterministic model) 29 | # 3: only temporal constraints (probabilistic model) 30 | # 4: anatomical and temporal constraints (probabilistic model) 31 | mode = 4 32 | # the number of frames to skip between time points in the sequence, which should be reconstructed 33 | dt = 5 34 | ``` 35 | 36 | #### model.npy 37 | 38 | The model defining the skeleton graph of the animal in the format: 39 | ``` 40 | dict( 41 | # bone lengths (legacy: can just be filled with zeros) # LEGACY 42 | 'bone_lengths': np.ndarray, shape=(nBones,), dtype('float64'), # LEGACY 43 | # defines to which joint a marker is connected to (i.e. each element is a respective joint index) 44 | 'joint_marker_index': np.ndarray, shape=(nMarkers,), dtype('int64'), 45 | # ordered list of marker names 46 | 'joint_marker_order': list, shape=(nMarkers,), type('str'), 47 | # relative marker locations (legacy: can just be filled with zeros) # LEGACY 48 | 'joint_marker_vectors': np.ndarray, shape=(nMarkers, 3,), dtype('float64'), # LEGACY 49 | # ordered list of joint names 50 | 'joint_order': list, shape=(nJoints,), type('str'), 51 | # local bone coordinate systems in rest pose (should be consistent with 'skeleton_vertices') 52 | 'skeleton_coords': np.ndarray, shape=(nBones, 3, 3,), dtype('float64'), 53 | # bone indices (legacy: can just be filled with zeros) # LEGACY 54 | 'skeleton_coords_index': np.ndarray, shape=(nBones,), dtype('int64'), # LEGACY 55 | # defines the bones of the skeleton graph (i.e. each entry contains indices of two joints, which form a bone when connected) 56 | 'skeleton_edges': np.ndarray, shape=(nBones, 2,), dtype('int64'), 57 | # absolute joint positions in rest pose (should be consistent with 'skeleton_coords'; resulting bone lengths should all be equal to one) 58 | 'skeleton_vertices': np.ndarray, shape=(nJoints, 3,), dtype('float64'), 59 | # indicates if a bone is affected by a rotation of a preceding bone in the skeleton graph 60 | 'skeleton_vertices_links': np.ndarray, shape=(nBones, nBones,), dtype('bool'), 61 | # defines triangles of a surface mesh (i.e. each triagle is defined by three vertex indices) (legacy: can just be an empty array) # LEGACY 62 | 'surface_triangles': np.ndarray, shape=(nTriangles, 3,), dtype('int32'), # LEGACY 63 | # defines absolute locations of vertices of a surface mesh (legacy: can just be an empty array) # LEGACY 64 | 'surface_vertices': np.ndarray, shape=(nVertices, 3,), dtype('float64'), # LEGACY 65 | # defines weights of surface vertices such that they can be adjusted according to linear blend skinning (legacy: can just be an empty array) # LEGACY 66 | 'surface_vertices_weights': np.ndarray, shape=(nVertices, nJoints,), dtype('float64'), # LEGACY 67 | ) 68 | ``` 69 | Any additional keys of the dictionary are not mandatory and originate from the development phase of the method. 70 | In the default model, joint names are: `['joint_ankle_left', 'joint_ankle_right', 'joint_elbow_left', 'joint_elbow_right', 'joint_finger_left_002', 'joint_finger_right_002', 'joint_head_001', 'joint_hip_left', 'joint_hip_right', 'joint_knee_left', 'joint_knee_right', 'joint_paw_hind_left', 'joint_paw_hind_right', 'joint_shoulder_left', 'joint_shoulder_right', 'joint_spine_001', 'joint_spine_002', 'joint_spine_003', 'joint_spine_004', 'joint_spine_005', 'joint_tail_001', 'joint_tail_002', 'joint_tail_003', 'joint_tail_004', 'joint_tail_005', 'joint_toe_left_002', 'joint_toe_right_002', 'joint_wrist_left', 'joint_wrist_right']` and marker names are: `['marker_ankle_left_start', 'marker_ankle_right_start', 'marker_elbow_left_start', 'marker_elbow_right_start', 'marker_finger_left_001_start', 'marker_finger_left_002_start', 'marker_finger_left_003_start', 'marker_finger_right_001_start', 'marker_finger_right_002_start', 'marker_finger_right_003_start', 'marker_head_001_start', 'marker_head_002_start', 'marker_head_003_start', 'marker_hip_left_start', 'marker_hip_right_start', 'marker_knee_left_start', 'marker_knee_right_start', 'marker_paw_front_left_start', 'marker_paw_front_right_start', 'marker_paw_hind_left_start', 'marker_paw_hind_right_start', 'marker_shoulder_left_start', 'marker_shoulder_right_start', 'marker_side_left_start', 'marker_side_right_start', 'marker_spine_001_start', 'marker_spine_002_start', 'marker_spine_003_start', 'marker_spine_004_start', 'marker_spine_005_start', 'marker_spine_006_start', 'marker_tail_001_start', 'marker_tail_002_start', 'marker_tail_003_start', 'marker_tail_004_start', 'marker_tail_005_start', 'marker_tail_006_start', 'marker_toe_left_001_start', 'marker_toe_left_002_start', 'marker_toe_left_003_start', 'marker_toe_right_001_start', 'marker_toe_right_002_start', 'marker_toe_right_003_start']`. 71 | Currently, some joint and marker names are hard coded. Therefore changing these names is not recommended (e.g. 'joint_hip_left', 'joint_hip_right', 'joint_shoulder_left', 'joint_shoulder_left' are hard coded; see functions `get_coord0` and `get_skeleton_coords0` in `anatomy.py`). 72 | 73 | #### multicalibration.npy 74 | 75 | The multicalibration in the format: 76 | ``` 77 | dict( 78 | # focal lengths and principal point locations for each camera (intrinsic parameters) 79 | 'A_fit': np.ndarray, shape=(nCameras, 4,), dtype('float64'), 80 | # rotation matrix for each camera (extrinsic parameters) 81 | 'RX1_fit': np.ndarray, shape=(nCameras, 3, 3,), dtype('float64'), 82 | # distortion coefficients for each camera (intrinsic parameters) 83 | 'k_fit': np.ndarray, shape=(nCameras, 5,), dtype('float64'), 84 | # total number of calibrated cameras 85 | 'nCameras': type('int'), 86 | # Rodrigues vectors corresponding to 'RX1_fit' for each camera (extrinsic parameters) 87 | 'rX1_fit': np.ndarray, shape=(nCameras, 3,), dtype('float64'), 88 | # translation vector for each camera (extrinsic parameters) 89 | 'tX1_fit': np.ndarray, shape=(nCameras, 3,), dtype('float64'), 90 | ) 91 | ``` 92 | Any additional keys of the dictionary are not mandatory and originate from the development phase of the method. 93 | A multi-camera calibration file in this format can be produced with our calibration software [calibcam](https://github.com/bbo-lab/calibcam), using ChArUco boards (use the `multicalibration-v1.npy` output file). 94 | 95 | #### origin_coord.npy 96 | 97 | Multicalibrations produced by [calibcam](https://github.com/bbo-lab/calibcam) are aligned based on the camera 1 position and direction. origin_coord determines the coordinate system of the final result, relative to the coordinate system of the camera calibration. This usually has the arena center as origin and the z-axis pointing upwards. The format is: 98 | ``` 99 | dict( 100 | # origin of the arena coordinate system in the coordinate system of the multicalibration 101 | 'origin': np.ndarray, shape=(3,), dtype('float64'), 102 | # orientation of arena coordinate system in the coordinate system of the multicalibration (i.e. [lateral1, lateral2, up]) 103 | 'coord': np.ndarray, shape=(3, 3,), dtype('float64'), 104 | ) 105 | ``` 106 | Thus, `origin_coord:coord * x_arena + origin_coord:origin = x_camera`. 107 | 108 | #### labels_manual.npz 109 | 110 | Manual labels of surface markers on the videography data. Content is a dictionary with integer frame indices as keys (`[frameidx]` below). Each value is another dictionary with the label names as keys (`[label name]` below). The format is: 111 | ``` 112 | dict( 113 | [frameidx]: dict( 114 | # positions of labels in 2D image 115 | [label name]: np.ndarray, shape=(nCameras, 2), dtype('int64'), 116 | ... 117 | ) 118 | ... 119 | ) 120 | ``` 121 | In the default model, label names are: `['spot_ankle_left', 'spot_ankle_right', 'spot_elbow_left', 'spot_elbow_right', 'spot_finger_left_001', 'spot_finger_left_002', 'spot_finger_left_003', 'spot_finger_right_001', 'spot_finger_right_002', 'spot_finger_right_003', 'spot_head_001', 'spot_head_002', 'spot_head_003', 'spot_hip_left', 'spot_hip_right', 'spot_knee_left', 'spot_knee_right', 'spot_paw_front_left', 'spot_paw_front_right', 'spot_paw_hind_left', 'spot_paw_hind_right', 'spot_shoulder_left', 'spot_shoulder_right', 'spot_side_left', 'spot_side_right', 'spot_spine_001', 'spot_spine_002', 'spot_spine_003', 'spot_spine_004', 'spot_spine_005', 'spot_spine_006', 'spot_tail_001', 'spot_tail_002', 'spot_tail_003', 'spot_tail_004', 'spot_tail_005', 'spot_tail_006', 'spot_toe_left_001', 'spot_toe_left_002', 'spot_toe_left_003', 'spot_toe_right_001', 'spot_toe_right_002', 'spot_toe_right_003']`. 122 | To differentiate manually annotated labels from reconstructed marker locations, these names are later on modified internally (see `joint_marker_order` in `model.npy`). 123 | Currently, some label names are hard coded. Therefore changing these names is not recommended (see function `initialize_x` in `calibration.py`). 124 | The hard coded label names are: 'spot_head_002', 'spot_head_003'. Labels with the strings 'spine' and 'tail' in their names are used for roughly aligning the 3D orientation of the animal before learning its anatomy. 125 | 126 | #### labels_dlc_n_m.npy 127 | 128 | Automatically detected labels of markers on the videography data in the format: 129 | ``` 130 | dict( 131 | # original file location (legacy: can just be an empty string) # LEGACY 132 | 'file_save', type('str') # LEGACY 133 | # list of frame indices that the frames in 'labels_all' correspond to 134 | 'frame_list': np.ndarray, shape=(nFrames,), dtype('int64'), 135 | # positions of markers in 2D image plus confidence value from DLC 136 | 'labels_all': np.ndarray, shape=(nFrames, nCameras, nMarkers, 3,), dtype('float64'), 137 | ) 138 | ``` 139 | Any additional keys of the dictionary are not mandatory and originate from the development phase of the method. 140 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | ACM (Anatomically-constrained model) - a framework for for videography based pose tracking of rodents 2 | Copyright (C) 2021 Research Center Caesar, Arne Monsees 3 | 4 | admin-bbo@caesar.de 5 | 6 | This library is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU Lesser General Public 8 | License as published by the Free Software Foundation; either 9 | version 2.1 of the License, or (at your option) any later version. 10 | 11 | This library is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public 17 | License along with this library; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 19 | USA 20 | 21 | GNU LESSER GENERAL PUBLIC LICENSE 22 | Version 2.1, February 1999 23 | 24 | Copyright (C) 1991, 1999 Free Software Foundation, Inc. 25 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 26 | Everyone is permitted to copy and distribute verbatim copies 27 | of this license document, but changing it is not allowed. 28 | 29 | [This is the first released version of the Lesser GPL. It also counts 30 | as the successor of the GNU Library Public License, version 2, hence 31 | the version number 2.1.] 32 | 33 | Preamble 34 | 35 | The licenses for most software are designed to take away your 36 | freedom to share and change it. By contrast, the GNU General Public 37 | Licenses are intended to guarantee your freedom to share and change 38 | free software--to make sure the software is free for all its users. 39 | 40 | This license, the Lesser General Public License, applies to some 41 | specially designated software packages--typically libraries--of the 42 | Free Software Foundation and other authors who decide to use it. You 43 | can use it too, but we suggest you first think carefully about whether 44 | this license or the ordinary General Public License is the better 45 | strategy to use in any particular case, based on the explanations below. 46 | 47 | When we speak of free software, we are referring to freedom of use, 48 | not price. Our General Public Licenses are designed to make sure that 49 | you have the freedom to distribute copies of free software (and charge 50 | for this service if you wish); that you receive source code or can get 51 | it if you want it; that you can change the software and use pieces of 52 | it in new free programs; and that you are informed that you can do 53 | these things. 54 | 55 | To protect your rights, we need to make restrictions that forbid 56 | distributors to deny you these rights or to ask you to surrender these 57 | rights. These restrictions translate to certain responsibilities for 58 | you if you distribute copies of the library or if you modify it. 59 | 60 | For example, if you distribute copies of the library, whether gratis 61 | or for a fee, you must give the recipients all the rights that we gave 62 | you. You must make sure that they, too, receive or can get the source 63 | code. If you link other code with the library, you must provide 64 | complete object files to the recipients, so that they can relink them 65 | with the library after making changes to the library and recompiling 66 | it. And you must show them these terms so they know their rights. 67 | 68 | We protect your rights with a two-step method: (1) we copyright the 69 | library, and (2) we offer you this license, which gives you legal 70 | permission to copy, distribute and/or modify the library. 71 | 72 | To protect each distributor, we want to make it very clear that 73 | there is no warranty for the free library. Also, if the library is 74 | modified by someone else and passed on, the recipients should know 75 | that what they have is not the original version, so that the original 76 | author's reputation will not be affected by problems that might be 77 | introduced by others. 78 | 79 | Finally, software patents pose a constant threat to the existence of 80 | any free program. We wish to make sure that a company cannot 81 | effectively restrict the users of a free program by obtaining a 82 | restrictive license from a patent holder. Therefore, we insist that 83 | any patent license obtained for a version of the library must be 84 | consistent with the full freedom of use specified in this license. 85 | 86 | Most GNU software, including some libraries, is covered by the 87 | ordinary GNU General Public License. This license, the GNU Lesser 88 | General Public License, applies to certain designated libraries, and 89 | is quite different from the ordinary General Public License. We use 90 | this license for certain libraries in order to permit linking those 91 | libraries into non-free programs. 92 | 93 | When a program is linked with a library, whether statically or using 94 | a shared library, the combination of the two is legally speaking a 95 | combined work, a derivative of the original library. The ordinary 96 | General Public License therefore permits such linking only if the 97 | entire combination fits its criteria of freedom. The Lesser General 98 | Public License permits more lax criteria for linking other code with 99 | the library. 100 | 101 | We call this license the "Lesser" General Public License because it 102 | does Less to protect the user's freedom than the ordinary General 103 | Public License. It also provides other free software developers Less 104 | of an advantage over competing non-free programs. These disadvantages 105 | are the reason we use the ordinary General Public License for many 106 | libraries. However, the Lesser license provides advantages in certain 107 | special circumstances. 108 | 109 | For example, on rare occasions, there may be a special need to 110 | encourage the widest possible use of a certain library, so that it becomes 111 | a de-facto standard. To achieve this, non-free programs must be 112 | allowed to use the library. A more frequent case is that a free 113 | library does the same job as widely used non-free libraries. In this 114 | case, there is little to gain by limiting the free library to free 115 | software only, so we use the Lesser General Public License. 116 | 117 | In other cases, permission to use a particular library in non-free 118 | programs enables a greater number of people to use a large body of 119 | free software. For example, permission to use the GNU C Library in 120 | non-free programs enables many more people to use the whole GNU 121 | operating system, as well as its variant, the GNU/Linux operating 122 | system. 123 | 124 | Although the Lesser General Public License is Less protective of the 125 | users' freedom, it does ensure that the user of a program that is 126 | linked with the Library has the freedom and the wherewithal to run 127 | that program using a modified version of the Library. 128 | 129 | The precise terms and conditions for copying, distribution and 130 | modification follow. Pay close attention to the difference between a 131 | "work based on the library" and a "work that uses the library". The 132 | former contains code derived from the library, whereas the latter must 133 | be combined with the library in order to run. 134 | 135 | GNU LESSER GENERAL PUBLIC LICENSE 136 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 137 | 138 | 0. This License Agreement applies to any software library or other 139 | program which contains a notice placed by the copyright holder or 140 | other authorized party saying it may be distributed under the terms of 141 | this Lesser General Public License (also called "this License"). 142 | Each licensee is addressed as "you". 143 | 144 | A "library" means a collection of software functions and/or data 145 | prepared so as to be conveniently linked with application programs 146 | (which use some of those functions and data) to form executables. 147 | 148 | The "Library", below, refers to any such software library or work 149 | which has been distributed under these terms. A "work based on the 150 | Library" means either the Library or any derivative work under 151 | copyright law: that is to say, a work containing the Library or a 152 | portion of it, either verbatim or with modifications and/or translated 153 | straightforwardly into another language. (Hereinafter, translation is 154 | included without limitation in the term "modification".) 155 | 156 | "Source code" for a work means the preferred form of the work for 157 | making modifications to it. For a library, complete source code means 158 | all the source code for all modules it contains, plus any associated 159 | interface definition files, plus the scripts used to control compilation 160 | and installation of the library. 161 | 162 | Activities other than copying, distribution and modification are not 163 | covered by this License; they are outside its scope. The act of 164 | running a program using the Library is not restricted, and output from 165 | such a program is covered only if its contents constitute a work based 166 | on the Library (independent of the use of the Library in a tool for 167 | writing it). Whether that is true depends on what the Library does 168 | and what the program that uses the Library does. 169 | 170 | 1. You may copy and distribute verbatim copies of the Library's 171 | complete source code as you receive it, in any medium, provided that 172 | you conspicuously and appropriately publish on each copy an 173 | appropriate copyright notice and disclaimer of warranty; keep intact 174 | all the notices that refer to this License and to the absence of any 175 | warranty; and distribute a copy of this License along with the 176 | Library. 177 | 178 | You may charge a fee for the physical act of transferring a copy, 179 | and you may at your option offer warranty protection in exchange for a 180 | fee. 181 | 182 | 2. You may modify your copy or copies of the Library or any portion 183 | of it, thus forming a work based on the Library, and copy and 184 | distribute such modifications or work under the terms of Section 1 185 | above, provided that you also meet all of these conditions: 186 | 187 | a) The modified work must itself be a software library. 188 | 189 | b) You must cause the files modified to carry prominent notices 190 | stating that you changed the files and the date of any change. 191 | 192 | c) You must cause the whole of the work to be licensed at no 193 | charge to all third parties under the terms of this License. 194 | 195 | d) If a facility in the modified Library refers to a function or a 196 | table of data to be supplied by an application program that uses 197 | the facility, other than as an argument passed when the facility 198 | is invoked, then you must make a good faith effort to ensure that, 199 | in the event an application does not supply such function or 200 | table, the facility still operates, and performs whatever part of 201 | its purpose remains meaningful. 202 | 203 | (For example, a function in a library to compute square roots has 204 | a purpose that is entirely well-defined independent of the 205 | application. Therefore, Subsection 2d requires that any 206 | application-supplied function or table used by this function must 207 | be optional: if the application does not supply it, the square 208 | root function must still compute square roots.) 209 | 210 | These requirements apply to the modified work as a whole. If 211 | identifiable sections of that work are not derived from the Library, 212 | and can be reasonably considered independent and separate works in 213 | themselves, then this License, and its terms, do not apply to those 214 | sections when you distribute them as separate works. But when you 215 | distribute the same sections as part of a whole which is a work based 216 | on the Library, the distribution of the whole must be on the terms of 217 | this License, whose permissions for other licensees extend to the 218 | entire whole, and thus to each and every part regardless of who wrote 219 | it. 220 | 221 | Thus, it is not the intent of this section to claim rights or contest 222 | your rights to work written entirely by you; rather, the intent is to 223 | exercise the right to control the distribution of derivative or 224 | collective works based on the Library. 225 | 226 | In addition, mere aggregation of another work not based on the Library 227 | with the Library (or with a work based on the Library) on a volume of 228 | a storage or distribution medium does not bring the other work under 229 | the scope of this License. 230 | 231 | 3. You may opt to apply the terms of the ordinary GNU General Public 232 | License instead of this License to a given copy of the Library. To do 233 | this, you must alter all the notices that refer to this License, so 234 | that they refer to the ordinary GNU General Public License, version 2, 235 | instead of to this License. (If a newer version than version 2 of the 236 | ordinary GNU General Public License has appeared, then you can specify 237 | that version instead if you wish.) Do not make any other change in 238 | these notices. 239 | 240 | Once this change is made in a given copy, it is irreversible for 241 | that copy, so the ordinary GNU General Public License applies to all 242 | subsequent copies and derivative works made from that copy. 243 | 244 | This option is useful when you wish to copy part of the code of 245 | the Library into a program that is not a library. 246 | 247 | 4. You may copy and distribute the Library (or a portion or 248 | derivative of it, under Section 2) in object code or executable form 249 | under the terms of Sections 1 and 2 above provided that you accompany 250 | it with the complete corresponding machine-readable source code, which 251 | must be distributed under the terms of Sections 1 and 2 above on a 252 | medium customarily used for software interchange. 253 | 254 | If distribution of object code is made by offering access to copy 255 | from a designated place, then offering equivalent access to copy the 256 | source code from the same place satisfies the requirement to 257 | distribute the source code, even though third parties are not 258 | compelled to copy the source along with the object code. 259 | 260 | 5. A program that contains no derivative of any portion of the 261 | Library, but is designed to work with the Library by being compiled or 262 | linked with it, is called a "work that uses the Library". Such a 263 | work, in isolation, is not a derivative work of the Library, and 264 | therefore falls outside the scope of this License. 265 | 266 | However, linking a "work that uses the Library" with the Library 267 | creates an executable that is a derivative of the Library (because it 268 | contains portions of the Library), rather than a "work that uses the 269 | library". The executable is therefore covered by this License. 270 | Section 6 states terms for distribution of such executables. 271 | 272 | When a "work that uses the Library" uses material from a header file 273 | that is part of the Library, the object code for the work may be a 274 | derivative work of the Library even though the source code is not. 275 | Whether this is true is especially significant if the work can be 276 | linked without the Library, or if the work is itself a library. The 277 | threshold for this to be true is not precisely defined by law. 278 | 279 | If such an object file uses only numerical parameters, data 280 | structure layouts and accessors, and small macros and small inline 281 | functions (ten lines or less in length), then the use of the object 282 | file is unrestricted, regardless of whether it is legally a derivative 283 | work. (Executables containing this object code plus portions of the 284 | Library will still fall under Section 6.) 285 | 286 | Otherwise, if the work is a derivative of the Library, you may 287 | distribute the object code for the work under the terms of Section 6. 288 | Any executables containing that work also fall under Section 6, 289 | whether or not they are linked directly with the Library itself. 290 | 291 | 6. As an exception to the Sections above, you may also combine or 292 | link a "work that uses the Library" with the Library to produce a 293 | work containing portions of the Library, and distribute that work 294 | under terms of your choice, provided that the terms permit 295 | modification of the work for the customer's own use and reverse 296 | engineering for debugging such modifications. 297 | 298 | You must give prominent notice with each copy of the work that the 299 | Library is used in it and that the Library and its use are covered by 300 | this License. You must supply a copy of this License. If the work 301 | during execution displays copyright notices, you must include the 302 | copyright notice for the Library among them, as well as a reference 303 | directing the user to the copy of this License. Also, you must do one 304 | of these things: 305 | 306 | a) Accompany the work with the complete corresponding 307 | machine-readable source code for the Library including whatever 308 | changes were used in the work (which must be distributed under 309 | Sections 1 and 2 above); and, if the work is an executable linked 310 | with the Library, with the complete machine-readable "work that 311 | uses the Library", as object code and/or source code, so that the 312 | user can modify the Library and then relink to produce a modified 313 | executable containing the modified Library. (It is understood 314 | that the user who changes the contents of definitions files in the 315 | Library will not necessarily be able to recompile the application 316 | to use the modified definitions.) 317 | 318 | b) Use a suitable shared library mechanism for linking with the 319 | Library. A suitable mechanism is one that (1) uses at run time a 320 | copy of the library already present on the user's computer system, 321 | rather than copying library functions into the executable, and (2) 322 | will operate properly with a modified version of the library, if 323 | the user installs one, as long as the modified version is 324 | interface-compatible with the version that the work was made with. 325 | 326 | c) Accompany the work with a written offer, valid for at 327 | least three years, to give the same user the materials 328 | specified in Subsection 6a, above, for a charge no more 329 | than the cost of performing this distribution. 330 | 331 | d) If distribution of the work is made by offering access to copy 332 | from a designated place, offer equivalent access to copy the above 333 | specified materials from the same place. 334 | 335 | e) Verify that the user has already received a copy of these 336 | materials or that you have already sent this user a copy. 337 | 338 | For an executable, the required form of the "work that uses the 339 | Library" must include any data and utility programs needed for 340 | reproducing the executable from it. However, as a special exception, 341 | the materials to be distributed need not include anything that is 342 | normally distributed (in either source or binary form) with the major 343 | components (compiler, kernel, and so on) of the operating system on 344 | which the executable runs, unless that component itself accompanies 345 | the executable. 346 | 347 | It may happen that this requirement contradicts the license 348 | restrictions of other proprietary libraries that do not normally 349 | accompany the operating system. Such a contradiction means you cannot 350 | use both them and the Library together in an executable that you 351 | distribute. 352 | 353 | 7. You may place library facilities that are a work based on the 354 | Library side-by-side in a single library together with other library 355 | facilities not covered by this License, and distribute such a combined 356 | library, provided that the separate distribution of the work based on 357 | the Library and of the other library facilities is otherwise 358 | permitted, and provided that you do these two things: 359 | 360 | a) Accompany the combined library with a copy of the same work 361 | based on the Library, uncombined with any other library 362 | facilities. This must be distributed under the terms of the 363 | Sections above. 364 | 365 | b) Give prominent notice with the combined library of the fact 366 | that part of it is a work based on the Library, and explaining 367 | where to find the accompanying uncombined form of the same work. 368 | 369 | 8. You may not copy, modify, sublicense, link with, or distribute 370 | the Library except as expressly provided under this License. Any 371 | attempt otherwise to copy, modify, sublicense, link with, or 372 | distribute the Library is void, and will automatically terminate your 373 | rights under this License. However, parties who have received copies, 374 | or rights, from you under this License will not have their licenses 375 | terminated so long as such parties remain in full compliance. 376 | 377 | 9. You are not required to accept this License, since you have not 378 | signed it. However, nothing else grants you permission to modify or 379 | distribute the Library or its derivative works. These actions are 380 | prohibited by law if you do not accept this License. Therefore, by 381 | modifying or distributing the Library (or any work based on the 382 | Library), you indicate your acceptance of this License to do so, and 383 | all its terms and conditions for copying, distributing or modifying 384 | the Library or works based on it. 385 | 386 | 10. Each time you redistribute the Library (or any work based on the 387 | Library), the recipient automatically receives a license from the 388 | original licensor to copy, distribute, link with or modify the Library 389 | subject to these terms and conditions. You may not impose any further 390 | restrictions on the recipients' exercise of the rights granted herein. 391 | You are not responsible for enforcing compliance by third parties with 392 | this License. 393 | 394 | 11. If, as a consequence of a court judgment or allegation of patent 395 | infringement or for any other reason (not limited to patent issues), 396 | conditions are imposed on you (whether by court order, agreement or 397 | otherwise) that contradict the conditions of this License, they do not 398 | excuse you from the conditions of this License. If you cannot 399 | distribute so as to satisfy simultaneously your obligations under this 400 | License and any other pertinent obligations, then as a consequence you 401 | may not distribute the Library at all. For example, if a patent 402 | license would not permit royalty-free redistribution of the Library by 403 | all those who receive copies directly or indirectly through you, then 404 | the only way you could satisfy both it and this License would be to 405 | refrain entirely from distribution of the Library. 406 | 407 | If any portion of this section is held invalid or unenforceable under any 408 | particular circumstance, the balance of the section is intended to apply, 409 | and the section as a whole is intended to apply in other circumstances. 410 | 411 | It is not the purpose of this section to induce you to infringe any 412 | patents or other property right claims or to contest validity of any 413 | such claims; this section has the sole purpose of protecting the 414 | integrity of the free software distribution system which is 415 | implemented by public license practices. Many people have made 416 | generous contributions to the wide range of software distributed 417 | through that system in reliance on consistent application of that 418 | system; it is up to the author/donor to decide if he or she is willing 419 | to distribute software through any other system and a licensee cannot 420 | impose that choice. 421 | 422 | This section is intended to make thoroughly clear what is believed to 423 | be a consequence of the rest of this License. 424 | 425 | 12. If the distribution and/or use of the Library is restricted in 426 | certain countries either by patents or by copyrighted interfaces, the 427 | original copyright holder who places the Library under this License may add 428 | an explicit geographical distribution limitation excluding those countries, 429 | so that distribution is permitted only in or among countries not thus 430 | excluded. In such case, this License incorporates the limitation as if 431 | written in the body of this License. 432 | 433 | 13. The Free Software Foundation may publish revised and/or new 434 | versions of the Lesser General Public License from time to time. 435 | Such new versions will be similar in spirit to the present version, 436 | but may differ in detail to address new problems or concerns. 437 | 438 | Each version is given a distinguishing version number. If the Library 439 | specifies a version number of this License which applies to it and 440 | "any later version", you have the option of following the terms and 441 | conditions either of that version or of any later version published by 442 | the Free Software Foundation. If the Library does not specify a 443 | license version number, you may choose any version ever published by 444 | the Free Software Foundation. 445 | 446 | 14. If you wish to incorporate parts of the Library into other free 447 | programs whose distribution conditions are incompatible with these, 448 | write to the author to ask for permission. For software which is 449 | copyrighted by the Free Software Foundation, write to the Free 450 | Software Foundation; we sometimes make exceptions for this. Our 451 | decision will be guided by the two goals of preserving the free status 452 | of all derivatives of our free software and of promoting the sharing 453 | and reuse of software generally. 454 | 455 | NO ARRANTY 456 | 457 | 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO 458 | WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. 459 | EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR 460 | OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY 461 | KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 462 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 463 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE 464 | LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME 465 | THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 466 | 467 | 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN 468 | WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY 469 | AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU 470 | FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR 471 | CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE 472 | LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING 473 | RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A 474 | FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF 475 | SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH 476 | DAMAGES. 477 | -------------------------------------------------------------------------------- /OUTPUTS.md: -------------------------------------------------------------------------------- 1 | ### File structure outputs 2 | 3 | #### x_calib.npy 4 | 5 | Learned bone lengths and relative marker positions as well as pose variables (i.e. bone rotations and translations) for all time points of the sequence, which is used for learning the animal's anatomy. The format is: 6 | ``` 7 | np.ndarray, shape=(nBones + 3*nMarkers + (3*nBones+3)*nFrames_calib,), dtype('float64'), 8 | ``` 9 | Thus, the individual variables are stored in `x_calib.npy` according to: 10 | ``` 11 | x_calib[0 : nBones] # bone lengths 12 | x_calib[nBones : nBones+3*nMarkers] # relative 3D marker positions 13 | x_calib[nBones+3*nMarkers : nBones+3*nMarkers+(3*nBones+3)*1] # bone rotations and translation for time point 1 14 | x_calib[nBones+3*nMarkers+(3*nBones+3)*1 : nBones+3*nMarkers+(3*nBones+3)*2] # bone rotations and translation for time point 2 15 | x_calib[nBones+3*nMarkers+(3*nBones+3)*2 : nBones+3*nMarkers+(3*nBones+3)*3] # bone rotations and translation for time point 3 16 | . 17 | . 18 | . 19 | x_calib[nBones+3*nMarkers+(3*nBones+3)*(nFrames_calib-1) : nBones+3*nMarkers+(3*nBones+3)*nFrames_calib] # bone rotations and translation for time point nFrames_calib 20 | ``` 21 | Note that bone lengths entries for right-sided bones are always zero, as the actual values are copied from the corresponding left-sided bone lengths entries to enforce symmetry. 22 | 23 | #### x_ini.npy 24 | 25 | Learned bone lengths and relative marker positions as well as pose variables (i.e. bone rotations and a single translation) for the first time point of the sequence, which should be reconstructed. The format is: 26 | ``` 27 | np.ndarray, shape=(nBones + 3*nMarkers + 3*nBones+3,), dtype('float64'), 28 | ``` 29 | Thus, the individual variables are stored in `x_ini.npy` according to: 30 | ``` 31 | x_ini[0 : nBones] # bone lengths 32 | x_ini[nBones : nBones+3*nMarkers] # relative 3D marker positions 33 | x_ini[nBones+3*nMarkers : nBones+3*nMarkers+(3*nBones+3)] # bone rotations and translation 34 | ``` 35 | Thus, the first `nBones+3*nMarkers` entries of `x_ini.npy` are identical to `x_calib.npy`. 36 | 37 | #### save_dict.npy 38 | 39 | Learned model parameters as well as resulting latent variables and corresponding covariance matrices for the entire sequence. If temporal constraints are enforced (i.e. `mode=4` or `mode=3` in `calibration.py`), the format is: 40 | ``` 41 | dict( 42 | # learned transition matrix (legacy: this is fixed to the identity matrix) # LEGACY 43 | 'A': np.ndarray, shape=(nLatent, nLatent,), dtype('float64'), # LEGACY 44 | # learned inital state of the latent variables 45 | 'mu0': np.ndarray, shape=(nLatent,), dtype('float64'), 46 | # inferred latent variables (inferrence is based on the learned model parameters) 47 | 'mu_uks': np.ndarray, shape=(nFrames+1, nLatent,), dtype('float64'), 48 | # learned initial covariance matrix of the latent variables 49 | 'var0': np.ndarray, shape=(nLatent, nLatent,), dtype('float64'), 50 | # learned covariance matrix of the transition noise 51 | 'var_f': np.ndarray, shape=(nLatent, nLatent,), dtype('float64'), 52 | # learned covariance matrix of the measurement noise 53 | 'var_g': np.ndarray, shape=(nMeasurement, nMeasurement,), dtype('float64'), 54 | # inferred covariance matrices of the latent variables (inferrence is based on the learned model parameters) 55 | 'var_uks': np.ndarray, shape=(nFrames+1, nLatent, nLatent,), dtype('float64'), 56 | ) 57 | ``` 58 | If only anatomical or no constraints are enforced (i.e. `mode=2` or `mode=1` in `calibration.py`), the format is: 59 | ``` 60 | dict( 61 | # inferred latent variables (inferrence is based on the learned model parameters; mu_fit[0] is filled with nan's, since it corresponds to mu0) 62 | 'mu_fit': np.ndarray, shape=(nFrames+1, nLatent,), dtype('float64'), 63 | ) 64 | ``` 65 | 66 | #### pose.npy 67 | 68 | Inferred 3D joint and marker positions as well as resulting projected marker locations in the 2D images. The format is: 69 | ``` 70 | dict( 71 | # reconstructed marker locations in 2D image 72 | 'marker_positions_2d': np.ndarray, shape=(nFrames, nCameras, nMarkers, 2,), dtype('float64'), 73 | # reconstructed marker positions in 3D 74 | 'marker_positions_3d': np.ndarray, shape=(nFrames, nMarkers, 3,), dtype('float64'), 75 | # reconstructed joint positions in 3D 76 | 'joint_positions_3d': np.ndarray, shape=(nFrames, nJoints, 3,), dtype('float64'), 77 | ) 78 | ``` -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ACM (Anatomically-constrained model) 2 | A framework for videography based pose tracking of rodents. 3 | By Arne Monsees. [In review] 4 | 5 | ## Installation 6 | 7 | 1. [Install Anaconda](https://docs.anaconda.com/anaconda/install/) 8 | 2. Clone https://github.com/bbo-lab/ACM.git 9 | 3. Start Anaconda Prompt (Windows) / terminal (linux) and navigate into repository directory 10 | 4. Create conda environment `conda env create -f https://raw.githubusercontent.com/bbo-lab/ACM/main/environment.yml` once repo is public) 11 | 6. Install using `pip install .` 12 | 13 | ## Testing 14 | 15 | 1. Download [example dataset](https://www.dropbox.com/sh/040587pwx5t7uh3/AAAI5MVilFrJY-mEPr97uADNa?dl=0). 16 | 2. Start Anaconda Prompt (Windows) / terminal (linux) 17 | 3. Activate conda environment `conda activate bbo_acm`. 18 | 4. View 2D labels by running `python -m ACM --viewer [Path of "table_1_20210511" folder from step 1.]` 19 | 5. Run `python -m ACM [Path of "table_1_20210511" folder from step 1.]` (expected to take around 20 minutes on modern 8-core CPU, tested with an AMD Vega iGPU and a Geforce GTX 1080 Ti) 20 | 6. View 3D tracked pose by running `python -m ACM --viewer [Path of "table_1_20210511" folder from step 1.]` 21 | 22 | ## Setting up your own dataset config 23 | 24 | ### Overview 25 | 26 | #### Input 27 | 28 | A dataset input config consists of a folder with the following files: 29 | 30 | - `configuration.py`: Configuration file that determines settings and input and out paths. 31 | - `model.npy`: Defines the underlying skeleton graph of the animal. 32 | - `multicalibration.npy`: Intrinsic and extrinsic camera parameters obtained from calibrating the camera setup. 33 | - `origin_coord.npy`: Coordinate system of the final result, relative to the coordinate system of the camera calibration. 34 | - `labels_dlc_n_m.npy`: Automatically detected labels on the videography data. 35 | - `labels_manual.npz`: Manual labels on the videography data. The labels are required for training a neural network for automated detection of surface markers as well as for learning the animal's antaomy via calibration.py. 36 | 37 | #### Output 38 | 39 | A dataset output consists of a folder with the following files: 40 | 41 | - `pose.npy`: A dictionary containing the inferred 3D joint and marker locations as well as the resulting projected marker locations in the 2D images. 42 | - `save_dict.npy`: A dictionary containing the model parameters learned with the EM algorithm (i.e. the initial state and covariance matrix as well as the transition and measurement covariance matrix). Additionally, it also contains the smoothed latent variables and covariance matrices for every time point of the reconstructed sequence, which were generated using the learned model parameters. 43 | 44 | Further, the following intermediate files are generated by calibration and initialization: 45 | 46 | - `x_calib.npy`: A flattened vector containing the latent variables encoding the learned bone lengths, surface marker locations and poses for all time points of the calibration data set, used to learn the animal's anatomy. For the subsequent pipeline only the first few entries of the flattened vector, corresponding to the bone lengths and surface marker locations, are relevant. 47 | - `x_ini.npy`: A flattened vector containing the latent variables encoding the learned bone lengths, surface marker locations and poses for the first time point of the sequence, which should be reconstructed. 48 | 49 | By default, results are saved into the `results` subfolder of the datset configuration. 50 | 51 | #### File structure of input and output files 52 | 53 | The structure of the inputs is documented in [INPUTS.md](https://github.com/bbo-lab/ACM/blob/main/INPUTS.md). The structure of the outputs is documented in [OUTPUTS.md](https://github.com/bbo-lab/ACM/blob/main/OUTPUTS.md). 54 | 55 | ### [FAQ](https://github.com/bbo-lab/ACM/blob/main/FAQ.md) -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: bbo_acm 2 | channels: 3 | - pytorch 4 | - defaults 5 | dependencies: 6 | - numpy 7 | - scipy 8 | - python=3.6 9 | - pytorch=1.9.0 10 | - imageio 11 | - matplotlib 12 | - pip 13 | - pip: 14 | - argparse 15 | - pyqt5 16 | - bbo-ccvtools 17 | - bbo-acm 18 | -------------------------------------------------------------------------------- /example_config/example_dataset/configuration.py: -------------------------------------------------------------------------------- 1 | import os 2 | from datetime import datetime 3 | 4 | # Frames present in the labels file, used to generate file name 5 | dlc_index_frame_start = 6900 6 | dlc_index_frame_end = 184000 7 | 8 | # Frames to actuallly process 9 | index_frame_start = 45800 10 | index_frame_end = 46800 11 | 12 | # MODE 13 | # 1: deterministic model 14 | # 2: deterministic model + joint angle limits 15 | # 3: probabilistic model 16 | # 4: probabilistic model + joint angle limits 17 | mode = 4 # should always be 4 for now 18 | 19 | # probability cutoff (labels with a smaller probability than pcutoff are not used for the pose reconstruction) 20 | pcutoff = 0.9 21 | # slope of the custom clipping function 22 | slope = 1.0 23 | # initial values of the covariance matrices' diagonal entries 24 | noise = 1e-4 25 | 26 | # videos [optional, for viewer only] 27 | videos = [ 'cam1_20210511_table_10.ccv.mp4', 'cam2_20210511_table_10.ccv.mp4', 'cam3_20210511_table_10.ccv.mp4', 'cam4_20210511_table_10.ccv.mp4' ] 28 | 29 | # camera calibration scaling factor (calibration board square size -> cm) 30 | # TODO This should really be in the calibration file 31 | scale_factor = 1 # [cm] 32 | # whether to use rodrigues parameterization for all joint angles 33 | use_rodrigues = True # should always be True 34 | # whether to use reparameterization of joint angles 35 | use_reparameterization = False # should always be False 36 | # numerical tolerance 37 | num_tol = 2**-52 38 | 39 | # NORMALIZATION 40 | normalize_bone_lengths = 1.0 # e.g. 1.0 [cm] 41 | normalize_joint_marker_vec = 1.0 # e.g. 1.0 [cm] 42 | normalize_t0 = 50.0 # e.g. distance from arena origin to furthest arena boundary [cm] 43 | normalize_r0 = 1.5707963267948966 # e.g. pi/2 [rad] 44 | normalize_camera_sensor_x = 1280.0 # e.g. camera sensor size (width) [px] 45 | normalize_camera_sensor_y = 1024.0 # e.g. camera sensor size (height) [px] 46 | 47 | # CALIBRATION 48 | plot_calib = False 49 | sigma_factor = 10.0 50 | body_weight = 72.0 # [g] 51 | dFrames_calib = 200 52 | index_frames_calib = 'all' 53 | # 54 | opt_method_calib = 'L-BFGS-B' 55 | opt_options_calib__disp = False 56 | opt_options_calib__ftol = 2**-23 # scipy default value: 2.220446049250313e-09 57 | opt_options_calib__gtol = 0.0 # scipy default value: 1e-05 58 | opt_options_calib__maxiter = float('inf') 59 | opt_options_calib__maxcor = 100 # scipy default value: 10 60 | opt_options_calib__maxfun = float('inf') 61 | opt_options_calib__iprint = -1 62 | opt_options_calib__maxls = 200 # scipy default value: 20 63 | 64 | # INITIALIZATION 65 | plot_ini = False 66 | index_frame_ini = index_frame_start 67 | # 68 | opt_method_ini = 'L-BFGS-B' 69 | opt_options_ini__disp = False 70 | opt_options_ini__ftol = 2**-23 # scipy default value: 2.220446049250313e-09 71 | opt_options_ini__gtol = 0.0 # scipy default value: 1e-05 72 | opt_options_ini__maxiter = float('inf') 73 | opt_options_ini__maxcor = 100 # scipy default value: 10 74 | opt_options_ini__maxfun = float('inf') 75 | opt_options_ini__iprint = -1 76 | opt_options_ini__maxls = 200 # scipy default value: 20 77 | 78 | # POSE RECONSTRUCTION 79 | plot_recon = False 80 | dt = 5 # number of time points to skip between frames 81 | nT = int((index_frame_end - index_frame_start) / dt) # number of time points to be reconstruced 82 | # for mode 1 & 2: 83 | opt_method_fit = opt_method_ini 84 | opt_options_fit__disp = False 85 | opt_options_fit__ftol = 2**-23 # scipy default value: 2.220446049250313e-09 86 | opt_options_fit__gtol = opt_options_ini__gtol # scipy default value: 1e-05 87 | opt_options_fit__maxiter = opt_options_ini__maxiter 88 | opt_options_fit__maxcor = opt_options_ini__maxcor 89 | opt_options_fit__maxfun = opt_options_ini__maxfun 90 | opt_options_fit__iprint = opt_options_ini__iprint 91 | opt_options_fit__maxls = opt_options_ini__maxls 92 | # for mode 3 & 4: 93 | use_cuda = False # running code on the CPU generally seems to be faster 94 | slow_mode = True # set to True to lower memory requirements (should not be changed for now) 95 | sigma_point_scheme = 3 # # UKF3: 3, UKF5: 5, naive: 0 (should always be 3) 96 | tol = 5e-2 # tolerance for convergence 97 | iter_max = 100 # maximum number of EM iterations 98 | 99 | # ASSERTS 100 | assert dlc_index_frame_start<=index_frame_start and dlc_index_frame_end>=index_frame_end, "Requested frame range not in label range." 101 | 102 | # DEFINE PATHS 103 | folder_project = os.path.dirname(os.path.realpath(__file__)) 104 | job_name = os.path.basename(folder_project) 105 | job_time = datetime.now() 106 | 107 | # folder to save initialization, calibration and pose reconstruction to 108 | folder_save = os.path.join(folder_project, 'results', f'{job_name}_{job_time.strftime("%Y%m%d-%H%M%S")}') 109 | folder_init = folder_save 110 | folder_calib = folder_save 111 | 112 | # add path to video filenames 113 | videos = [ os.path.join(folder_project,video) for video in videos ] 114 | 115 | # define location of base folder 116 | folder_reqFiles = folder_project 117 | 118 | # define location of all needed files 119 | file_origin_coord = os.path.realpath(os.path.join(folder_reqFiles, 'origin_coord.npy')) 120 | file_calibration = os.path.realpath(os.path.join(folder_reqFiles, 'multicalibration.npy')) 121 | file_model = os.path.realpath(os.path.join(folder_reqFiles, 'model.npy')) 122 | file_labelsDLC = os.path.realpath(os.path.join(folder_reqFiles, f'labels_dlc_{dlc_index_frame_start:06}_{dlc_index_frame_end:06}.npy')) 123 | file_labelsManual = os.path.realpath(os.path.join(folder_reqFiles, 'labels_manual.npz')) # only needed for calibration 124 | -------------------------------------------------------------------------------- /example_config/example_dataset/labels_dlc_006900_184000.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/example_config/example_dataset/labels_dlc_006900_184000.npy -------------------------------------------------------------------------------- /example_config/example_dataset/labels_manual.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/example_config/example_dataset/labels_manual.npz -------------------------------------------------------------------------------- /example_config/example_dataset/model.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/example_config/example_dataset/model.npy -------------------------------------------------------------------------------- /example_config/example_dataset/multicalibration.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/example_config/example_dataset/multicalibration.npy -------------------------------------------------------------------------------- /example_config/example_dataset/origin_coord.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bbo-lab/ACM/3d68e9507d5b222b928d7c3e6b5407565f82db41/example_config/example_dataset/origin_coord.npy -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | from setuptools import find_packages, setup 3 | # The directory containing this file 4 | HERE = pathlib.Path(__file__).parent 5 | 6 | # The text of the README file 7 | README = (HERE / "README.md").read_text() 8 | 9 | VERSION = "0.2.4" 10 | 11 | # This call to setup() does all the work 12 | setup( 13 | name="bbo-acm", 14 | version=VERSION, 15 | description="Anatomically constraint pose reconstruction from video data", 16 | long_description=README, 17 | long_description_content_type="text/markdown", 18 | url="https://github.com/bbo-lab/ACM", 19 | author="Arne Monsees, BBO lab", 20 | author_email="bbo-admin@caesar.de", 21 | license="LGPLv2+", 22 | classifiers=[ 23 | "License :: OSI Approved :: GNU Lesser General Public License v2 or later (LGPLv2+)", 24 | "Programming Language :: Python :: 3", 25 | "Programming Language :: Python :: 3.6", 26 | "Programming Language :: Python :: 3.7", 27 | "Programming Language :: Python :: 3.8", 28 | ], 29 | packages=['ACM','ACM.gui'], 30 | include_package_data=True, 31 | install_requires=["numpy", "torch", 'scipy', 'argparse'], 32 | ) 33 | --------------------------------------------------------------------------------