sports2d 0.5.5__py3-none-any.whl → 0.6.1__py3-none-any.whl

Sports2D/Demo/Config_demo.toml

@@ -13,7 +13,7 @@
 
 [project]
 video_input = 'demo.mp4' # 'webcam' or '<video_path.ext>', or ['video1_path.mp4', 'video2_path.avi>', ...]
-                        # Time ranges can be different for each video. All other processing arguments will be identical.
+                        # On Windows, replace '\' with '/'
                         # Beware that images won't be saved if paths contain non ASCII characters.
 person_height = 1.70    # Height of the person in meters (for pixels -> meters conversion)
 load_trc = ''           # If you do not want to recalculate pose, load it from a trc file (in px, not in m)
@@ -21,7 +21,8 @@ compare = false         # Not implemented yet
 
 # Video parameters
 time_range = []   # [] for the whole video, or [start_time, end_time] (in seconds), or [[start_time1, end_time1], [start_time2, end_time2], ...]
-video_dir = ''    # If empty, result dir is current dir
+                  # Time ranges can be different for each video. 
+video_dir = ''    # If empty, video dir is current dir
 
 # Webcam parameters
 webcam_id = 0 # your webcam id (0 is default)
@@ -48,12 +49,32 @@ result_dir = '' # If empty, project dir is current dir
 slowmo_factor = 1       # 1 for normal speed. For a video recorded at 240 fps and exported to 30 fps, it would be 240/30 = 8
 
 # Pose detection parameters
-pose_model = 'body_with_feet' # Only body_with_feet is available for now
-mode = 'balanced'       # 'lightweight', 'balanced', or 'performance'
+pose_model = 'Body_with_feet'  #With RTMLib: Body_with_feet (default HALPE_26 model), Whole_body (COCO_133: body + feet + hands), Body (COCO_17), CUSTOM (see example at the end of the file), or any from skeletons.py
+mode = 'balanced' # 'lightweight', 'balanced', 'performance', or """{dictionary}""" (see below)
+
+# A dictionary (WITHIN THREE DOUBLE QUOTES) allows you to manually select the person detection (if top_down approach) and/or pose estimation models (see https://github.com/Tau-J/rtmlib). 
+# Models can be local paths or URLs.
+# Make sure the input_sizes are within triple quotes, and that they are in the opposite order from the one in the model path (for example, it would be [192,256] for rtmpose-m_simcc-body7_pt-body7-halpe26_700e-256x192-4d3e73dd_20230605.zip). 
+# If your pose_model is not provided in skeletons.py, you may have to create your own one (see example at the end of the file).
+# Example, equivalent to mode='balanced':
+# mode = """{'det_class':'YOLOX',
+#          'det_model':'https://download.openmmlab.com/mmpose/v1/projects/rtmposev1/onnx_sdk/yolox_m_8xb8-300e_humanart-c2c7a14a.zip',
+#          'det_input_size':[640, 640],
+#          'pose_class':'RTMPose',
+#          'pose_model':'https://download.openmmlab.com/mmpose/v1/projects/rtmposev1/onnx_sdk/rtmpose-m_simcc-body7_pt-body7-halpe26_700e-256x192-4d3e73dd_20230605.zip',
+#          'pose_input_size':[192,256]}"""
+# Example with one-stage RTMO model (Requires pose_model = 'Body'):
+# mode = """{'pose_class':'RTMO', 
+#          'pose_model':'https://download.openmmlab.com/mmpose/v1/projects/rtmo/onnx_sdk/rtmo-m_16xb16-600e_body7-640x640-39e78cc4_20231211.zip', 
+#          'pose_input_size':[640, 640]}"""
+
 det_frequency = 1       # Run person detection only every N frames, and inbetween track previously detected bounding boxes (keypoint detection is still run on all frames). 
                         # Equal to or greater than 1, can be as high as you want in simple uncrowded cases. Much faster, but might be less accurate. 
+device = 'auto' # 'auto', 'CPU', 'CUDA', 'MPS', 'ROCM'
+backend = 'auto' # 'auto', 'openvino', 'onnxruntime', 'opencv'
 tracking_mode = 'sports2d' # 'rtmlib' or 'sports2d'. 'sports2d' is generally much more accurate and comparable in speed
 
+
 # Processing parameters
 keypoint_likelihood_threshold = 0.3 # Keypoints whose likelihood is lower will not be taken into account
 average_likelihood_threshold = 0.5  # Person will be ignored if average likelihood of good keypoints is lower than this value
@@ -83,13 +104,14 @@ fontSize = 0.3
 
 # Select joint angles among
 # ['Right ankle', 'Left ankle', 'Right knee', 'Left knee', 'Right hip', 'Left hip', 'Right shoulder', 'Left shoulder', 'Right elbow', 'Left elbow', 'Right wrist', 'Left wrist']
-joint_angles = ['Right ankle', 'Left ankle', 'Right knee', 'Left knee', 'Right hip', 'Left hip', 'Right shoulder', 'Left shoulder', 'Right elbow', 'Left elbow']
+joint_angles = ['Right ankle', 'Left ankle', 'Right knee', 'Left knee', 'Right hip', 'Left hip', 'Right shoulder', 'Left shoulder', 'Right elbow', 'Left elbow', 'Right wrist', 'Left wrist']
 # Select segment angles among
 # ['Right foot', 'Left foot', 'Right shank', 'Left shank', 'Right thigh', 'Left thigh', 'Pelvis', 'Trunk', 'Shoulders', 'Head', 'Right arm', 'Left arm', 'Right forearm', 'Left forearm']
 segment_angles = ['Right foot', 'Left foot', 'Right shank', 'Left shank', 'Right thigh', 'Left thigh', 'Pelvis', 'Trunk', 'Shoulders', 'Head', 'Right arm', 'Left arm', 'Right forearm', 'Left forearm']
 
 # Processing parameters
 flip_left_right = true  # Same angles whether the participant faces left/right. Set it to false if you want timeseries to be continuous even when the participent switches their stance.
+correct_segment_angles_with_floor_angle = true # If the camera is tilted, corrects segment angles as regards to the floor angle. Set to false is the floor is tilted instead
 
 
 [post-processing]
@@ -121,5 +143,88 @@ person_orientation = ['front', 'none', 'left'] # Choose among 'auto', 'none', 'f
 osim_setup_path = '../OpenSim_setup' # Path to the OpenSim setup folder
 close_to_zero_speed_m = 0.2 # Sum for all keypoints: about 50 px/frame or 0.2 m/frame 
 
+
 [logging]
-use_custom_logging = false # if integrated in an API that already has logging
+use_custom_logging = false # if integrated in an API that already has logging
+
+
+
+# CUSTOM skeleton
+# If you use a model with different keypoints and/or different ordering
+# Useful if you trained your own model, from DeepLabCut or MMPose for example. 
+# Make sure the ids are set in the right order and start from zero.
+# 
+# If you want to perform inverse kinematics, you will also need to create an OpenSim model
+# and add to its markerset the location where you expect the triangulated keypoints to be detected.
+# 
+# In this example, CUSTOM reproduces the HALPE_26 skeleton (default skeletons are stored in skeletons.py).
+# You can create as many custom skeletons as you want, just add them further down and rename them.
+# 
+# Check your model hierarchy with:  for pre, _, node in RenderTree(model): 
+#                                      print(f'{pre}{node.name} id={node.id}')
+[pose.CUSTOM]
+name = "Hip"
+id = 19
+  [[pose.CUSTOM.children]]
+  name = "RHip"
+  id = 12
+     [[pose.CUSTOM.children.children]]
+     name = "RKnee"
+     id = 14
+        [[pose.CUSTOM.children.children.children]]
+        name = "RAnkle"
+        id = 16
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "RBigToe"
+           id = 21
+              [[pose.CUSTOM.children.children.children.children.children]]
+              name = "RSmallToe"
+              id = 23
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "RHeel"
+           id = 25
+  [[pose.CUSTOM.children]]
+  name = "LHip"
+  id = 11
+     [[pose.CUSTOM.children.children]]
+     name = "LKnee"
+     id = 13
+        [[pose.CUSTOM.children.children.children]]
+        name = "LAnkle"
+        id = 15
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "LBigToe"
+           id = 20
+              [[pose.CUSTOM.children.children.children.children.children]]
+              name = "LSmallToe"
+              id = 22
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "LHeel"
+           id = 24
+  [[pose.CUSTOM.children]]
+  name = "Neck"
+  id = 18
+     [[pose.CUSTOM.children.children]]
+     name = "Head"
+     id = 17
+        [[pose.CUSTOM.children.children.children]]
+        name = "Nose"
+        id = 0
+     [[pose.CUSTOM.children.children]]
+     name = "RShoulder"
+     id = 6
+        [[pose.CUSTOM.children.children.children]]
+        name = "RElbow"
+        id = 8
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "RWrist"
+           id = 10
+     [[pose.CUSTOM.children.children]]
+     name = "LShoulder"
+     id = 5
+        [[pose.CUSTOM.children.children.children]]
+        name = "LElbow"
+        id = 7
+           [[pose.CUSTOM.children.children.children.children]]
+           name = "LWrist"
+           id = 9

Sports2D/Sports2D.py

@@ -143,6 +143,8 @@ DEFAULT_CONFIG =   {'project': {'video_input': ['demo.mp4'],
                                 'pose_model': 'body_with_feet',
                                 'mode': 'balanced',
                                 'det_frequency': 4,
+                                'device': 'auto',
+                                'backend': 'auto',
                                 'tracking_mode': 'sports2d',
                                 'keypoint_likelihood_threshold': 0.3,
                                 'average_likelihood_threshold': 0.5,
@@ -171,7 +173,9 @@ DEFAULT_CONFIG =   {'project': {'video_input': ['demo.mp4'],
                                                     'Right shoulder',
                                                     'Left shoulder',
                                                     'Right elbow',
-                                                    'Left elbow'],
+                                                    'Left elbow',
+                                                    'Right wrist',
+                                                    'Left wrist'],
                                 'segment_angles': [ 'Right foot',
                                                     'Left foot',
                                                     'Right shank',
@@ -186,7 +190,8 @@ DEFAULT_CONFIG =   {'project': {'video_input': ['demo.mp4'],
                                                     'Left arm',
                                                     'Right forearm',
                                                     'Left forearm'],
-                                'flip_left_right': True
+                                'flip_left_right': True,
+                                'correct_segment_angles_with_floor_angle': True
                                 },
                     'post-processing': {'interpolate': True,
                                         'interp_gap_smaller_than': 10,
@@ -228,9 +233,11 @@ CONFIG_HELP =   {'config': ["C", "path to a toml configuration file"],
                 'save_angles': ["A", "save angles as mot files. true if not specified"],
                 'slowmo_factor': ["", "slow-motion factor. For a video recorded at 240 fps and exported to 30 fps, it would be 240/30 = 8. 1 if not specified"],
                 'pose_model': ["p", "only body_with_feet is available for now. body_with_feet if not specified"],
-                'mode': ["m", "light, balanced, or performance. balanced if not specified"],
+                'mode': ["m", 'light, balanced, performance, or a """{dictionary within triple quote}""". balanced if not specified. Use a dictionary to specify your own detection and/or pose estimation models (more about in the documentation).'],
                 'det_frequency': ["f", "run person detection only every N frames, and inbetween track previously detected bounding boxes. keypoint detection is still run on all frames.\n\
                                  Equal to or greater than 1, can be as high as you want in simple uncrowded cases. Much faster, but might be less accurate. 1 if not specified: detection runs on all frames"],
+                'backend': ["", "Backend for pose estimation can be 'auto', 'cpu', 'cuda', 'mps' (for MacOS), or 'rocm' (for AMD GPUs)"],
+                'device': ["", "Device for pose estimatino can be 'auto', 'openvino', 'onnxruntime', 'opencv'"],
                 'to_meters': ["M", "convert pixels to meters. true if not specified"],
                 'calib_on_person_id': ["", "person ID to calibrate on. 0 if not specified"],
                 'floor_angle': ["", "angle of the floor. 'auto' if not specified"],
@@ -252,6 +259,7 @@ CONFIG_HELP =   {'config': ["C", "path to a toml configuration file"],
                 'trimmed_extrema_percent': ["", "Proportion of the most extreme segment values to remove before calculating their mean. Defaults to 50"],
                 'fontSize': ["", "font size for angle values. 0.3 if not specified"],
                 'flip_left_right': ["", "true or false. true to get consistent angles with people facing both left and right sides. Set it to false if you want timeseries to be continuous even when the participent switches their stance. true if not specified"],
+                'correct_segment_angles_with_floor_angle': ["", "true or false. If the camera is tilted, corrects segment angles as regards to the floor angle. Set to false is the floor is tilted instead. True if not specified"],
                 'interpolate': ["", "interpolate missing data. true if not specified"],
                 'interp_gap_smaller_than': ["", "interpolate sequences of missing data if they are less than N frames long. 10 if not specified"],
                 'fill_large_gaps_with': ["", "last_value, nan, or zeros. last_value if not specified"],
@@ -320,7 +328,7 @@ def base_params(config_dict):
             video = cv2.VideoCapture(str(video_dir / video_file)) if video_dir else cv2.VideoCapture(str(video_file))
             if not video.isOpened():
                 raise FileNotFoundError(f'Error: Could not open {video_dir/video_file}. Check that the file exists.')
-            frame_rate = video.get(cv2.CAP_PROP_FPS)
+            frame_rate = round(video.get(cv2.CAP_PROP_FPS))
             if frame_rate == 0:
                 frame_rate = 30
                 logging.warning(f'Error: Could not retrieve frame rate from {video_dir/video_file}. Defaulting to 30fps.')
@@ -435,7 +443,7 @@ def process(config='Config_demo.toml'):
 
         process_fun(config_dict, video_file, time_range, frame_rate, result_dir)
 
-        elapsed_time = (datetime.now() - currentDateAndTime).total_seconds()        
+        elapsed_time = (datetime.now() - currentDateAndTime).total_seconds()
         logging.info(f'\nProcessing {video_file} took {elapsed_time:.2f} s.')
 
     logging.shutdown()
@@ -501,10 +509,11 @@ def main():
     # Override dictionary with command-line arguments if provided
     leaf_keys = get_leaf_keys(new_config)
     for leaf_key, default_value in leaf_keys.items():
-        leaf_name = leaf_key.split('.')[-1]
-        cli_value = getattr(args, leaf_name)
-        if cli_value is not None:
-            set_nested_value(new_config, leaf_key, cli_value)
+        if not 'CUSTOM' in leaf_key:
+            leaf_name = leaf_key.split('.')[-1]
+            cli_value = getattr(args, leaf_name)
+            if cli_value is not None:
+                set_nested_value(new_config, leaf_key, cli_value)
 
     # Run process with the new configuration dictionary
     Sports2D.process(new_config)

Sports2D/Utilities/common.py

@@ -23,6 +23,7 @@ from pathlib import Path
 import logging
 
 import numpy as np
+import pandas as pd
 from scipy import interpolate
 import imageio_ffmpeg as ffmpeg
 import cv2
@@ -43,6 +44,49 @@ __email__ = "contact@david-pagnon.com"
 __status__ = "Development"
 
 
+## CONSTANTS
+angle_dict = { # lowercase!
+    # joint angles
+    'right ankle': [['RKnee', 'RAnkle', 'RBigToe', 'RHeel'], 'dorsiflexion', 90, 1],
+    'left ankle': [['LKnee', 'LAnkle', 'LBigToe', 'LHeel'], 'dorsiflexion', 90, 1],
+    'right knee': [['RAnkle', 'RKnee', 'RHip'], 'flexion', -180, 1],
+    'left knee': [['LAnkle', 'LKnee', 'LHip'], 'flexion', -180, 1],
+    'right hip': [['RKnee', 'RHip', 'Hip', 'Neck'], 'flexion', 0, -1],
+    'left hip': [['LKnee', 'LHip', 'Hip', 'Neck'], 'flexion', 0, -1],
+    # 'lumbar': [['Neck', 'Hip', 'RHip', 'LHip'], 'flexion', -180, -1],
+    # 'neck': [['Head', 'Neck', 'RShoulder', 'LShoulder'], 'flexion', -180, -1],
+    'right shoulder': [['RElbow', 'RShoulder', 'Hip', 'Neck'], 'flexion', 0, -1],
+    'left shoulder': [['LElbow', 'LShoulder', 'Hip', 'Neck'], 'flexion', 0, -1],
+    'right elbow': [['RWrist', 'RElbow', 'RShoulder'], 'flexion', 180, -1],
+    'left elbow': [['LWrist', 'LElbow', 'LShoulder'], 'flexion', 180, -1],
+    'right wrist': [['RElbow', 'RWrist', 'RIndex'], 'flexion', -180, 1],
+    'left wrist': [['LElbow', 'LIndex', 'LWrist'], 'flexion', -180, 1],
+
+    # segment angles
+    'right foot': [['RBigToe', 'RHeel'], 'horizontal', 0, -1],
+    'left foot': [['LBigToe', 'LHeel'], 'horizontal', 0, -1],
+    'right shank': [['RAnkle', 'RKnee'], 'horizontal', 0, -1],
+    'left shank': [['LAnkle', 'LKnee'], 'horizontal', 0, -1],
+    'right thigh': [['RKnee', 'RHip'], 'horizontal', 0, -1],
+    'left thigh': [['LKnee', 'LHip'], 'horizontal', 0, -1],
+    'pelvis': [['LHip', 'RHip'], 'horizontal', 0, -1],
+    'trunk': [['Neck', 'Hip'], 'horizontal', 0, -1],
+    'shoulders': [['LShoulder', 'RShoulder'], 'horizontal', 0, -1],
+    'head': [['Head', 'Neck'], 'horizontal', 0, -1],
+    'right arm': [['RElbow', 'RShoulder'], 'horizontal', 0, -1],
+    'left arm': [['LElbow', 'LShoulder'], 'horizontal', 0, -1],
+    'right forearm': [['RWrist', 'RElbow'], 'horizontal', 0, -1],
+    'left forearm': [['LWrist', 'LElbow'], 'horizontal', 0, -1],
+    'right hand': [['RIndex', 'RWrist'], 'horizontal', 0, -1],
+    'left hand': [['LIndex', 'LWrist'], 'horizontal', 0, -1]
+    }
+
+colors = [(255, 0, 0), (0, 0, 255), (255, 255, 0), (255, 0, 255), (0, 255, 255), (0, 0, 0), (255, 255, 255),
+            (125, 0, 0), (0, 125, 0), (0, 0, 125), (125, 125, 0), (125, 0, 125), (0, 125, 125), 
+            (255, 125, 125), (125, 255, 125), (125, 125, 255), (255, 255, 125), (255, 125, 255), (125, 255, 255), (125, 125, 125),
+            (255, 0, 125), (255, 125, 0), (0, 125, 255), (0, 255, 125), (125, 0, 255), (125, 255, 0), (0, 255, 0)]
+thickness = 1
+
 ## CLASSES
 class plotWindow():
     '''
@@ -96,6 +140,35 @@ class plotWindow():
         self.app.exec_()
         
 ## FUNCTIONS
+def read_trc(trc_path):
+    '''
+    Read a TRC file and extract its contents.
+
+    INPUTS:
+    - trc_path (str): The path to the TRC file.
+
+    OUTPUTS:
+    - tuple: A tuple containing the Q coordinates, frames column, time column, marker names, and header.
+    '''
+
+    try:
+        with open(trc_path, 'r') as trc_file:
+            header = [next(trc_file) for _ in range(5)]
+        markers = header[3].split('\t')[2::3]
+        markers = [m.strip() for m in markers if m.strip()] # remove last \n character
+       
+        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)
+        Q_coords = Q_coords.loc[:, ~Q_coords.columns.str.startswith('Unnamed')] # remove unnamed columns
+        Q_coords.columns = np.array([[m,m,m] for m in markers]).ravel().tolist()
+
+        return Q_coords, frames_col, time_col, markers, header
+    
+    except Exception as e:
+        raise ValueError(f"Error reading TRC file at {trc_path}: {e}")
+
+
 def interpolate_zeros_nans(col, *args):
     '''
     Interpolate missing points (of value zero),
@@ -247,6 +320,10 @@ def points_to_angles(points_list):
     If parameters are arrays, returns an array of floats between 0.0 and 360.0
 
     INPUTS:
+    - points_list: list of arrays of points
+
+    OUTPUTS:
+    - ang_deg: float or array of floats. The angle(s) in degrees.
     '''
 
     if len(points_list) < 2: # if not enough points, return None
@@ -288,6 +365,220 @@ def points_to_angles(points_list):
     return ang_deg
 
 
+def fixed_angles(points_list, ang_name):
+    '''
+    Add offset and multiplying factor to angles
+
+    INPUTS:
+    - points_list: list of arrays of points
+    - ang_name: str. The name of the angle to consider.
+
+    OUTPUTS:
+    - ang: float. The angle in degrees.
+    '''
+
+    ang_params = angle_dict[ang_name]
+    ang = points_to_angles(points_list)
+    ang += ang_params[2]
+    ang *= ang_params[3]
+    if ang_name in ['pelvis', 'shoulders']:
+        ang = np.where(ang>90, ang-180, ang)
+        ang = np.where(ang<-90, ang+180, ang)
+    else:
+        ang = np.where(ang>180, ang-360, ang)
+        ang = np.where(ang<-180, ang+360, ang)
+
+    return ang
+
+
+def mean_angles(trc_data, ang_to_consider = ['right knee', 'left knee', 'right hip', 'left hip']):
+    '''
+    Compute the mean angle time series from 3D points for a given list of angles.
+
+    INPUTS:
+    - trc_data (DataFrame): The triangulated coordinates of the markers.
+    - ang_to_consider (list): The list of angles to consider (requires angle_dict).
+
+    OUTPUTS:
+    - ang_mean: The mean angle time series.
+    '''
+
+    ang_to_consider = ['right knee', 'left knee', 'right hip', 'left hip']
+
+    angs = []
+    for ang_name in ang_to_consider:
+        ang_params = angle_dict[ang_name]
+        ang_mk = ang_params[0]
+        if 'Neck' not in trc_data.columns:
+            df_MidShoulder = pd.DataFrame((trc_data['RShoulder'].values + trc_data['LShoulder'].values) /2)
+            df_MidShoulder.columns = ['Neck']*3
+            trc_data = pd.concat((trc_data.reset_index(drop=True), df_MidShoulder), axis=1)
+
+        pts_for_angles = []
+        for pt in ang_mk:
+            # pts_for_angles.append(trc_data.iloc[:,markers.index(pt)*3:markers.index(pt)*3+3])
+            pts_for_angles.append(trc_data[pt])
+
+        ang = fixed_angles(pts_for_angles, ang_name)
+        ang = np.abs(ang)
+        angs.append(ang)
+
+    ang_mean = np.mean(angs, axis=0)
+
+    return ang_mean
+
+
+def add_neck_hip_coords(kpt_name, p_X, p_Y, p_scores, kpt_ids, kpt_names):
+    '''
+    Add neck (midshoulder) and hip (midhip) coordinates if neck and hip are not available
+    
+    INPUTS:
+    - kpt_name: name of the keypoint to add (neck, hip)
+    - p_X: list of x coordinates after flipping if needed
+    - p_Y: list of y coordinates
+    - p_scores: list of confidence scores
+    - kpt_ids: list of keypoint ids (see skeletons.py)
+    - kpt_names: list of keypoint names (see skeletons.py)
+    
+    OUTPUTS:
+    - p_X: list of x coordinates with added missing coordinate
+    - p_Y: list of y coordinates with added missing coordinate
+    - p_scores: list of confidence scores with added missing score
+    '''
+
+    names, ids = kpt_names.copy(), kpt_ids.copy()
+    names.append(kpt_name)
+    ids.append(len(p_X))
+    if kpt_name == 'Neck':
+        mid_X = (np.abs(p_X[ids[names.index('LShoulder')]]) + np.abs(p_X[ids[names.index('RShoulder')]])) /2
+        mid_Y = (p_Y[ids[names.index('LShoulder')]] + p_Y[ids[names.index('RShoulder')]])/2
+        mid_score = (p_scores[ids[names.index('LShoulder')]] + p_scores[ids[names.index('RShoulder')]])/2
+    elif kpt_name == 'Hip':
+        mid_X = (np.abs(p_X[ids[names.index('LHip')]]) + np.abs(p_X[ids[names.index('RHip')]]) ) /2
+        mid_Y = (p_Y[ids[names.index('LHip')]] + p_Y[ids[names.index('RHip')]])/2
+        mid_score = (p_scores[ids[names.index('LHip')]] + p_scores[ids[names.index('RHip')]])/2
+    else:
+        raise ValueError("kpt_name must be 'Neck' or 'Hip'")
+    p_X = np.append(p_X, mid_X)
+    p_Y = np.append(p_Y, mid_Y)
+    p_scores = np.append(p_scores, mid_score)
+
+    return p_X, p_Y, p_scores
+
+
+def best_coords_for_measurements(trc_data, keypoints_names, fastest_frames_to_remove_percent=0.2, close_to_zero_speed=0.2, large_hip_knee_angles=45):
+    '''
+    Compute the best coordinates for measurements, after removing:
+    - 20% fastest frames (may be outliers)
+    - frames when speed is close to zero (person is out of frame): 0.2 m/frame, or 50 px/frame
+    - frames when hip and knee angle below 45° (imprecise coordinates when person is crouching)
+    
+    INPUTS:
+    - trc_data: pd.DataFrame. The XYZ coordinates of each marker
+    - keypoints_names: list. The list of marker names
+    - fastest_frames_to_remove_percent: float
+    - close_to_zero_speed: float (sum for all keypoints: about 50 px/frame or 0.2 m/frame)
+    - large_hip_knee_angles: int
+    - trimmed_extrema_percent
+
+    OUTPUT:
+    - trc_data_low_speeds_low_angles: pd.DataFrame. The best coordinates for measurements
+    '''
+
+    # Add MidShoulder column
+    df_MidShoulder = pd.DataFrame((trc_data['RShoulder'].values + trc_data['LShoulder'].values) /2)
+    df_MidShoulder.columns = ['MidShoulder']*3
+    trc_data = pd.concat((trc_data.reset_index(drop=True), df_MidShoulder), axis=1)
+
+    # Add Hip column if not present
+    n_markers_init = len(keypoints_names)
+    if 'Hip' not in keypoints_names:
+        df_Hip = pd.DataFrame((trc_data['RHip'].values + trc_data['LHip'].values) /2)
+        df_Hip.columns = ['Hip']*3
+        trc_data = pd.concat((trc_data.reset_index(drop=True), df_Hip), axis=1)
+    n_markers = len(keypoints_names)
+
+    # Using 80% slowest frames
+    sum_speeds = pd.Series(np.nansum([np.linalg.norm(trc_data.iloc[:,kpt:kpt+3].diff(), axis=1) for kpt in range(n_markers)], axis=0))
+    sum_speeds = sum_speeds[sum_speeds>close_to_zero_speed] # Removing when speeds close to zero (out of frame)
+    if len(sum_speeds)==0:
+        raise ValueError('All frames have speed close to zero. Make sure the person is moving and correctly detected, or change close_to_zero_speed to a lower value.')
+    min_speed_indices = sum_speeds.abs().nsmallest(int(len(sum_speeds) * (1-fastest_frames_to_remove_percent))).index
+    trc_data_low_speeds = trc_data.iloc[min_speed_indices].reset_index(drop=True)
+    
+    # Only keep frames with hip and knee flexion angles below 45% 
+    # (if more than 50 of them, else take 50 smallest values)
+    try:
+        ang_mean = mean_angles(trc_data_low_speeds, ang_to_consider = ['right knee', 'left knee', 'right hip', 'left hip'])
+        trc_data_low_speeds_low_angles = trc_data_low_speeds[ang_mean < large_hip_knee_angles]
+        if len(trc_data_low_speeds_low_angles) < 50:
+            trc_data_low_speeds_low_angles = trc_data_low_speeds.iloc[pd.Series(ang_mean).nsmallest(50).index]
+    except:
+        logging.warning(f"At least one among the RAnkle, RKnee, RHip, RShoulder, LAnkle, LKnee, LHip, LShoulder markers is missing for computing the knee and hip angles. Not restricting these agles to be below {large_hip_knee_angles}°.")
+
+    if n_markers_init < n_markers:
+        trc_data_low_speeds_low_angles = trc_data_low_speeds_low_angles.iloc[:,:-3]
+
+    return trc_data_low_speeds_low_angles
+
+
+def compute_height(trc_data, keypoints_names, fastest_frames_to_remove_percent=0.1, close_to_zero_speed=50, large_hip_knee_angles=45, trimmed_extrema_percent=0.5):
+    '''
+    Compute the height of the person from the trc data.
+
+    INPUTS:
+    - trc_data: pd.DataFrame. The XYZ coordinates of each marker
+    - keypoints_names: list. The list of marker names
+    - fastest_frames_to_remove_percent: float. Frames with high speed are considered as outliers
+    - close_to_zero_speed: float. Sum for all keypoints: about 50 px/frame or 0.2 m/frame
+    - large_hip_knee_angles5: float. Hip and knee angles below this value are considered as imprecise
+    - trimmed_extrema_percent: float. Proportion of the most extreme segment values to remove before calculating their mean)
+    
+    OUTPUT:
+    - height: float. The estimated height of the person
+    '''
+    
+    # Retrieve most reliable coordinates, adding MidShoulder and Hip columns if not present
+    trc_data_low_speeds_low_angles = best_coords_for_measurements(trc_data, keypoints_names, 
+                                                                  fastest_frames_to_remove_percent=fastest_frames_to_remove_percent, close_to_zero_speed=close_to_zero_speed, large_hip_knee_angles=large_hip_knee_angles)
+
+    # Automatically compute the height of the person
+    feet_pairs = [['RHeel', 'RAnkle'], ['LHeel', 'LAnkle']]
+    try:
+        rfoot, lfoot = [euclidean_distance(trc_data_low_speeds_low_angles[pair[0]],trc_data_low_speeds_low_angles[pair[1]]) for pair in feet_pairs]
+    except:
+        rfoot, lfoot = 10, 10
+        logging.warning('The Heel marker is missing from your model. Considering Foot to Heel size as 10 cm.')
+
+    ankle_to_shoulder_pairs =  [['RAnkle', 'RKnee'], ['RKnee', 'RHip'], ['RHip', 'RShoulder'],
+                                ['LAnkle', 'LKnee'], ['LKnee', 'LHip'], ['LHip', 'LShoulder']]
+    try:
+        rshank, rfemur, rback, lshank, lfemur, lback = [euclidean_distance(trc_data_low_speeds_low_angles[pair[0]],trc_data_low_speeds_low_angles[pair[1]]) for pair in ankle_to_shoulder_pairs]
+    except:
+        logging.error('At least one of the following markers is missing for computing the height of the person:\
+                            RAnkle, RKnee, RHip, RShoulder, LAnkle, LKnee, LHip, LShoulder.\n\
+                            Make sure that the person is entirely visible, or use a calibration file instead, or set "to_meters=false".')
+        raise ValueError('At least one of the following markers is missing for computing the height of the person:\
+                         RAnkle, RKnee, RHip, RShoulder, LAnkle, LKnee, LHip, LShoulder.\
+                         Make sure that the person is entirely visible, or use a calibration file instead, or set "to_meters=false".')
+
+    try:
+        head_pair = [['MidShoulder', 'Head']]
+        head = [euclidean_distance(trc_data_low_speeds_low_angles[pair[0]],trc_data_low_speeds_low_angles[pair[1]]) for pair in head_pair][0]
+    except:
+        head_pair = [['MidShoulder', 'Nose']]
+        head = [euclidean_distance(trc_data_low_speeds_low_angles[pair[0]],trc_data_low_speeds_low_angles[pair[1]]) for pair in head_pair][0]\
+                *1.33
+        logging.warning('The Head marker is missing from your model. Considering Neck to Head size as 1.33 times Neck to MidShoulder size.')
+    
+    heights = (rfoot + lfoot)/2 + (rshank + lshank)/2 + (rfemur + lfemur)/2 + (rback + lback)/2 + head
+    
+    # Remove the 20% most extreme values
+    height = trimmed_mean(heights, trimmed_extrema_percent=trimmed_extrema_percent)
+
+    return height
+
+
 def euclidean_distance(q1, q2):
     '''
     Euclidean distance between 2 points (N-dim).