From 65e675ab64eaf3a98d8d2a2d188fa01c2806b55c Mon Sep 17 00:00:00 2001 From: davidpagnon Date: Fri, 20 Sep 2024 02:23:13 +0200 Subject: [PATCH] beta test opensim scaling and kinematics with edits --- Pose2Sim/Demo_Batch/Config.toml | 15 +- Pose2Sim/Demo_Batch/Trial_1/Config.toml | 15 +- Pose2Sim/Demo_Batch/Trial_2/Config.toml | 19 +- Pose2Sim/Demo_MultiPerson/Config.toml | 19 +- Pose2Sim/Demo_SinglePerson/Config.toml | 23 +- .../OpenSim_Setup/Model_Pose2Sim_Coco133.osim | 2 +- .../OpenSim_Setup/Model_Pose2Sim_Halpe26.osim | 4 +- .../Model_Pose2Sim_Halpe68_136.osim | 2 +- Pose2Sim/Pose2Sim.py | 2 +- Pose2Sim/common.py | 34 +- Pose2Sim/kinematics.py | 822 ++++++++++-------- Pose2Sim/markerAugmentation.py | 35 +- README.md | 33 +- 13 files changed, 554 insertions(+), 471 deletions(-) diff --git a/Pose2Sim/Demo_Batch/Config.toml b/Pose2Sim/Demo_Batch/Config.toml index 4960c9c..f25e3df 100644 --- a/Pose2Sim/Demo_Batch/Config.toml +++ b/Pose2Sim/Demo_Batch/Config.toml @@ -136,7 +136,6 @@ calibration_type = 'convert' # 'convert' or 'calculate' [triangulation] -reorder_trc = false # only checked if multi_person analysis reproj_error_threshold_triangulation = 15 # px likelihood_threshold_triangulation= 0.3 min_cameras_for_triangulation = 2 @@ -181,14 +180,10 @@ make_c3d = true # save triangulated data in c3d format in addition to trc [opensim] -static_trial = ['S00_P00_Participant/S00_P00_T00_StaticTrial'] -# # If this Config.toml file is at the Trial level, set to true or false (lowercase); -# # At the Participant level, specify the name of the static trial folder name, e.g. ['S00_P00_T00_StaticTrial']; -# # At the Session level, add participant subdirectory, e.g. ['S00_P00_Participant/S00_P00_T00_StaticTrial', 'S00_P01_Participant/S00_P00_T00_StaticTrial'] -opensim_bin_path = 'C:\OpenSim 4.4\bin' -use_augmentation = false # If using augmented measurements then set it true -load_trc_name = 'filtered' # 'default' or 'filtered', if use_augmentation = true, this line will be ignored instead using __LSTM.trc -IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] +use_augmentation = true # true or false (lowercase) # Set to true if you want to use the model with augmented markers +right_left_symmetry = true # true or false (lowercase) # Set to false only if you have good reasons to think the participant is not symmetrical (e.g. prosthetic limb) +remove_individual_scaling_setup = true # true or false (lowercase) # If true, the individual scaling setup files are removed to avoid cluttering +remove_individual_IK_setup = true # true or false (lowercase) # If true, the individual IK setup files are removed to avoid cluttering @@ -204,7 +199,7 @@ IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] # Check your model hierarchy with: for pre, _, node in RenderTree(model): # print(f'{pre}{node.name} id={node.id}') [pose.CUSTOM] -name = "CHip" +name = "Hip" id = "19" [[pose.CUSTOM.children]] name = "RHip" diff --git a/Pose2Sim/Demo_Batch/Trial_1/Config.toml b/Pose2Sim/Demo_Batch/Trial_1/Config.toml index ab4b395..d18a7d5 100644 --- a/Pose2Sim/Demo_Batch/Trial_1/Config.toml +++ b/Pose2Sim/Demo_Batch/Trial_1/Config.toml @@ -136,7 +136,6 @@ # [triangulation] -# reorder_trc = false # only checked if multi_person analysis # reproj_error_threshold_triangulation = 15 # px # likelihood_threshold_triangulation= 0.3 # min_cameras_for_triangulation = 2 @@ -181,14 +180,10 @@ # [opensim] -#static_trial = ['S00_P00_Participant/S00_P00_T00_StaticTrial'] -# # If this Config.toml file is at the Trial level, set to true or false (lowercase); -# # At the Participant level, specify the name of the static trial folder name, e.g. ['S00_P00_T00_StaticTrial']; -# # At the Session level, add participant subdirectory, e.g. ['S00_P00_Participant/S00_P00_T00_StaticTrial', 'S00_P01_Participant/S00_P00_T00_StaticTrial'] -#opensim_bin_path = 'C:\OpenSim 4.4\bin' -#use_augmentation = false # If using augmented measurements then set it true -#load_trc_name = 'filtered' # 'default' or 'filtered', if use_augmentation = true, this line will be ignored instead using __LSTM.trc -#IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] +# use_augmentation = true # true or false (lowercase) # Set to true if you want to use the model with augmented markers +# right_left_symmetry = true # true or false (lowercase) # Set to false only if you have good reasons to think the participant is not symmetrical (e.g. prosthetic limb) +# remove_individual_scaling_setup = true # true or false (lowercase) # If true, the individual scaling setup files are removed to avoid cluttering +# remove_individual_IK_setup = true # true or false (lowercase) # If true, the individual IK setup files are removed to avoid cluttering @@ -204,7 +199,7 @@ # # Check your model hierarchy with: for pre, _, node in RenderTree(model): # # print(f'{pre}{node.name} id={node.id}') # [pose.CUSTOM] -# name = "CHip" +# name = "Hip" # id = "19" # [[pose.CUSTOM.children]] # name = "RHip" diff --git a/Pose2Sim/Demo_Batch/Trial_2/Config.toml b/Pose2Sim/Demo_Batch/Trial_2/Config.toml index 6cfcc94..7dc68ee 100644 --- a/Pose2Sim/Demo_Batch/Trial_2/Config.toml +++ b/Pose2Sim/Demo_Batch/Trial_2/Config.toml @@ -19,8 +19,8 @@ [project] multi_person = true # true for trials with multiple participants. If false, only the main person in scene is analyzed (and it run much faster). -participant_height = [1.72, 1.40, 1.86] # m # float if single person, list of float if multi-person (same order as the Static trials) # Only used for marker augmentation -participant_mass = [70.0, 63.5, 88.0] # kg # Only used for marker augmentation and scaling +participant_height = [1.72, 1.40] # m # float if single person, list of float if multi-person (same order as the Static trials) # Only used for marker augmentation +participant_mass = [70.0, 63.5] # kg # Only used for marker augmentation and scaling # frame_rate = 'auto' # fps # int or 'auto'. If 'auto', finds from video (or defaults to 60 fps if you work with images) # frame_range = [] # For example [10,300], or [] for all frames. @@ -136,7 +136,6 @@ keypoints_to_consider = 'all' # 'all' if all points should be considered, for ex # [triangulation] -# reorder_trc = false # only checked if multi_person analysis # reproj_error_threshold_triangulation = 15 # px # likelihood_threshold_triangulation= 0.3 # min_cameras_for_triangulation = 2 @@ -181,14 +180,10 @@ keypoints_to_consider = 'all' # 'all' if all points should be considered, for ex # [opensim] -#static_trial = ['S00_P00_Participant/S00_P00_T00_StaticTrial'] -# # If this Config.toml file is at the Trial level, set to true or false (lowercase); -# # At the Participant level, specify the name of the static trial folder name, e.g. ['S00_P00_T00_StaticTrial']; -# # At the Session level, add participant subdirectory, e.g. ['S00_P00_Participant/S00_P00_T00_StaticTrial', 'S00_P01_Participant/S00_P00_T00_StaticTrial'] -#opensim_bin_path = 'C:\OpenSim 4.4\bin' -#use_augmentation = false # If using augmented measurements then set it true -#load_trc_name = 'filtered' # 'default' or 'filtered', if use_augmentation = true, this line will be ignored instead using __LSTM.trc -#IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] +# use_augmentation = true # true or false (lowercase) # Set to true if you want to use the model with augmented markers +# right_left_symmetry = true # true or false (lowercase) # Set to false only if you have good reasons to think the participant is not symmetrical (e.g. prosthetic limb) +# remove_individual_scaling_setup = true # true or false (lowercase) # If true, the individual scaling setup files are removed to avoid cluttering +# remove_individual_IK_setup = true # true or false (lowercase) # If true, the individual IK setup files are removed to avoid cluttering @@ -204,7 +199,7 @@ keypoints_to_consider = 'all' # 'all' if all points should be considered, for ex # # Check your model hierarchy with: for pre, _, node in RenderTree(model): # # print(f'{pre}{node.name} id={node.id}') # [pose.CUSTOM] -# name = "CHip" +# name = "Hip" # id = "19" # [[pose.CUSTOM.children]] # name = "RHip" diff --git a/Pose2Sim/Demo_MultiPerson/Config.toml b/Pose2Sim/Demo_MultiPerson/Config.toml index a4d0e29..64c3039 100644 --- a/Pose2Sim/Demo_MultiPerson/Config.toml +++ b/Pose2Sim/Demo_MultiPerson/Config.toml @@ -19,8 +19,8 @@ [project] multi_person = true # true for trials with multiple participants. If false, only the main person in scene is analyzed (and it run much faster). -participant_height = [1.72, 1.40, 1.90] # m # float if single person, list of float if multi-person (same order as the Static trials) # Only used for marker augmentation -participant_mass = [70.0, 63.5, 90.0] # kg # Only used for marker augmentation and scaling +participant_height = [1.72, 1.40] # m # float if single person, list of float if multi-person (same order as the Static trials) # Only used for marker augmentation +participant_mass = [70.0, 63.5] # kg # Only used for marker augmentation and scaling frame_rate = 'auto' # fps # int or 'auto'. If 'auto', finds from video (or defaults to 60 fps if you work with images) frame_range = [] # For example [10,300], or [] for all frames. @@ -136,7 +136,6 @@ calibration_type = 'convert' # 'convert' or 'calculate' [triangulation] -reorder_trc = false # only checked if multi_person analysis reproj_error_threshold_triangulation = 15 # px likelihood_threshold_triangulation= 0.3 min_cameras_for_triangulation = 2 @@ -181,14 +180,10 @@ make_c3d = true # save triangulated data in c3d format in addition to trc [opensim] -static_trial = ['S00_P00_Participant/S00_P00_T00_StaticTrial'] -# # If this Config.toml file is at the Trial level, set to true or false (lowercase); -# # At the Participant level, specify the name of the static trial folder name, e.g. ['S00_P00_T00_StaticTrial']; -# # At the Session level, add participant subdirectory, e.g. ['S00_P00_Participant/S00_P00_T00_StaticTrial', 'S00_P01_Participant/S00_P00_T00_StaticTrial'] -opensim_bin_path = 'C:\OpenSim 4.4\bin' -use_augmentation = false # If using augmented measurements then set it true -load_trc_name = 'filtered' # 'default' or 'filtered', if use_augmentation = true, this line will be ignored instead using __LSTM.trc -IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] +use_augmentation = true # true or false (lowercase) # Set to true if you want to use the model with augmented markers +right_left_symmetry = true # true or false (lowercase) # Set to false only if you have good reasons to think the participant is not symmetrical (e.g. prosthetic limb) +remove_individual_scaling_setup = true # true or false (lowercase) # If true, the individual scaling setup files are removed to avoid cluttering +remove_individual_IK_setup = true # true or false (lowercase) # If true, the individual IK setup files are removed to avoid cluttering @@ -204,7 +199,7 @@ IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] # Check your model hierarchy with: for pre, _, node in RenderTree(model): # print(f'{pre}{node.name} id={node.id}') [pose.CUSTOM] -name = "CHip" +name = "Hip" id = "19" [[pose.CUSTOM.children]] name = "RHip" diff --git a/Pose2Sim/Demo_SinglePerson/Config.toml b/Pose2Sim/Demo_SinglePerson/Config.toml index 1c4ec8d..44eddb7 100644 --- a/Pose2Sim/Demo_SinglePerson/Config.toml +++ b/Pose2Sim/Demo_SinglePerson/Config.toml @@ -53,9 +53,9 @@ output_format = 'openpose' # 'openpose', 'mmpose', 'deeplabcut', 'none' or a lis [synchronization] display_sync_plots = true # true or false (lowercase) keypoints_to_consider = ['RWrist'] # 'all' if all points should be considered, for example if the participant did not perform any particicular sharp movement. In this case, the capture needs to be 5-10 seconds long at least - # ['RWrist', 'RElbow'] list of keypoint names if you want to specify the keypoints to consider. + # ['RWrist', 'RElbow'] list of keypoint names if you want to specify keypoints with a sharp vertical motion. approx_time_maxspeed = 'auto' # 'auto' if you want to consider the whole capture (default, slower if long sequences) - # [10.0, 2.0, 8.0, 11.0] list of times in seconds, one value per camera if you want to specify the approximate time of a clear vertical event by one person standing alone in the scene + # [10.0, 2.0, 8.0, 11.0] list of times (seconds) if you want to specify the approximate time of a clear vertical event for each camera time_range_around_maxspeed = 2.0 # Search for best correlation in the range [approx_time_maxspeed - time_range_around_maxspeed, approx_time_maxspeed + time_range_around_maxspeed] likelihood_threshold = 0.4 # Keypoints whose likelihood is below likelihood_threshold are filtered out filter_cutoff = 6 # time series are smoothed to get coherent time-lagged correlation @@ -136,7 +136,6 @@ calibration_type = 'convert' # 'convert' or 'calculate' [triangulation] -reorder_trc = false # only checked if multi_person analysis reproj_error_threshold_triangulation = 15 # px likelihood_threshold_triangulation= 0.3 min_cameras_for_triangulation = 2 @@ -180,17 +179,11 @@ make_c3d = true # also save triangulated data in c3d format make_c3d = true # save triangulated data in c3d format in addition to trc -[opensim] -static_trial = ['S00_P00_Participant/S00_P00_T00_StaticTrial'] -# # If this Config.toml file is at the Trial level, set to true or false (lowercase); -# # At the Participant level, specify the name of the static trial folder name, e.g. ['S00_P00_T00_StaticTrial']; -# # At the Session level, add participant subdirectory, e.g. ['S00_P00_Participant/S00_P00_T00_StaticTrial', 'S00_P01_Participant/S00_P00_T00_StaticTrial'] -opensim_bin_path = 'C:\OpenSim 4.4\bin' -use_augmentation = false # If using augmented measurements then set it true -load_trc_name = 'filtered' # 'default' or 'filtered', if use_augmentation = true, this line will be ignored instead using __LSTM.trc -IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] - - +[kinematics] +use_augmentation = true # true or false (lowercase) # Set to true if you want to use the model with augmented markers +right_left_symmetry = true # true or false (lowercase) # Set to false only if you have good reasons to think the participant is not symmetrical (e.g. prosthetic limb) +remove_individual_scaling_setup = true # true or false (lowercase) # If true, the individual scaling setup files are removed to avoid cluttering +remove_individual_IK_setup = true # true or false (lowercase) # If true, the individual IK setup files are removed to avoid cluttering @@ -206,7 +199,7 @@ IK_timeRange = [] #left empty to IK full range or eg.[0.5,1.0] # Check your model hierarchy with: for pre, _, node in RenderTree(model): # print(f'{pre}{node.name} id={node.id}') [pose.CUSTOM] -name = "CHip" +name = "Hip" id = "19" [[pose.CUSTOM.children]] name = "RHip" diff --git a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Coco133.osim b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Coco133.osim index 299913e..5b69e03 100644 --- a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Coco133.osim +++ b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Coco133.osim @@ -1,6 +1,6 @@ - + diff --git a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe26.osim b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe26.osim index f58c506..a6de9ca 100644 --- a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe26.osim +++ b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe26.osim @@ -1,6 +1,6 @@ - + @@ -6808,7 +6808,7 @@ /bodyset/pelvis - -0.063927399999999995 0 0 + -0.063927399999999995 -0.081343112945556642 0 diff --git a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe68_136.osim b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe68_136.osim index 7da528c..3a8c108 100644 --- a/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe68_136.osim +++ b/Pose2Sim/OpenSim_Setup/Model_Pose2Sim_Halpe68_136.osim @@ -1,6 +1,6 @@ - + diff --git a/Pose2Sim/Pose2Sim.py b/Pose2Sim/Pose2Sim.py index c33c55c..05d23de 100644 --- a/Pose2Sim/Pose2Sim.py +++ b/Pose2Sim/Pose2Sim.py @@ -539,7 +539,7 @@ def kinematics(config=None): end = time.time() elapsed = end - start - logging.info(f'\nOpenSim scaling and inverse kinematics took {time.strftime("%Hh%Mm%Ss", time.gmtime(elapsed))}.\n') + logging.info(f'OpenSim scaling and inverse kinematics took {time.strftime("%Hh%Mm%Ss", time.gmtime(elapsed))}.\n') def runAll(config=None, do_calibration=True, do_poseEstimation=True, do_synchronization=True, do_personAssociation=True, do_triangulation=True, do_filtering=True, do_markerAugmentation=True, do_kinematics=True): diff --git a/Pose2Sim/common.py b/Pose2Sim/common.py index a3b8ca0..2c1d085 100644 --- a/Pose2Sim/common.py +++ b/Pose2Sim/common.py @@ -219,11 +219,11 @@ def euclidean_distance(q1, q2): INPUTS: - q1: list of N_dimensional coordinates of point + or list of N points of N_dimensional coordinates - q2: idem OUTPUTS: - euc_dist: float. Euclidian distance between q1 and q2 - ''' q1 = np.array(q1) @@ -233,11 +233,40 @@ def euclidean_distance(q1, q2): dist = np.empty_like(dist) dist[...] = np.inf - euc_dist = np.sqrt(np.nansum( [d**2 for d in dist])) + if len(dist.shape)==1: + euc_dist = np.sqrt(np.nansum( [d**2 for d in dist])) + else: + euc_dist = np.sqrt(np.nansum( [d**2 for d in dist], axis=1)) return euc_dist +def trimmed_mean(arr, trimmed_percent=0.5): + ''' + Trimmed mean calculation for an array. + + INPUTS: + - arr (np.array): The input array. + - trimmed_percent (float): The percentage of values to be trimmed from both ends. + + OUTPUTS: + - float: The trimmed mean of the array. + ''' + + # Sort the array + sorted_arr = np.sort(arr) + + # Determine the indices for the 25th and 75th percentiles (if trimmed_percent = 0.5) + lower_idx = int(len(sorted_arr) * (trimmed_percent/2)) + upper_idx = int(len(sorted_arr) * (1 - trimmed_percent/2)) + + # Slice the array to exclude the 25% lowest and highest values + trimmed_arr = sorted_arr[lower_idx:upper_idx] + + # Return the mean of the remaining values + return np.mean(trimmed_arr) + + def world_to_camera_persp(r, t): ''' Converts rotation R and translation T @@ -364,6 +393,7 @@ def natural_sort_key(s): Sorts list of strings with numbers in natural order (alphabetical and numerical) Example: ['item_1', 'item_2', 'item_10', 'stuff_1'] ''' + s=str(s) return [int(c) if c.isdigit() else c.lower() for c in re.split(r'(\d+)', s)] diff --git a/Pose2Sim/kinematics.py b/Pose2Sim/kinematics.py index bb96056..08d3dff 100644 --- a/Pose2Sim/kinematics.py +++ b/Pose2Sim/kinematics.py @@ -24,352 +24,213 @@ OUTPUT: - scaled OpenSim model files (.osim) - joint angle data files (.mot) ''' + + +## INIT import os import sys -from collections import defaultdict from pathlib import Path import numpy as np import pandas as pd from lxml import etree import logging +from anytree import PreOrderIter + import opensim +from Pose2Sim.common import natural_sort_key, euclidean_distance, trimmed_mean +from Pose2Sim.skeletons import * + + + +## AUTHORSHIP INFORMATION +__author__ = "Ivan Sun, David Pagnon" +__copyright__ = "Copyright 2021, Pose2Sim" +__credits__ = ["Ivan Sun", "David Pagnon"] +__license__ = "BSD 3-Clause License" +__version__ = "0.10.0" +__maintainer__ = "David Pagnon" +__email__ = "contact@david-pagnon.com" +__status__ = "Development" + ## FUNCTIONS -def find_config_and_pose3d(project_dir): - """ - Find configuration files and associated pose-3d directories in the project directory. - - Args: - project_dir (str): The root directory of the project. - - Returns: - list: A list of tuples containing the config path and the corresponding pose-3d directory. - """ - config_paths = [] - for root, dirs, files in os.walk(project_dir): - if 'Config.toml' in files: - config_path = Path(root) / 'Config.toml' - possible_pose3d_dir = Path(root) / 'pose-3d' - if not possible_pose3d_dir.exists(): - possible_pose3d_dir = Path(root).parent / 'pose-3d' - if possible_pose3d_dir.exists(): - config_paths.append((config_path, possible_pose3d_dir)) - else: - logging.warning(f"No pose-3d directory found for config: {config_path}") - return config_paths - - -def get_grouped_files(directory, pattern='*.trc'): - """ - Group TRC files by person ID or treat them as single-person if no ID is found. - - Args: - directory (str): The directory containing TRC files. - pattern (str): The file pattern to search for. - - Returns: - dict: A dictionary grouping TRC files by person ID. - """ - files = list(Path(directory).glob(pattern)) - grouped_files = defaultdict(list) - - for file in files: - parts = file.stem.split('_') - if len(parts) > 2 and 'P' in parts[2]: # Multi-person file naming convention - person_id = parts[2] - else: - person_id = "SinglePerson" - grouped_files[person_id].append(file) - - return grouped_files - - -def process_all_groups(config_dict): - """ - Process all groups (single or multi-person) based on the configuration. - - Args: - config_dict (dict): The configuration dictionary containing project details. - """ - logging.info("Processing all groups in the project.") - project_dir = config_dict.get('project', {}).get('project_dir') - config_and_pose3d_paths = find_config_and_pose3d(project_dir) - - for config_path, pose3d_dir in config_and_pose3d_paths: - logging.info(f"Processing setup with config: {config_path}") - - trc_groups = get_grouped_files(pose3d_dir) - trial_name = Path(pose3d_dir).parent.name # Use the parent directory name as the trial name - - for person_id, trc_files in trc_groups.items(): - filtered_trc_files = load_trc(config_dict, trc_files) - - # Ensure output directory includes the trial name - trial_output_dir = get_output_dir(Path(config_dict['project']['project_dir']).parent / trial_name, person_id) - perform_scaling(config_dict, person_id, filtered_trc_files, trial_output_dir) - perform_inverse_kinematics(config_dict, person_id, filtered_trc_files, trial_output_dir) - -def load_trc(config_dict, trc_files): - """ - Load and filter TRC files according to the configuration. - - Args: - config_dict (dict): The configuration dictionary. - trc_files (list): A list of TRC file paths. - - Returns: - list: A list of filtered TRC files based on the criteria specified in the configuration. - """ - opensim_config = config_dict.get('opensim', {}) - use_lstm = opensim_config.get('use_augmentation', False) - load_trc_name = opensim_config.get('load_trc_name', 'default') - - # Filter out any scaled TRC files - unscaled_trc_files = [file for file in trc_files if '_scaling' not in str(file)] - - logging.info(f"Starting TRC file filtering with criteria: use_lstm = {use_lstm}, load_trc_name = {load_trc_name}") - logging.info(f"Initial list of TRC files: {unscaled_trc_files}") - - # Initialize the list to store filtered TRC files - trc_files = [] - - # Check for LSTM files if LSTM is being used - if use_lstm: - lstm_files = [file for file in unscaled_trc_files if '_LSTM.trc' in str(file)] - if not lstm_files: - raise FileNotFoundError("No LSTM TRC file found in the provided list.") - trc_files.extend(lstm_files) - - # Check for default or filtered TRC files - if load_trc_name == 'default': - default_files = [file for file in unscaled_trc_files if '_LSTM' not in str(file) and '_filt_butterworth' not in str(file)] - trc_files.extend(default_files) - elif load_trc_name == 'filtered': - filtered_files = [file for file in unscaled_trc_files if '_filt_butterworth' in str(file) and '_LSTM' not in str(file)] - trc_files.extend(filtered_files) - - # If no TRC files are found after filtering, raise an error - if not trc_files: - logging.error(f"No suitable TRC files found with the specified criteria: use_lstm = {use_lstm}, load_trc_name = {load_trc_name}") - raise FileNotFoundError(f"No suitable TRC files found in the provided list with the specified criteria: use_lstm = {use_lstm}, load_trc_name = {load_trc_name}") - - logging.info(f"Filtered TRC files: {trc_files}") - - return trc_files - - def read_trc(trc_path): - """ + ''' Read a TRC file and extract its contents. - Args: - trc_path (str): The path to the TRC file. + INPUTS: + - trc_path (str): The path to the TRC file. + + OUTPUTS: + - tuple: A tuple containing the Q coordinates, frames column, time column, and header. + ''' - Returns: - tuple: A tuple containing the Q coordinates, frames column, time column, and header. - """ try: - logging.info(f"Attempting to read TRC file: {trc_path}") with open(trc_path, 'r') as trc_file: header = [next(trc_file) for _ in range(5)] + markers = header[3].split('\t')[2::3][:-1] + trc_df = pd.read_csv(trc_path, sep="\t", skiprows=4, encoding='utf-8') frames_col, time_col = trc_df.iloc[:, 0], trc_df.iloc[:, 1] - Q_coords = trc_df.drop(trc_df.columns[[0, 1]], axis=1) - return Q_coords, frames_col, time_col, header + Q_coords = trc_df.drop(trc_df.columns[[0, 1, -1]], axis=1) + + return Q_coords, frames_col, time_col, markers, header + except Exception as e: logging.error(f"Error reading TRC file at {trc_path}: {e}") raise -def make_trc_with_Q(Q, header, trc_path): - """ - Write the processed Q coordinates back to a TRC file. - - Args: - Q (pd.DataFrame): The Q coordinates data. - header (list): The header of the original TRC file. - trc_path (str): Path to save the new TRC file. - """ - header_2_split = header[2].split('\t') - header_2_split[2] = str(len(Q)) - header_2_split[-1] = str(len(Q)) - header[2] = '\t'.join(header_2_split) + '\n' - - time = pd.Series(np.arange(len(Q)) / float(header_2_split[0]), name='t') - Q.insert(0, 't', time) - - with open(trc_path, 'w') as trc_o: - [trc_o.write(line) for line in header] - Q.to_csv(trc_o, sep='\t', index=True, header=None, lineterminator='\n') - - -def get_key(config_dict): - """ - Determine the key for the OpenSim model and setup files based on the configuration. - - Args: - config_dict (dict): The configuration dictionary. - - Returns: - str: The key used to select the model and setup files. - """ - use_augmentation = config_dict.get('opensim', {}).get('use_augmentation', False) - - if use_augmentation: - return 'LSTM' - - pose_model = config_dict.get('pose', {}).get('pose_model', '').upper() - if not pose_model: - raise ValueError(f"Invalid or missing 'pose_model' in config: {pose_model}") - - return pose_model - - -def get_OpenSim_Setup(): - """ +def get_opensim_setup_dir(): + ''' Locate the OpenSim setup directory within the Pose2Sim package. - Returns: - Path: The path to the OpenSim setup directory. - """ + OUTPUTS: + - Path: The path to the OpenSim setup directory. + ''' + pose2sim_path = Path(sys.modules['Pose2Sim'].__file__).resolve().parent setup_dir = pose2sim_path / 'OpenSim_Setup' return setup_dir -def get_Model(config_dict): - """ - Retrieve the OpenSim model file path based on the configuration. +def get_model_path(model_name, osim_setup_dir): + ''' + Retrieve the path of the OpenSim model file. - Args: - config_dict (dict): The configuration dictionary. + INPUTS: + - model_name (str): Name of the model + - osim_setup_dir (Path): Path to the OpenSim setup directory. - Returns: - str: The path to the OpenSim model file. - """ - setup_key = get_key(config_dict) - setup_dir = get_OpenSim_Setup() + OUTPUTS: + - pose_model_path: (Path) Path to the OpenSim model file. + ''' - if setup_key == 'LSTM': - pose_model_file = 'Model_Pose2Sim_LSTM.osim' - elif setup_key == 'BLAZEPOSE': - pose_model_file = 'Model_Pose2Sim_Blazepose.osim' - elif setup_key == 'BODY_25': - pose_model_file = 'Model_Pose2Sim_Body25.osim' - elif setup_key == 'BODY_25B': + if model_name == 'BODY_25B': pose_model_file = 'Model_Setup_Pose2Sim_Body25b.osim' - elif setup_key == 'BODY_135': + elif model_name == 'BODY_25': + pose_model_file = 'Model_Pose2Sim_Body25.osim' + elif model_name == 'BODY_135': pose_model_file = 'Model_Pose2Sim_Body135.osim' - elif setup_key == 'COCO_17': - pose_model_file = 'Model_Pose2Sim_Coco17.osim' - elif setup_key == 'COCO_133': - pose_model_file = 'Model_Pose2Sim_Coco133.osim' - elif setup_key == 'HALPE_26': + elif model_name == 'BLAZEPOSE': + pose_model_file = 'Model_Pose2Sim_Blazepose.osim' + elif model_name == 'HALPE_26': pose_model_file = 'Model_Pose2Sim_Halpe26.osim' - elif setup_key == 'HALPE_68': + elif model_name == 'HALPE_68' or model_name == 'HALPE_136': pose_model_file = 'Model_Pose2Sim_Halpe68_136.osim' + elif model_name == 'COCO_133': + pose_model_file = 'Model_Pose2Sim_Coco133.osim' + # elif model_name == 'COCO' or model_name == 'MPII': + # pose_model_file = 'Model_Pose2Sim_Coco.osim' + elif model_name == 'COCO_17': + pose_model_file = 'Model_Pose2Sim_Coco17.osim' + elif model_name == 'LSTM': + pose_model_file = 'Model_Pose2Sim_LSTM.osim' else: - raise ValueError(f"pose_model '{setup_key}' not found.") + raise ValueError(f"Pose model '{model_name}' not found.") - pose_model_path = os.path.join(setup_dir, pose_model_file) - return pose_model_path + unscaled_model_path = osim_setup_dir / pose_model_file + return unscaled_model_path -def get_Scale_Setup(config_dict): - """ - Retrieve the OpenSim scaling setup file path based on the configuration. +def get_scaling_setup(model_name, osim_setup_dir): + ''' + Retrieve the OpenSim scaling setup file path. - Args: - config_dict (dict): The configuration dictionary. + INPUTS: + - model_name (str): Name of the model + - osim_setup_dir (Path): Path to the OpenSim setup directory. - Returns: - str: The path to the OpenSim scaling setup file. - """ - setup_key = get_key(config_dict) - setup_dir = get_OpenSim_Setup() + OUTPUTS: + - scaling_setup_path: (Path) Path to the OpenSim scaling setup file. + ''' - if setup_key == 'LSTM': - scale_setup_file = 'Scaling_Setup_Pose2Sim_LSTM.xml' - elif setup_key == 'BLAZEPOSE': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Blazepose.xml' - elif setup_key == 'BODY_25': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Body25.xml' - elif setup_key == 'BODY_25B': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Body25b.xml' - elif setup_key == 'BODY_135': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Body135.xml' - elif setup_key == 'COCO_17': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Coco17.xml' - elif setup_key == 'COCO_133': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Coco133.xml' - elif setup_key == 'HALPE_26': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Halpe26.xml' - elif setup_key == 'HALPE_68': - scale_setup_file = 'Scaling_Setup_Pose2Sim_Halpe68_136.xml' + if model_name == 'BODY_25B': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Body25b.xml' + elif model_name == 'BODY_25': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Body25.xml' + elif model_name == 'BODY_135': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Body135.xml' + elif model_name == 'BLAZEPOSE': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Blazepose.xml' + elif model_name == 'HALPE_26': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Halpe26.xml' + elif model_name == 'HALPE_68' or model_name == 'HALPE_136': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Halpe68_136.xml' + elif model_name == 'COCO_133': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Coco133.xml' + # elif model_name == 'COCO' or model_name == 'MPII': + # scaling_setup_file = 'Scaling_Setup_Pose2Sim_Coco.xml' + elif model_name == 'COCO_17': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_Coco17.xml' + elif model_name == 'LSTM': + scaling_setup_file = 'Scaling_Setup_Pose2Sim_LSTM.xml' else: - raise ValueError(f"pose_model '{setup_key}' not found.") + raise ValueError(f"Pose model '{model_name}' not found.") - scale_setup_path = os.path.join(setup_dir, scale_setup_file) - return scale_setup_path + scaling_setup_path = osim_setup_dir / scaling_setup_file + return scaling_setup_path -def get_IK_Setup(config_dict): - """ - Retrieve the OpenSim inverse kinematics setup file path based on the configuration. +def get_IK_Setup(model_name, osim_setup_dir): + ''' + Retrieve the OpenSim inverse kinematics setup file path. - Args: - config_dict (dict): The configuration dictionary. + INPUTS: + - model_name (str): Name of the model + - osim_setup_dir (Path): Path to the OpenSim setup directory. - Returns: - str: The path to the OpenSim inverse kinematics setup file. - """ - setup_key = get_key(config_dict) - setup_dir = get_OpenSim_Setup() - - if setup_key == 'LSTM': - ik_setup_file = 'IK_Setup_Pose2Sim_LSTM.xml' - elif setup_key == 'BLAZEPOSE': - ik_setup_file = 'IK_Setup_Pose2Sim_Blazepose.xml' - elif setup_key == 'BODY_25': - ik_setup_file = 'IK_Setup_Pose2Sim_Body25.xml' - elif setup_key == 'BODY_25B': + OUTPUTS: + - ik_setup_path: (Path) Path to the OpenSim IK setup file. + ''' + + if model_name == 'BODY_25B': ik_setup_file = 'IK_Setup_Pose2Sim_Body25b.xml' - elif setup_key == 'BODY_135': + elif model_name == 'BODY_25': + ik_setup_file = 'IK_Setup_Pose2Sim_Body25.xml' + elif model_name == 'BODY_135': ik_setup_file = 'IK_Setup_Pose2Sim_Body135.xml' - elif setup_key == 'COCO_17': - ik_setup_file = 'IK_Setup_Pose2Sim_Coco17.xml' - elif setup_key == 'COCO_133': - ik_setup_file = 'IK_Setup_Pose2Sim_Coco133.xml' - elif setup_key == 'HALPE_26': + elif model_name == 'BLAZEPOSE': + ik_setup_file = 'IK_Setup_Pose2Sim_Blazepose.xml' + elif model_name == 'HALPE_26': ik_setup_file = 'IK_Setup_Pose2Sim_Halpe26.xml' - elif setup_key == 'HALPE_68': + elif model_name == 'HALPE_68' or model_name == 'HALPE_136': ik_setup_file = 'IK_Setup_Pose2Sim_Halpe68_136.xml' + elif model_name == 'COCO_133': + ik_setup_file = 'IK_Setup_Pose2Sim_Coco133.xml' + # elif model_name == 'COCO' or model_name == 'MPII': + # ik_setup_file = 'IK_Setup_Pose2Sim_Coco.xml' + elif model_name == 'COCO_17': + ik_setup_file = 'IK_Setup_Pose2Sim_Coco17.xml' + elif model_name == 'LSTM': + ik_setup_file = 'IK_Setup_Pose2Sim_LSTM.xml' else: - raise ValueError(f"pose_model '{setup_key}' not found.") + raise ValueError(f"Pose model '{model_name}' not found.") - ik_setup_path = os.path.join(setup_dir, ik_setup_file) + ik_setup_path = osim_setup_dir / ik_setup_file return ik_setup_path -def get_output_dir(config_dir, person_id): - """ +# def get_output_dir(config_dir, person_id): + ''' Determines the correct output directory based on the configuration and the person identifier. - Args: - config_dir (Path): The root directory where the configuration file is located. - person_id (str): Identifier for the person (e.g., 'SinglePerson', 'P1'). + INPUTS: + - config_dir (Path): The root directory where the configuration file is located. + - person_id (str): Identifier for the person (e.g., 'SinglePerson', 'P1'). - Returns: - Path: The path where the output files should be stored. - """ - output_dir = config_dir / 'opensim' # Assuming 'opensim' as the default output subdirectory + OUTPUTS: + - Path: The path where the output files should be stored. + ''' + + output_dir = config_dir / 'kinematics' # Assuming 'opensim' as the default output subdirectory # Append the person_id to the output directory if it's a multi-person setup if person_id != "SinglePerson": output_dir = output_dir / person_id - logging.debug(f"Output directory determined as: {output_dir}") + logging.info(f"Output directory determined as: {output_dir}") # Create the directory if it does not exist if not output_dir.exists(): @@ -378,134 +239,329 @@ def get_output_dir(config_dir, person_id): return output_dir -def perform_scaling(config_dict, person_id, trc_files, output_dir): - """ - Perform scaling on the TRC files according to the OpenSim configuration. +def get_kpt_pairs_from_tree(root_node): + ''' + Get name pairs for all parent-child relationships in the tree. + # Excludes the root node. - Args: - config_dict (dict): The configuration dictionary. - person_id (str): The person identifier (e.g., 'SinglePerson', 'P1'). - trc_files (list): List of TRC files to be processed. - output_dir (Path): The directory where the output files should be saved. - """ - geometry_path = Path(get_OpenSim_Setup()) / 'Geometry' - geometry_path_str = str(geometry_path) - opensim.ModelVisualizer.addDirToGeometrySearchPaths(geometry_path_str) + INPUTS: + - root_node (Node): The root node of the tree. + + OUTPUTS: + - list: A list of name pairs for all parent-child relationships in the tree. + ''' + + pairs = [] + for node in PreOrderIter(root_node): + # if node.is_root: + # continue + for child in node.children: + pairs.append([node.name, child.name]) + + return pairs + + +def get_kpt_pairs_from_scaling(scaling_root): + ''' + Get name pairs for all marker pairs in the scaling setup file. + ''' + + pairs = [pair.find('markers').text.strip().split(' ') for pair in scaling_root[0].findall(".//MarkerPair")] + + return pairs + + +def dict_segment_marker_pairs(scaling_root, right_left_symmetry=True): + ''' + + ''' + + measurement_dict = {} + for measurement in scaling_root.findall(".//Measurement"): + # Collect all marker pairs for this measurement + marker_pairs = [pair.find('markers').text.strip().split() for pair in measurement.findall(".//MarkerPair")] + + # Collect all body scales for this measurement + for body_scale in measurement.findall(".//BodyScale"): + body_name = body_scale.get('name') + if right_left_symmetry: + measurement_dict[body_name] = marker_pairs + else: + if body_name.endswith('_r'): + marker_pairs_r = [pair for pair in marker_pairs if any([pair[0].startswith('R'), pair[1].startswith('R')])] + measurement_dict[body_name] = marker_pairs_r + elif body_name.endswith('_l'): + marker_pairs_l = [pair for pair in marker_pairs if any([pair[0].startswith('L'), pair[1].startswith('L')])] + measurement_dict[body_name] = marker_pairs_l + + return measurement_dict + + +def dict_segment_ratio(scaling_root, unscaled_model, Q_coords_scaling, markers, right_left_symmetry=True): + ''' + ''' + + # segment_pairs = get_kpt_pairs_from_tree(eval(model_name)) + segment_pairs = get_kpt_pairs_from_scaling(scaling_root) + + # Get model segment lengths + model_markers_locs = [unscaled_model.getMarkerSet().get(marker).getLocationInGround(unscaled_model.getWorkingState()).to_numpy() for marker in markers] + model_segment_lengths = np.array([euclidean_distance(model_markers_locs[markers.index(pt1)], + model_markers_locs[markers.index(pt2)]) + for (pt1,pt2) in segment_pairs]) + + # Get median segment lengths from Q_coords_scaling. Trimmed mean works better than mean or median + trc_segment_lengths = np.array([euclidean_distance(Q_coords_scaling.iloc[:,markers.index(pt1)*3:markers.index(pt1)*3+3], + Q_coords_scaling.iloc[:,markers.index(pt2)*3:markers.index(pt2)*3+3]) + for (pt1,pt2) in segment_pairs]) + # trc_segment_lengths = np.median(trc_segment_lengths, axis=1) + # trc_segment_lengths = np.mean(trc_segment_lengths, axis=1) + trc_segment_lengths = np.array([trimmed_mean(arr, trimmed_percent=0.5) for arr in trc_segment_lengths]) + + # Calculate ratio for each segment + segment_ratios = trc_segment_lengths / model_segment_lengths + segment_markers_dict = dict_segment_marker_pairs(scaling_root, right_left_symmetry=right_left_symmetry) + segment_ratio_dict = segment_markers_dict.copy() + segment_ratio_dict.update({key: np.mean([segment_ratios[segment_pairs.index(k)] + for k in segment_markers_dict[key]]) + for key in segment_markers_dict.keys()}) + + return segment_ratio_dict + + +def deactivate_measurements(scaling_root): + ''' + ''' + + measurement_set = scaling_root.find(".//MeasurementSet/objects") + for measurement in measurement_set.findall('Measurement'): + apply_elem = measurement.find('apply') + apply_elem.text = 'false' + + +def update_scale_values(scaling_root, segment_ratio_dict): + ''' + ''' + + # Get the ScaleSet/objects element + scale_set = scaling_root.find(".//ScaleSet/objects") + + # Remove all existing Scale elements + for scale in scale_set.findall('Scale'): + scale_set.remove(scale) + + # Add new Scale elements based on scale_dict + for segment, scale in segment_ratio_dict.items(): + new_scale = etree.Element('Scale') + # scales + scales_elem = etree.SubElement(new_scale, 'scales') + scales_elem.text = ' '.join([str(scale)]*3) + # segment name + segment_elem = etree.SubElement(new_scale, 'segment') + segment_elem.text = segment + # apply True + apply_elem = etree.SubElement(new_scale, 'apply') + apply_elem.text = 'true' + + scale_set.append(new_scale) + + +def perform_scaling(trc_file, kinematics_dir, osim_setup_dir, model_name, right_left_symmetry=True, subject_height=1.75, subject_mass=70, remove_scaling_setup=True): + ''' + Perform model scaling based on the (not necessarily static) TRC file: + - Retrieve the 80% slowest frames, excluding frames where the person is out of frame. + - From these frames, measure median segment lengths. + - Calculate ratio between model and measured segment lengths -> OpenSim manual scaling. + + INPUTS: + - config_dict (dict): The configuration dictionary. + - person_id (str): The person identifier (e.g., 'SinglePerson', 'P1'). + - trc_files (list): List of TRC files to be processed. + - output_dir (Path): The directory where the output files should be saved. + ''' try: - athlete_config = config_dict.get('project', {}) - athlete_height = athlete_config.get('participant_height', -1) - athlete_weight = athlete_config.get('participant_mass', -1) + # Load model + opensim.ModelVisualizer.addDirToGeometrySearchPaths(str(osim_setup_dir / 'Geometry')) + unscaled_model_path = get_model_path(model_name, osim_setup_dir) + if not unscaled_model_path: + raise ValueError(f"Unscaled OpenSim model not found at: {unscaled_model_path}") + unscaled_model = opensim.Model(str(unscaled_model_path)) + unscaled_model.initSystem() + scaled_model_path = (kinematics_dir / (trc_file.stem + '.osim')).resolve() - if person_id == "SinglePerson": - if not isinstance(athlete_height, float) or not isinstance(athlete_weight, float): - raise ValueError("For a single person configuration, 'participant_height' and 'participant_mass' must be floats.") - else: - if person_id.startswith("P"): - try: - person_idx = int(person_id.replace('P', '')) - 1 - athlete_height = athlete_height[person_idx] - athlete_weight = athlete_weight[person_idx] - except (ValueError, IndexError) as e: - raise ValueError(f"Error processing multi-person data for '{person_id}': {e}") - else: - raise ValueError(f"Unexpected person_id format: '{person_id}'") + # Load scaling setup + scaling_path = get_scaling_setup(model_name, osim_setup_dir) + scaling_tree = etree.parse(scaling_path) + scaling_root = scaling_tree.getroot() + scaling_path_temp = str(kinematics_dir / Path(scaling_path).name) - logging.debug(f"Performing scaling. Output directory: {output_dir}") + # Read trc file + Q_coords, _, _, markers, _ = read_trc(trc_file) - pose_model = get_Model(config_dict) - if not pose_model: - raise ValueError(f"Model path not found for pose_model: {pose_model}") + # Using 80% slowest frames for scaling, removing frames when person is out of frame + Q_diff = Q_coords.diff(axis=0).sum(axis=1) + Q_diff = Q_diff[Q_diff != 0] # remove when speed is 0 (person out of frame) + min_speed_indices = Q_diff.abs().nsmallest(int(len(Q_diff) * 0.8)).index + Q_coords_scaling = Q_coords.iloc[min_speed_indices].reset_index(drop=True) - for trc_file in trc_files: - trc_file = Path(trc_file) - scaling_path = get_Scale_Setup(config_dict) + # Get manual scale values (scale on trimmed mean of measured segments rather than on raw keypoints) + segment_ratio_dict = dict_segment_ratio(scaling_root, unscaled_model, Q_coords_scaling, markers, right_left_symmetry=right_left_symmetry) - Q_coords, _, _, header = read_trc(trc_file) + # Update scaling setup file + scaling_root[0].find('mass').text = str(subject_mass) + scaling_root[0].find('height').text = str(subject_height) + scaling_root[0].find('GenericModelMaker').find('model_file').text = str(unscaled_model_path) + scaling_root[0].find(".//scaling_order").text = ' manualScale measurements' + deactivate_measurements(scaling_root) + update_scale_values(scaling_root, segment_ratio_dict) + for mk_f in scaling_root[0].findall(".//marker_file"): + mk_f.text = "Unassigned" + scaling_root[0].find('ModelScaler').find('output_model_file').text = str(scaled_model_path) - Q_diff = Q_coords.diff(axis=0).sum(axis=1) - Q_diff = Q_diff[Q_diff != 0] - min_speed_indices = Q_diff.abs().nsmallest(int(len(Q_diff) * 0.1)).index - Q_coords_scaling = Q_coords.iloc[min_speed_indices].reset_index(drop=True) + etree.indent(scaling_tree, space='\t', level=0) + scaling_tree.write(scaling_path_temp, pretty_print=True, xml_declaration=True, encoding='utf-8') + + # Run scaling + opensim.ScaleTool(scaling_path_temp).run() - trc_scaling_path = trc_file.parent / (trc_file.stem + '_scaling.trc') - make_trc_with_Q(Q_coords_scaling, header, str(trc_scaling_path)) - - scaling_file_path = str(trc_file.parent / (trc_file.stem + '_' + Path(scaling_path).name)) - scaled_model_path = (output_dir / (trc_file.stem + '_scaled.osim')).resolve() - scaling_tree = etree.parse(str(scaling_path)) - scaling_root = scaling_tree.getroot() - - scaling_root[0].find('mass').text = str(athlete_weight) - scaling_root[0].find('height').text = str(athlete_height) - scaling_root[0].find('GenericModelMaker').find('model_file').text = str(pose_model) - scaling_root[0].find('ModelScaler').find('marker_file').text = trc_scaling_path.name - scaling_root[0].find('ModelScaler').find('time_range').text = '0 ' + str(Q_coords_scaling['t'].iloc[-1]) - scaling_root[0].find('ModelScaler').find('output_model_file').text = str(scaled_model_path) - scaling_root[0].find('MarkerPlacer').find('marker_file').text = trc_scaling_path.name - scaling_root[0].find('MarkerPlacer').find('time_range').text = '0 ' + str(Q_coords_scaling['t'].iloc[-1]) - scaling_root[0].find('MarkerPlacer').find('output_model_file').text = str(scaled_model_path) - scaling_tree.write(scaling_file_path) - - logging.debug(f"Running ScaleTool with scaling file: {scaling_file_path}") - opensim.ScaleTool(scaling_file_path).run() + # Remove scaling setup + if remove_scaling_setup: + Path(scaling_path_temp).unlink() except Exception as e: - logging.error(f"Error during scaling for {person_id}: {e}") + logging.error(f"Error during scaling for {trc_file}: {e}") raise -def perform_inverse_kinematics(config_dict, person_id, trc_files, output_dir): - """ + +def perform_IK(trc_file, kinematics_dir, osim_setup_dir, model_name, remove_IK_setup=True): + ''' Perform inverse kinematics on the TRC files according to the OpenSim configuration. - Args: - config_dict (dict): The configuration dictionary. - person_id (str): The person identifier (e.g., 'SinglePerson', 'P1'). - trc_files (list): List of TRC files to be processed. - output_dir (Path): The directory where the output files should be saved. - """ + INPUTS: + - config_dict (dict): The configuration dictionary. + - person_id (str): The person identifier (e.g., 'SinglePerson', 'P1'). + - trc_files (list): List of TRC files to be processed. + - output_dir (Path): The directory where the output files should be saved. + ''' + try: - logging.debug(f"Performing inverse kinematics. Output directory: {output_dir}") + # Retrieve data + ik_path = get_IK_Setup(model_name, osim_setup_dir) + ik_path_temp = str(kinematics_dir / Path(ik_path).name) + scaled_model_path = (kinematics_dir / (trc_file.stem + '.osim')).resolve() + output_motion_file = Path(kinematics_dir, trc_file.stem + '.mot').resolve() + if not trc_file.exists(): + raise FileNotFoundError(f"TRC file does not exist: {trc_file}") + _, _, time_col, _, _ = read_trc(trc_file) + start_time, end_time = time_col.iloc[0], time_col.iloc[-1] - for trc_file in trc_files: - trc_file_path = Path(trc_file).resolve() - scaled_model_path = Path(output_dir) / (trc_file_path.stem + '_scaled.osim') + # Update IK setup file + ik_tree = etree.parse(ik_path) + ik_root = ik_tree.getroot() + ik_root.find('.//model_file').text = str(scaled_model_path) + ik_root.find('.//time_range').text = f'{start_time} {end_time}' + ik_root.find('.//output_motion_file').text = str(output_motion_file) + ik_root.find('.//marker_file').text = str(trc_file.resolve()) + ik_tree.write(ik_path_temp) - ik_setup_path = get_IK_Setup(config_dict) - Q_coords, frames_col, time_col, header = read_trc(trc_file_path) - ik_time_range = config_dict.get('opensim', {}).get('IK_timeRange', []) + # Run IK + opensim.InverseKinematicsTool(str(ik_path_temp)).run() - if not ik_time_range: - start_time = time_col.iloc[0] - end_time = time_col.iloc[-1] - else: - start_time, end_time = ik_time_range[0], ik_time_range[1] - - ik_file_path = Path(trc_file_path.parent / (trc_file_path.stem + '_' + Path(ik_setup_path).name)).resolve() - scaled_model_path = scaled_model_path.resolve() - output_motion_file = Path(output_dir, trc_file_path.stem + '.mot').resolve() - - ik_tree = etree.parse(ik_setup_path) - ik_root = ik_tree.getroot() - ik_root.find('.//model_file').text = str(scaled_model_path) - ik_root.find('.//time_range').text = f'{start_time} {end_time}' - ik_root.find('.//output_motion_file').text = str(output_motion_file) - ik_root.find('.//marker_file').text = str(trc_file_path) - ik_tree.write(ik_file_path) - - logging.info(f"Running InverseKinematicsTool with TRC file: {trc_file_path}") - if not trc_file_path.exists(): - raise FileNotFoundError(f"TRC file does not exist: {trc_file_path}") - - logging.debug(f"Running InverseKinematicsTool with IK setup file: {ik_file_path}") - opensim.InverseKinematicsTool(str(ik_file_path)).run() + # Remove IK setup + if remove_IK_setup: + Path(ik_path_temp).unlink() except Exception as e: - logging.error(f"Error during IK for {person_id}: {e}") + logging.error(f"Error during IK for {trc_file}: {e}") raise -def opensimProcessing(config_dict): - logging.info("Starting OpenSim processing...") - process_all_groups(config_dict) - logging.info("OpenSim processing completed successfully.") +def kinematics(config_dict): + ''' + Runs OpenSim scaling and inverse kinematics on the trc files of triangulated coordinates. + + INPUTS: + - config_dict (dict): Generated from a .toml calibration file + + OUTPUTS: + - A scaled .osim model for each person. + - Joint angle data files (.mot) for each person. + ''' + + try: + # Read config_dict + project_dir = config_dict.get('project').get('project_dir') + # if batch + session_dir = Path(project_dir) / '..' + # if single trial + session_dir = session_dir if 'Config.toml' in os.listdir(session_dir) else os.getcwd() + use_augmentation = config_dict.get('kinematics').get('use_augmentation') + if use_augmentation: model_name = 'LSTM' + else: model_name = config_dict.get('pose').get('pose_model').upper() + right_left_symmetry = config_dict.get('kinematics').get('right_left_symmetry') + remove_scaling_setup = config_dict.get('kinematics').get('remove_individual_scaling_setup') + remove_IK_setup = config_dict.get('kinematics').get('remove_individual_IK_setup') + subject_height = config_dict.get('project').get('participant_height') + subject_mass = config_dict.get('project').get('participant_mass') + + pose3d_dir = Path(project_dir) / 'pose-3d' + kinematics_dir = Path(project_dir) / 'kinematics' + kinematics_dir.mkdir(parents=True, exist_ok=True) + osim_setup_dir = get_opensim_setup_dir() + + # OpenSim logs saved to a different file + opensim_logs_file = kinematics_dir / 'opensim_logs.txt' + opensim.Logger.setLevelString('Info') + opensim.Logger.removeFileSink() + opensim.Logger.addFileSink(str(opensim_logs_file)) + + # Find all trc files + trc_files = [] + if use_augmentation: + trc_files = [f for f in pose3d_dir.glob('*.trc') if '_LSTM' in f.name] + if len(trc_files) == 0: + model_name = config_dict.get('pose').get('pose_model').upper() + logging.warning("No LSTM trc files found. Using non augmented trc files instead.") + if len(trc_files) == 0: # filtered files by default + trc_files = [f for f in pose3d_dir.glob('*.trc') if '_LSTM' not in f.name and '_filt' in f.name and '_scaling' not in f.name] + if len(trc_files) == 0: + trc_files = [f for f in pose3d_dir.glob('*.trc') if '_LSTM' not in f.name and '_scaling' not in f.name] + if len(trc_files) == 0: + raise ValueError(f'No trc files found in {pose3d_dir}.') + sorted(trc_files, key=natural_sort_key) + + # Get subject heights and masses + if subject_height is None or subject_height == 0: + subject_height = [1.75] * len(trc_files) + logging.warning("No subject height found in Config.toml. Using default height of 1.75m.") + elif not type(subject_height) == list: # int or float + subject_height = [subject_height] + elif len(subject_height) < len(trc_files): + logging.warning("Number of subject heights does not match number of TRC files. Missing heights are set to 1.75m.") + subject_height += [1.75] * (len(trc_files) - len(subject_height)) + + if subject_mass is None or subject_mass == 0: + subject_mass = [70] * len(trc_files) + logging.warning("No subject mass found in Config.toml. Using default mass of 70kg.") + elif not type(subject_mass) == list: + subject_mass = [subject_mass] + elif len(subject_mass) < len(trc_files): + logging.warning("Number of subject masses does not match number of TRC files. Missing masses are set to 70kg.") + subject_mass += [70] * (len(trc_files) - len(subject_mass)) + + # Perform scaling and IK for each trc file + for p, trc_file in enumerate(trc_files): + logging.info(f"Processing TRC file: {trc_file.resolve()}") + + logging.info("Scaling...") + perform_scaling(trc_file, kinematics_dir, osim_setup_dir, model_name, right_left_symmetry=right_left_symmetry, subject_height=subject_height[p], subject_mass=subject_mass[p], remove_scaling_setup=remove_scaling_setup) + logging.info(f"\tDone. OpenSim logs saved to {opensim_logs_file.resolve()}.") + logging.info(f"\tScaled model saved to {(kinematics_dir / (trc_file.stem + '_scaled.osim')).resolve()}") + + logging.info("\nInverse Kinematics...") + perform_IK(trc_file, kinematics_dir, osim_setup_dir, model_name, remove_IK_setup=remove_IK_setup) + logging.info(f"\tDone. OpenSim logs saved to {opensim_logs_file.resolve()}.") + logging.info(f"\tJoint angle data saved to {(kinematics_dir / (trc_file.stem + '.mot')).resolve()}\n") + except RuntimeError as e: + logging.error(f"Error occurred: {e}") diff --git a/Pose2Sim/markerAugmentation.py b/Pose2Sim/markerAugmentation.py index d213026..f33bd0d 100644 --- a/Pose2Sim/markerAugmentation.py +++ b/Pose2Sim/markerAugmentation.py @@ -49,7 +49,7 @@ __status__ = "Development" def check_midhip_data(trc_file): try: # Find MidHip data - midhip_data = trc_file.marker("CHip") + midhip_data = trc_file.marker("Hip") if midhip_data is None or len(midhip_data) == 0: raise ValueError("MidHip data is empty") except (KeyError, ValueError): @@ -57,7 +57,7 @@ def check_midhip_data(trc_file): rhip_data = trc_file.marker("RHip") lhip_data = trc_file.marker("LHip") midhip_data = (rhip_data + lhip_data) / 2 - trc_file.add_marker('CHip', *midhip_data.T) + trc_file.add_marker('Hip', *midhip_data.T) return trc_file @@ -83,14 +83,10 @@ def augmentTRC(config_dict): project_dir = config_dict.get('project').get('project_dir') pathInputTRCFile = os.path.realpath(os.path.join(project_dir, 'pose-3d')) pathOutputTRCFile = os.path.realpath(os.path.join(project_dir, 'pose-3d')) - subject_height = config_dict.get('project').get('participant_height') - if subject_height is None or subject_height == 0 or subject_height==0: - raise ValueError("Subject height is not set or is invalid.") - subject_mass = config_dict.get('project').get('participant_mass') - if not type(subject_height) == list: - subject_height = [subject_height] - subject_mass = [subject_mass] make_c3d = config_dict.get('markerAugmentation').get('make_c3d') + subject_height = config_dict.get('project').get('participant_height') + subject_mass = config_dict.get('project').get('participant_mass') + augmenterDir = os.path.dirname(utilsDataman.__file__) augmenterModelName = 'LSTM' augmenter_model = 'v0.3' @@ -110,6 +106,25 @@ def augmentTRC(config_dict): trc_files = trc_no_filtering sorted(trc_files, key=natural_sort_key) + # Get subject heights and masses + if subject_height is None or subject_height == 0: + subject_height = [1.75] * len(trc_files) + logging.warning("No subject height found in Config.toml. Using default height of 1.75m.") + elif not type(subject_height) == list: # int or float + subject_height = [subject_height] + elif len(subject_height) < len(trc_files): + logging.warning("Number of subject heights does not match number of TRC files. Missing heights are set to 1.75m.") + subject_height += [1.75] * (len(trc_files) - len(subject_height)) + + if subject_mass is None or subject_mass == 0: + subject_mass = [70] * len(trc_files) + logging.warning("No subject mass found in Config.toml. Using default mass of 70kg.") + elif not type(subject_mass) == list: + subject_mass = [subject_mass] + elif len(subject_mass) < len(trc_files): + logging.warning("Number of subject masses does not match number of TRC files. Missing masses are set to 70kg.") + subject_mass += [70] * (len(trc_files) - len(subject_mass)) + for p in range(len(subject_mass)): pathInputTRCFile = trc_files[p] pathOutputTRCFile = os.path.splitext(pathInputTRCFile)[0] + '_LSTM.trc' @@ -172,7 +187,7 @@ def augmentTRC(config_dict): trc_data_data = trc_data[:,1:] # Step 2: Normalize with reference marker position. - referenceMarker_data = trc_file.marker("CHip") # instead of trc_file.marker(referenceMarker) # change by HunMin + referenceMarker_data = trc_file.marker("Hip") # instead of trc_file.marker(referenceMarker) # change by HunMin norm_trc_data_data = np.zeros((trc_data_data.shape[0], trc_data_data.shape[1])) for i in range(0,trc_data_data.shape[1],3): diff --git a/README.md b/README.md index 13cc93d..2944d76 100644 --- a/README.md +++ b/README.md @@ -16,9 +16,9 @@ ##### N.B:. Please set undistort_points and handle_LR_swap to false for now since it currently leads to inaccuracies. I'll try to fix it soon. -> **_News_: Version 0.9:**\ -> **Pose estimation with RTMPose is now included in Pose2Sim!**\ -> **Other recently added features**: Automatic camera synchronization, multi-person analysis, Blender visualization, Marker augmentation, Batch processing. +> **_News_: Version 0.109:**\ +> **OpenSim scaling and inverse kinematics are now integrated in Pose2Sim!** No static trial needed.\ +> **Other recently added features**: Pose estimation, Automatic camera synchronization, Multi-person analysis, Blender visualization, Marker augmentation, Batch processing. > To upgrade, type `pip install pose2sim --upgrade` (note that you need Python 3.9 or higher). @@ -27,6 +27,14 @@ and lens distortions are better taken into account.\ --> `Pose2Sim` provides a workflow for 3D markerless kinematics, as an alternative to marker-based motion capture methods. It aims to provide a free tool to obtain research-grade results from consumer-grade equipment. Any combination of phone, webcam, GoPro, etc. can be used. + + + + + + + + **Pose2Sim** stands for "OpenPose to OpenSim", as it originally used *OpenPose* inputs (2D keypoints coordinates) from multiple videos and lead to an [OpenSim](https://opensim.stanford.edu/) result (full-body 3D joint angles). Pose estimation is now performed with more recent models from [RTMPose](https://github.com/open-mmlab/mmpose/tree/main/projects/rtmpose). OpenPose and other models are kept as legacy options. For real-time analysis with a single camera, please consider **[Sports2D](https://github.com/davidpagnon/Sports2D)** (note that the motion must lie in the sagittal or frontal plane). @@ -48,11 +56,12 @@ For real-time analysis with a single camera, please consider **[Sports2D](https: - [x] **v0.6** *(02/2024)*: Marker augmentation, Blender visualizer - [x] **v0.7** *(03/2024)*: Multi-person analysis - [x] **v0.8** *(04/2024)*: New synchronization tool -- [x] **v0.9: *(07/2024)*: Integration of pose estimation in the pipeline** -- [ ] v0.10: Integration of OpenSim in the pipeline -- [ ] v0.11: Calibration based on keypoint detection, Handling left/right swaps, Correcting lens distortions -- [ ] v0.12: Graphical User Interface -- [ ] v1.0: First accomplished release +- [x] **v0.9** *(07/2024)*: Integration of pose estimation in the pipeline +- [x] **v0.10 *(09/2024)*: Integration of OpenSim in the pipeline** +- [ ] v0.11: Migrated documentation to new github.io website +- [ ] v0.12: Calibration based on keypoint detection, Handling left/right swaps, Correcting lens distortions +- [ ] v0.13: Graphical User Interface +- [ ] v1.0: First full release
@@ -175,7 +184,7 @@ Pose2Sim.personAssociation() Pose2Sim.triangulation() Pose2Sim.filtering() Pose2Sim.markerAugmentation() -Pose2Sim.opensimProcessing() +Pose2Sim.kinematics() ``` 3D results are stored as .trc files in each trial folder in the `pose-3d` directory. @@ -189,7 +198,8 @@ OpenSim results are stored as scaled model .osim and .mot in each trial folder i - You can run all stages at once: ``` python from Pose2Sim import Pose2Sim - Pose2Sim.runAll(do_calibration=True, do_poseEstimation=True, do_synchronization=True, do_personAssociation=True, do_triangulation=True, do_filtering=True, do_markerAugmentation=True, do_opensimProcessing=True) + Pose2Sim.runAll(do_calibration=True, do_poseEstimation=True, do_synchronization=True, do_personAssociation=True, do_triangulation=True, do_filtering=True, do_markerAugmentation=True, do_kinematics=True) + # or simply: Pose2Sim.runAll() ``` - Try the calibration tool by changing `calibration_type` to `calculate` instead of `convert` in [Config.toml](https://github.com/perfanalytics/pose2sim/blob/main/Pose2Sim/Demo_SinglePerson/Config.toml) (more info [there](#calculate-from-scratch)). - If the results are not convincing, refer to Section [OpenSim-kinematics](#OpenSim-kinematics) in the document. @@ -260,8 +270,7 @@ You can then run OpenSim scaling and inverse kinematics for each resulting .trc You can also visualize your results with Blender as in [Demonstration Part-3](#demonstration-part-3-optional-visualize-your-results-with-blender). *N.B.:* Set *[project]* `multi_person = true` for each trial that contains multiple persons.\ -Set *[triangulation]* `reorder_trc = true` if you need to run OpenSim and to match the generated .trc files with the static trials.\ -Make sure that the order of *[markerAugmentation]* `participant_height` and `participant_mass` matches the order of the static trials. +Make sure that the order of *[markerAugmentation]* `participant_height` and `participant_mass` matches the person's IDs.