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

Sports2D/process.py

@@ -60,7 +60,7 @@ from functools import partial
 from datetime import datetime
 import itertools as it
 from tqdm import tqdm
-from anytree import RenderTree, PreOrderIter
+from anytree import RenderTree
 
 import numpy as np
 import pandas as pd
@@ -68,6 +68,7 @@ import cv2
 import matplotlib as mpl
 import matplotlib.pyplot as plt
 from rtmlib import PoseTracker, BodyWithFeet, Wholebody, Body, Custom
+from deep_sort_realtime.deepsort_tracker import DeepSort
 
 from Sports2D.Utilities import filter
 from Sports2D.Utilities.common import *
@@ -337,161 +338,6 @@ def compute_angle(ang_name, person_X_flipped, person_Y, angle_dict, keypoints_id
     return ang
 
 
-def min_with_single_indices(L, T):
-    '''
-    Let L be a list (size s) with T associated tuple indices (size s).
-    Select the smallest values of L, considering that 
-    the next smallest value cannot have the same numbers 
-    in the associated tuple as any of the previous ones.
-
-    Example:
-    L = [  20,   27,  51,    33,   43,   23,   37,   24,   4,   68,   84,    3  ]
-    T = list(it.product(range(2),range(3)))
-      = [(0,0),(0,1),(0,2),(0,3),(1,0),(1,1),(1,2),(1,3),(2,0),(2,1),(2,2),(2,3)]
-
-    - 1st smallest value: 3 with tuple (2,3), index 11
-    - 2nd smallest value when excluding indices (2,.) and (.,3), i.e. [(0,0),(0,1),(0,2),X,(1,0),(1,1),(1,2),X,X,X,X,X]:
-    20 with tuple (0,0), index 0
-    - 3rd smallest value when excluding [X,X,X,X,X,(1,1),(1,2),X,X,X,X,X]:
-    23 with tuple (1,1), index 5
-    
-    INPUTS:
-    - L: list (size s)
-    - T: T associated tuple indices (size s)
-
-    OUTPUTS: 
-    - minL: list of smallest values of L, considering constraints on tuple indices
-    - argminL: list of indices of smallest values of L (indices of best combinations)
-    - T_minL: list of tuples associated with smallest values of L
-    '''
-
-    minL = [np.nanmin(L)]
-    argminL = [np.nanargmin(L)]
-    T_minL = [T[argminL[0]]]
-    
-    mask_tokeep = np.array([True for t in T])
-    i=0
-    while mask_tokeep.any()==True:
-        mask_tokeep = mask_tokeep & np.array([t[0]!=T_minL[i][0] and t[1]!=T_minL[i][1] for t in T])
-        if mask_tokeep.any()==True:
-            indicesL_tokeep = np.where(mask_tokeep)[0]
-            minL += [np.nanmin(np.array(L)[indicesL_tokeep]) if not np.isnan(np.array(L)[indicesL_tokeep]).all() else np.nan]
-            argminL += [indicesL_tokeep[np.nanargmin(np.array(L)[indicesL_tokeep])] if not np.isnan(minL[-1]) else indicesL_tokeep[0]]
-            T_minL += (T[argminL[i+1]],)
-            i+=1
-    
-    return np.array(minL), np.array(argminL), np.array(T_minL)
-    
-    
-def pad_shape(arr, target_len, fill_value=np.nan):
-    '''
-    Pads an array to the target length with specified fill values
-    
-    INPUTS:
-    - arr: Input array to be padded.
-    - target_len: The target length of the first dimension after padding.
-    - fill_value: The value to use for padding (default: np.nan).
-    
-    OUTPUTS:
-    - Padded array with shape (target_len, ...) matching the input dimensions.
-    '''
-
-    if len(arr) < target_len:
-        pad_shape = (target_len - len(arr),) + arr.shape[1:]
-        padding = np.full(pad_shape, fill_value)
-        return np.concatenate((arr, padding))
-    
-    return arr
-
-
-def sort_people_sports2d(keyptpre, keypt, scores=None):
-    '''
-    Associate persons across frames (Sports2D method)
-    Persons' indices are sometimes swapped when changing frame
-    A person is associated to another in the next frame when they are at a small distance
-    
-    N.B.: Requires min_with_single_indices and euclidian_distance function (see common.py)
-
-    INPUTS:
-    - keyptpre: (K, L, M) array of 2D coordinates for K persons in the previous frame, L keypoints, M 2D coordinates
-    - keypt: idem keyptpre, for current frame
-    - score: (K, L) array of confidence scores for K persons, L keypoints (optional) 
-    
-    OUTPUTS:
-    - sorted_prev_keypoints: array with reordered persons with values of previous frame if current is empty
-    - sorted_keypoints: array with reordered persons --> if scores is not None
-    - sorted_scores: array with reordered scores     --> if scores is not None
-    - associated_tuples: list of tuples with correspondences between persons across frames --> if scores is None (for Pose2Sim.triangulation())
-    '''
-    
-    # Generate possible person correspondences across frames
-    max_len = max(len(keyptpre), len(keypt))
-    keyptpre = pad_shape(keyptpre, max_len, fill_value=np.nan)
-    keypt = pad_shape(keypt, max_len, fill_value=np.nan)
-    if scores is not None:
-        scores = pad_shape(scores, max_len, fill_value=np.nan)
-    
-    # Compute distance between persons from one frame to another
-    personsIDs_comb = sorted(list(it.product(range(len(keyptpre)), range(len(keypt)))))
-    frame_by_frame_dist = [euclidean_distance(keyptpre[comb[0]],keypt[comb[1]]) for comb in personsIDs_comb]
-    frame_by_frame_dist = np.mean(frame_by_frame_dist, axis=1)
-    
-    # Sort correspondences by distance
-    _, _, associated_tuples = min_with_single_indices(frame_by_frame_dist, personsIDs_comb)
-    
-    # Associate points to same index across frames, nan if no correspondence
-    sorted_keypoints = []
-    for i in range(len(keyptpre)):
-        id_in_old =  associated_tuples[:,1][associated_tuples[:,0] == i].tolist()
-        if len(id_in_old) > 0:      sorted_keypoints += [keypt[id_in_old[0]]]
-        else:                       sorted_keypoints += [keypt[i]]
-    sorted_keypoints = np.array(sorted_keypoints)
-
-    if scores is not None:
-        sorted_scores = []
-        for i in range(len(keyptpre)):
-            id_in_old =  associated_tuples[:,1][associated_tuples[:,0] == i].tolist()
-            if len(id_in_old) > 0:  sorted_scores += [scores[id_in_old[0]]]
-            else:                   sorted_scores += [scores[i]]
-        sorted_scores = np.array(sorted_scores)
-
-    # Keep track of previous values even when missing for more than one frame
-    sorted_prev_keypoints = np.where(np.isnan(sorted_keypoints) & ~np.isnan(keyptpre), keyptpre, sorted_keypoints)
-    
-    if scores is not None:
-        return sorted_prev_keypoints, sorted_keypoints, sorted_scores
-    else: # For Pose2Sim.triangulation()
-        return sorted_keypoints, associated_tuples
-
-
-def sort_people_rtmlib(pose_tracker, keypoints, scores):
-    '''
-    Associate persons across frames (RTMLib method)
-
-    INPUTS:
-    - pose_tracker: PoseTracker. The initialized RTMLib pose tracker object
-    - keypoints: array of shape K, L, M with K the number of detected persons,
-    L the number of detected keypoints, M their 2D coordinates
-    - scores: array of shape K, L with K the number of detected persons,
-    L the confidence of detected keypoints
-
-    OUTPUT:
-    - sorted_keypoints: array with reordered persons
-    - sorted_scores: array with reordered scores
-    '''
-    
-    try:
-        desired_size = max(pose_tracker.track_ids_last_frame)+1
-        sorted_keypoints = np.full((desired_size, keypoints.shape[1], 2), np.nan)
-        sorted_keypoints[pose_tracker.track_ids_last_frame] = keypoints[:len(pose_tracker.track_ids_last_frame), :, :]
-        sorted_scores = np.full((desired_size, scores.shape[1]), np.nan)
-        sorted_scores[pose_tracker.track_ids_last_frame] = scores[:len(pose_tracker.track_ids_last_frame), :]
-    except:
-        sorted_keypoints, sorted_scores = keypoints, scores
-
-    return sorted_keypoints, sorted_scores
-
-
 def draw_dotted_line(img, start, direction, length, color=(0, 255, 0), gap=7, dot_length=3, thickness=thickness):
     '''
     Draw a dotted line with on a cv2 image
@@ -516,109 +362,6 @@ def draw_dotted_line(img, start, direction, length, color=(0, 255, 0), gap=7, do
         cv2.line(img, tuple(line_start.astype(int)), tuple(line_end.astype(int)), color, thickness)
 
 
-def draw_bounding_box(img, X, Y, colors=[(255, 0, 0), (0, 255, 0), (0, 0, 255)], fontSize=0.3, thickness=1):
-    '''
-    Draw bounding boxes and person ID around list of lists of X and Y coordinates.
-    Bounding boxes have a different color for each person.
-    
-    INPUTS:
-    - img: opencv image
-    - X: list of list of x coordinates
-    - Y: list of list of y coordinates
-    - colors: list of colors to cycle through
-    
-    OUTPUT:
-    - img: image with rectangles and person IDs
-    '''
-   
-    color_cycle = it.cycle(colors)
-
-    for i,(x,y) in enumerate(zip(X,Y)):
-        color = next(color_cycle)
-        if not np.isnan(x).all():
-            x_min, y_min = np.nanmin(x).astype(int), np.nanmin(y).astype(int)
-            x_max, y_max = np.nanmax(x).astype(int), np.nanmax(y).astype(int)
-            if x_min < 0: x_min = 0
-            if x_max > img.shape[1]: x_max = img.shape[1]
-            if y_min < 0: y_min = 0
-            if y_max > img.shape[0]: y_max = img.shape[0]
-
-            # Draw rectangles
-            cv2.rectangle(img, (x_min-25, y_min-25), (x_max+25, y_max+25), color, thickness) 
-        
-            # Write person ID
-            cv2.putText(img, str(i), (x_min-30, y_min-30), cv2.FONT_HERSHEY_SIMPLEX, fontSize+1, color, 2, cv2.LINE_AA) 
-    
-    return img
-
-
-def draw_skel(img, X, Y, model, colors=[(255, 0, 0), (0, 255, 0), (0, 0, 255)]):
-    '''
-    Draws keypoints and skeleton for each person.
-    Skeletons have a different color for each person.
-
-    INPUTS:
-    - img: opencv image
-    - X: list of list of x coordinates
-    - Y: list of list of y coordinates
-    - model: skeleton model (from skeletons.py)
-    - colors: list of colors to cycle through
-    
-    OUTPUT:
-    - img: image with keypoints and skeleton
-    '''
-    
-    # Get (unique) pairs between which to draw a line
-    node_pairs = []
-    for data_i in PreOrderIter(model.root, filter_=lambda node: node.is_leaf):
-        node_branches = [node_i.id for node_i in data_i.path]
-        node_pairs += [[node_branches[i],node_branches[i+1]] for i in range(len(node_branches)-1)]
-    node_pairs = [list(x) for x in set(tuple(x) for x in node_pairs)]
-    
-    # Draw lines
-    color_cycle = it.cycle(colors)
-    for (x,y) in zip(X,Y):
-        c = next(color_cycle)
-        if not np.isnan(x).all():
-            [cv2.line(img,
-                (int(x[n[0]]), int(y[n[0]])), (int(x[n[1]]), int(y[n[1]])), c, thickness)
-                for n in node_pairs
-                if not None in n and not (np.isnan(x[n[0]]) or np.isnan(y[n[0]]) or np.isnan(x[n[1]]) or np.isnan(y[n[1]]))] # IF NOT NONE
-
-    return img
-
-
-def draw_keypts(img, X, Y, scores, cmap_str='RdYlGn'):
-    '''
-    Draws keypoints and skeleton for each person.
-    Keypoints' colors depend on their score.
-
-    INPUTS:
-    - img: opencv image
-    - X: list of list of x coordinates
-    - Y: list of list of y coordinates
-    - scores: list of list of scores
-    - cmap_str: colormap name
-    
-    OUTPUT:
-    - img: image with keypoints and skeleton
-    '''
-    
-    scores = np.where(np.isnan(scores), 0, scores)
-    # scores = (scores - 0.4) / (1-0.4) # to get a red color for scores lower than 0.4
-    scores = np.where(scores>0.99, 0.99, scores)
-    scores = np.where(scores<0, 0, scores)
-    
-    cmap = plt.get_cmap(cmap_str)
-    for (x,y,s) in zip(X,Y,scores):
-        c_k = np.array(cmap(s))[:,:-1]*255
-        [cv2.circle(img, (int(x[i]), int(y[i])), thickness+4, c_k[i][::-1], -1)
-            for i in range(len(x))
-            if not (np.isnan(x[i]) or np.isnan(y[i]))]
-
-    return img
-
-
 def draw_angles(img, valid_X, valid_Y, valid_angles, valid_X_flipped, keypoints_ids, keypoints_names, angle_names, display_angle_values_on= ['body', 'list'], colors=[(255, 0, 0), (0, 255, 0), (0, 0, 255)], fontSize=0.3, thickness=1):
     '''
     Draw angles on the image.
@@ -1044,9 +787,9 @@ def get_personID_with_highest_scores(all_frames_scores):
     return person_id
 
 
-def compute_floor_line(trc_data, keypoint_names = ['LBigToe', 'RBigToe'], toe_speed_below = 1.0, tot_speed_above=2.0):
+def compute_floor_line(trc_data, keypoint_names = ['LBigToe', 'RBigToe'], toe_speed_below = 7, tot_speed_above=2.0):
     '''
-    Compute the floor line equation and angle 
+    Compute the floor line equation, angle, and direction
     from the feet keypoints when they have zero speed.
 
     N.B.: Y coordinates point downwards
@@ -1059,6 +802,7 @@ def compute_floor_line(trc_data, keypoint_names = ['LBigToe', 'RBigToe'], toe_sp
     OUTPUT:
     - angle: float. The angle of the floor line in radians
     - xy_origin: list. The origin of the floor line
+    - gait_direction: float. Left if < 0, 'right' otherwise
     '''
 
     # Remove frames where the person is mostly not moving (outlier)
@@ -1067,36 +811,45 @@ def compute_floor_line(trc_data, keypoint_names = ['LBigToe', 'RBigToe'], toe_sp
 
     # Retrieve zero-speed coordinates for the foot
     low_speeds_X, low_speeds_Y = [], []
+    gait_direction_val = []
     for kpt in keypoint_names:
         speeds = np.linalg.norm(trc_data[kpt].diff(), axis=1)
-        
+
         low_speed_frames = trc_data[speeds<toe_speed_below].index
         low_speeds_coords = trc_data[kpt].loc[low_speed_frames]
         low_speeds_coords = low_speeds_coords[low_speeds_coords!=0]
 
-        low_speeds_X += low_speeds_coords.iloc[:,0].tolist()
+        low_speeds_X_kpt = low_speeds_coords.iloc[:,0].tolist()
+        low_speeds_X += low_speeds_X_kpt
         low_speeds_Y += low_speeds_coords.iloc[:,1].tolist()
 
+        # gait direction (between [-1,1])
+        X_trend_val = np.polyfit(range(len(low_speeds_X_kpt)), low_speeds_X_kpt, 1)[0]
+        gait_direction_kpt = X_trend_val * len(low_speeds_X_kpt) / (np.max(low_speeds_X_kpt) - np.min(low_speeds_X_kpt))
+        gait_direction_val.append(gait_direction_kpt)
+
     # Fit a line to the zero-speed coordinates
     floor_line = np.polyfit(low_speeds_X, low_speeds_Y, 1) # (slope, intercept)
-    xy_origin = [0, floor_line[1]]
-
-    # Compute the angle of the floor line in degrees
-    angle = -np.arctan(floor_line[0])
+    angle = -np.arctan(floor_line[0]) # angle of the floor line in degrees
+    xy_origin = [0, floor_line[1]] # origin of the floor line
 
-    return angle, xy_origin
+    # Gait direction
+    gait_direction = np.mean(gait_direction_val)
+    
+    return angle, xy_origin, gait_direction
 
 
-def convert_px_to_meters(Q_coords_kpt, person_height_m, height_px, cx, cy, floor_angle):
+def convert_px_to_meters(Q_coords_kpt, px_to_m_person_height_m, height_px, cx, cy, floor_angle, visible_side='none'):
     '''
     Convert pixel coordinates to meters.
 
     INPUTS:
     - Q_coords_kpt: pd.DataFrame. The xyz coordinates of a keypoint in pixels, with z filled with zeros
-    - person_height_m: float. The height of the person in meters
+    - px_to_m_person_height_m: float. The height of the person in meters
     - height_px: float. The height of the person in pixels
     - cx, cy: float. The origin of the image in pixels
     - floor_angle: float. The angle of the floor in radians
+    - visible_side: str. The side of the person that is visible ('right', 'left', 'front', 'back', 'none')
 
     OUTPUT:
     - Q_coords_kpt_m: pd.DataFrame. The XYZ coordinates of a keypoint in meters
@@ -1105,10 +858,17 @@ def convert_px_to_meters(Q_coords_kpt, person_height_m, height_px, cx, cy, floor
     u = Q_coords_kpt.iloc[:,0]
     v = Q_coords_kpt.iloc[:,1]
 
-    X = person_height_m / height_px * ((u-cx) + (v-cy)*np.sin(floor_angle))
-    Y = - person_height_m / height_px * np.cos(floor_angle) * (v-cy - np.tan(floor_angle)*(u-cx))
+    X = px_to_m_person_height_m / height_px * ((u-cx) + (v-cy)*np.sin(floor_angle))
+    Y = - px_to_m_person_height_m / height_px * np.cos(floor_angle) * (v-cy - np.tan(floor_angle)*(u-cx))
+
+    if 'marker_Z_positions' in globals() and visible_side!='none':
+        marker_name = Q_coords_kpt.columns[0]
+        Z = X.copy()
+        Z[:] = marker_Z_positions[visible_side][marker_name]
+    else:
+        Z = np.zeros_like(X)
 
-    Q_coords_kpt_m = pd.DataFrame(np.array([X, Y, np.zeros_like(X)]).T, columns=Q_coords_kpt.columns)
+    Q_coords_kpt_m = pd.DataFrame(np.array([X, Y, Z]).T, columns=Q_coords_kpt.columns)
 
     return Q_coords_kpt_m
 
@@ -1159,11 +919,13 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     
     # Base parameters
     video_dir = Path(config_dict.get('project').get('video_dir'))
-    person_height_m = config_dict.get('project').get('person_height')
+    px_to_m_from_person_id = int(config_dict.get('project').get('px_to_m_from_person_id'))
+    px_to_m_person_height_m = config_dict.get('project').get('px_to_m_person_height')
+    visible_side = config_dict.get('project').get('visible_side')
     # Pose from file
-    load_trc = config_dict.get('project').get('load_trc')
-    if load_trc == '': load_trc = None
-    else: load_trc = Path(load_trc).resolve()
+    load_trc_px = config_dict.get('project').get('load_trc_px')
+    if load_trc_px == '': load_trc_px = None
+    else: load_trc_px = Path(load_trc_px).resolve()
     compare = config_dict.get('project').get('compare')
     # Webcam settings
     webcam_id =  config_dict.get('project').get('webcam_id')
@@ -1184,18 +946,28 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     mode = config_dict.get('pose').get('mode')
     det_frequency = config_dict.get('pose').get('det_frequency')
     tracking_mode = config_dict.get('pose').get('tracking_mode')
+    if tracking_mode == 'deepsort':
+        deepsort_params = config_dict.get('pose').get('deepsort_params')
+        try:
+            deepsort_params = ast.literal_eval(deepsort_params)
+        except: # if within single quotes instead of double quotes when run with sports2d --mode """{dictionary}"""
+            deepsort_params = deepsort_params.strip("'").replace('\n', '').replace(" ", "").replace(",", '", "').replace(":", '":"').replace("{", '{"').replace("}", '"}').replace('":"/',':/').replace('":"\\',':\\')
+            deepsort_params = re.sub(r'"\[([^"]+)",\s?"([^"]+)\]"', r'[\1,\2]', deepsort_params) # changes "[640", "640]" to [640,640]
+            deepsort_params = json.loads(deepsort_params)
+        deepsort_tracker = DeepSort(**deepsort_params)
+        deepsort_tracker.tracker.tracks.clear()
     backend = config_dict.get('pose').get('backend')
     device = config_dict.get('pose').get('device')
     
     # Pixel to meters conversion
     to_meters = config_dict.get('px_to_meters_conversion').get('to_meters')
+    make_c3d = config_dict.get('px_to_meters_conversion').get('make_c3d')
     save_calib = config_dict.get('px_to_meters_conversion').get('save_calib')
     # Calibration from file
     calib_file = config_dict.get('px_to_meters_conversion').get('calib_file')
     if calib_file == '': calib_file = None
     else: calib_file = Path(calib_file).resolve()
     # Calibration from person height
-    calib_on_person_id = int(config_dict.get('px_to_meters_conversion').get('calib_on_person_id'))
     floor_angle = config_dict.get('px_to_meters_conversion').get('floor_angle') # 'auto' or float
     floor_angle = np.radians(float(floor_angle)) if floor_angle != 'auto' else floor_angle
     xy_origin = config_dict.get('px_to_meters_conversion').get('xy_origin') # ['auto'] or [x, y]    
@@ -1239,9 +1011,21 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                            gaussian_filter_kernel, loess_filter_kernel, median_filter_kernel]
 
     # Inverse kinematics settings
-    do_ik = config_dict.get('inverse-kinematics').get('do_ik')
-    osim_setup_path = config_dict.get('inverse-kinematics').get('osim_setup_path')
-    person_orientations = config_dict.get('inverse-kinematics').get('person_orientation')
+    do_ik = config_dict.get('kinematics').get('do_ik')
+    use_augmentation = config_dict.get('kinematics').get('use_augmentation')
+    use_contacts_muscles = config_dict.get('kinematics').get('use_contacts_muscles')
+    
+    osim_setup_path = config_dict.get('kinematics').get('osim_setup_path')
+    right_left_symmetry = config_dict.get('kinematics').get('right_left_symmetry')
+    default_height = config_dict.get('kinematics').get('default_height')
+    remove_scaling_setup = config_dict.get('kinematics').get('remove_individual_scaling_setup')
+    remove_ik_setup = config_dict.get('kinematics').get('remove_individual_ik_setup')
+    fastest_frames_to_remove_percent = config_dict.get('kinematics').get('fastest_frames_to_remove_percent')
+    large_hip_knee_angles = config_dict.get('kinematics').get('large_hip_knee_angles')
+    trimmed_extrema_percent = config_dict.get('kinematics').get('trimmed_extrema_percent')
+    close_to_zero_speed = config_dict.get('kinematics').get('close_to_zero_speed_m')
+
+    if do_ik: from Pose2Sim import Pose2Sim
 
     # Create output directories
     if video_file == "webcam":
@@ -1321,15 +1105,15 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             logging.warning("\nInvalid mode. Must be 'lightweight', 'balanced', 'performance', or '''{dictionary}''' of parameters within triple quotes. Make sure input_sizes are within square brackets.")
             logging.warning('Using the default "balanced" mode.')
             mode = 'balanced'
-
     
+
     # Skip pose estimation or set it up:
-    if load_trc:
-        if not '_px' in str(load_trc): 
-            logging.error(f'\n{load_trc} file needs to be in px, not in meters.')
-        logging.info(f'\nUsing a pose file instead of running pose estimation and tracking: {load_trc}.')
+    if load_trc_px:
+        if not '_px' in str(load_trc_px): 
+            logging.error(f'\n{load_trc_px} file needs to be in px, not in meters.')
+        logging.info(f'\nUsing a pose file instead of running pose estimation and tracking: {load_trc_px}.')
         # Load pose file in px
-        Q_coords, _, _, keypoints_names, _ = read_trc(load_trc)
+        Q_coords, _, _, keypoints_names, _ = read_trc(load_trc_px)
         keypoints_ids = [i for i in range(len(keypoints_names))]
         keypoints_all, scores_all = load_pose_file(Q_coords)
         for pre, _, node in RenderTree(model_name):
@@ -1341,12 +1125,21 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
         keypoints_ids = [node.id for _, _, node in RenderTree(pose_model) if node.id!=None]
         keypoints_names = [node.name for _, _, node in RenderTree(pose_model) if node.id!=None]
 
-        tracking_rtmlib = True if (tracking_mode == 'rtmlib' and multiperson) else False
-        pose_tracker = setup_pose_tracker(ModelClass, det_frequency, mode, tracking_rtmlib, backend, device)
+        # Set up pose tracker
+        try:
+            pose_tracker = setup_pose_tracker(ModelClass, det_frequency, mode, False, backend, device)
+        except:
+            logging.error('Error: Pose estimation failed. Check in Config.toml that pose_model and mode are valid.')
+            raise ValueError('Error: Pose estimation failed. Check in Config.toml that pose_model and mode are valid.')
+        
+        if tracking_mode not in ['deepsort', 'sports2d']:
+            logging.warning(f"Tracking mode {tracking_mode} not recognized. Using sports2d method.")
+            tracking_mode = 'sports2d'
         logging.info(f'\nPose tracking set up for "{pose_model_name}" model.')
         logging.info(f'Mode: {mode}.\n')
-        logging.info(f'Persons are detected every {det_frequency} frames and tracked inbetween. Multi-person is {"" if multiperson else "not "}selected.')
-        logging.info(f"Parameters: {keypoint_likelihood_threshold=}, {average_likelihood_threshold=}, {keypoint_number_threshold=}")
+        logging.info(f'Persons are detected every {det_frequency} frames and tracked inbetween. Multi-person is {"" if multiperson else "not "}selected. Tracking is done with {tracking_mode}.')
+        if tracking_mode == 'deepsort': logging.info(f'Deepsort parameters: {deepsort_params}.')
+        logging.info(f"{keypoint_likelihood_threshold=}, {average_likelihood_threshold=}, {keypoint_number_threshold=}")
 
     if flip_left_right:
         try:
@@ -1375,7 +1168,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     frame_count = 0
     while cap.isOpened():
         # Skip to the starting frame
-        if frame_count < frame_range[0] and not load_trc:
+        if frame_count < frame_range[0] and not load_trc_px:
             cap.read()
             frame_count += 1
             continue
@@ -1383,25 +1176,25 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
         for frame_nb in frame_iterator:
             start_time = datetime.now()
             success, frame = cap.read()
+            frame_count += 1
 
             # If frame not grabbed
             if not success:
-                logging.warning(f"Failed to grab frame {frame_count}.")
+                logging.warning(f"Failed to grab frame {frame_count-1}.")
                 if save_pose:
                     all_frames_X.append([])
                     all_frames_Y.append([])
                     all_frames_scores.append([])
                 if save_angles:
                     all_frames_angles.append([])
-                frame_count += 1
                 continue
             else:
                 cv2.putText(frame, f"Press 'q' to quit", (cam_width-int(400*fontSize), cam_height-20), cv2.FONT_HERSHEY_SIMPLEX, fontSize+0.2, (255,255,255), thickness+1, cv2.LINE_AA)
                 cv2.putText(frame, f"Press 'q' to quit", (cam_width-int(400*fontSize), cam_height-20), cv2.FONT_HERSHEY_SIMPLEX, fontSize+0.2, (0,0,255), thickness, cv2.LINE_AA)
-                frame_count += 1
+
 
             # Retrieve pose or Estimate pose and track people
-            if load_trc: 
+            if load_trc_px: 
                 if frame_nb >= len(keypoints_all):
                     break
                 keypoints = keypoints_all[frame_nb]
@@ -1409,19 +1202,20 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             else: 
                 # Detect poses
                 keypoints, scores = pose_tracker(frame)
-                # Track persons
-                if tracking_rtmlib:
-                    keypoints, scores = sort_people_rtmlib(pose_tracker, keypoints, scores)
-                else:
+
+                # Track poses across frames
+                if tracking_mode == 'deepsort':
+                    keypoints, scores = sort_people_deepsort(keypoints, scores, deepsort_tracker, frame, frame_count)
+                if tracking_mode == 'sports2d': 
                     if 'prev_keypoints' not in locals(): prev_keypoints = keypoints
                     prev_keypoints, keypoints, scores = sort_people_sports2d(prev_keypoints, keypoints, scores=scores)
-                
+                        
             
             # Process coordinates and compute angles
             valid_X, valid_Y, valid_scores = [], [], []
             valid_X_flipped, valid_angles = [], []
             for person_idx in range(len(keypoints)):
-                if load_trc:
+                if load_trc_px:
                     person_X = keypoints[person_idx][:,0]
                     person_Y = keypoints[person_idx][:,1]
                     person_scores = scores[person_idx]
@@ -1478,7 +1272,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                 img = frame.copy()
                 img = draw_bounding_box(img, valid_X, valid_Y, colors=colors, fontSize=fontSize, thickness=thickness)
                 img = draw_keypts(img, valid_X, valid_Y, valid_scores, cmap_str='RdYlGn')
-                img = draw_skel(img, valid_X, valid_Y, pose_model, colors=colors)
+                img = draw_skel(img, valid_X, valid_Y, pose_model)
                 if calculate_angles:
                     img = draw_angles(img, valid_X, valid_Y, valid_angles, valid_X_flipped, new_keypoints_ids, new_keypoints_names, angle_names, display_angle_values_on=display_angle_values_on, colors=colors, fontSize=fontSize, thickness=thickness)
 
@@ -1530,8 +1324,8 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     frame_range = [0,frame_count] if video_file == 'webcam' else frame_range
     all_frames_time = pd.Series(np.linspace(frame_range[0]/fps, frame_range[1]/fps, frame_count+1), name='time')
     if not multiperson:
-        calib_on_person_id = get_personID_with_highest_scores(all_frames_scores)
-        detected_persons = [calib_on_person_id]
+        px_to_m_from_person_id = get_personID_with_highest_scores(all_frames_scores)
+        detected_persons = [px_to_m_from_person_id]
     else:
         detected_persons = range(all_frames_X_homog.shape[1])
 
@@ -1604,7 +1398,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                 # Build TRC file
                 trc_data_i = trc_data_from_XYZtime(all_frames_X_person_filt, all_frames_Y_person_filt, all_frames_Z_homog, all_frames_time)
                 trc_data.append(trc_data_i)
-                if not load_trc:
+                if not load_trc_px:
                     make_trc_with_trc_data(trc_data_i, str(pose_path_person), fps=fps)
                     logging.info(f'Pose in pixels saved to {pose_path_person.resolve()}.')
 
@@ -1619,9 +1413,9 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
         # Convert px to meters
         if to_meters:
             logging.info('\nConverting pose to meters:')
-            if calib_on_person_id>=len(trc_data):
-                logging.warning(f'Person #{calib_on_person_id} not detected in the video. Calibrating on person #0 instead.')
-                calib_on_person_id = 0
+            if px_to_m_from_person_id>=len(trc_data):
+                logging.warning(f'Person #{px_to_m_from_person_id} not detected in the video. Calibrating on person #0 instead.')
+                px_to_m_from_person_id = 0
             if calib_file:
                 logging.info(f'Using calibration file to convert coordinates in meters: {calib_file}.')
                 calib_params_dict = retrieve_calib_params(calib_file)
@@ -1630,43 +1424,68 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             else:
                 # Compute calibration parameters
                 if not multiperson: 
-                    selected_person_id = calib_on_person_id
-                    calib_on_person_id = 0
-                height_px = compute_height(trc_data[calib_on_person_id].iloc[:,1:], keypoints_names,
+                    selected_person_id = px_to_m_from_person_id
+                    px_to_m_from_person_id = 0
+                height_px = compute_height(trc_data[px_to_m_from_person_id].iloc[:,1:], keypoints_names,
                                             fastest_frames_to_remove_percent=fastest_frames_to_remove_percent, close_to_zero_speed=close_to_zero_speed_px, large_hip_knee_angles=large_hip_knee_angles, trimmed_extrema_percent=trimmed_extrema_percent)
 
+                toe_speed_below = 1 # m/s (below which the foot is considered to be stationary)
+                px_per_m = height_px/px_to_m_person_height_m
+                toe_speed_below_px_frame = toe_speed_below * px_per_m / fps
                 if floor_angle == 'auto' or xy_origin == 'auto':
                     # estimated from the line formed by the toes when they are on the ground (where speed = 0)
                     try:
-                        toe_speed_below = 1 # m/s (below which the foot is considered to be stationary)
-                        px_per_m = height_px/person_height_m
-                        toe_speed_below_px_frame = toe_speed_below * px_per_m / fps
-                        try:
-                            floor_angle_estim, xy_origin_estim = compute_floor_line(trc_data[calib_on_person_id], keypoint_names=['LBigToe', 'RBigToe'], toe_speed_below=toe_speed_below_px_frame)
-                        except: # no feet points
-                            floor_angle_estim, xy_origin_estim = compute_floor_line(trc_data[calib_on_person_id], keypoint_names=['LAnkle', 'RAnkle'], toe_speed_below=toe_speed_below_px_frame)
+                        if all(key in trc_data[px_to_m_from_person_id] for key in ['LBigToe', 'RBigToe']):
+                            floor_angle_estim, xy_origin_estim, _ = compute_floor_line(trc_data[px_to_m_from_person_id], keypoint_names=['LBigToe', 'RBigToe'], toe_speed_below=toe_speed_below_px_frame)
+                        else:
+                            floor_angle_estim, xy_origin_estim, _ = compute_floor_line(trc_data[px_to_m_from_person_id], keypoint_names=['LAnkle', 'RAnkle'], toe_speed_below=toe_speed_below_px_frame)
                             xy_origin_estim[0] = xy_origin_estim[0]-0.13
                             logging.warning(f'The RBigToe and LBigToe are missing from your model. Using ankles - 13 cm to compute the floor line.')
                     except:
                         floor_angle_estim = 0
                         xy_origin_estim = cam_width/2, cam_height/2
-                        logging.warning(f'Could not estimate the floor angle and xy_origin. Make sure that the full body is visible. Using floor angle = 0° and xy_origin = [{cam_width/2}, {cam_height/2}].')
+                        logging.warning(f'Could not estimate the floor angle and xy_origin for person {px_to_m_from_person_id}. Make sure that the full body is visible. Using floor angle = 0° and xy_origin = [{cam_width/2}, {cam_height/2}].')
                 if not floor_angle == 'auto':
                     floor_angle_estim = floor_angle
                 if xy_origin == 'auto':
                     cx, cy = xy_origin_estim
                 else:
                     cx, cy = xy_origin
-                logging.info(f'Using height of person #{calib_on_person_id} ({person_height_m}m) to convert coordinates in meters. '
+                logging.info(f'Using height of person #{px_to_m_from_person_id} ({px_to_m_person_height_m}m) to convert coordinates in meters. '
                              f'Floor angle: {np.degrees(floor_angle_estim) if not floor_angle=="auto" else f"auto (estimation: {round(np.degrees(floor_angle_estim),2)}°)"}, '
                              f'xy_origin: {xy_origin if not xy_origin=="auto" else f"auto (estimation: {[round(c) for c in xy_origin_estim]})"}.')
 
             # Coordinates in m
             for i in range(len(trc_data)):
+                # print(i)
                 if not np.array(trc_data[i].iloc[:,1:] ==0).all():
-                    trc_data_m_i = pd.concat([convert_px_to_meters(trc_data[i][kpt_name], person_height_m, height_px, cx, cy, -floor_angle_estim) for kpt_name in keypoints_names], axis=1)
+                    # Automatically determine visible side
+                    visible_side_i = visible_side[i] if len(visible_side)>i else 'auto' # set to 'auto' if list too short
+                    
+                    # Set to 'front' if slope of X values between [-5,5]
+                    if visible_side_i == 'auto':
+                        try:
+                            if all(key in trc_data[i] for key in ['LBigToe', 'RBigToe']):
+                                _, _, gait_direction = compute_floor_line(trc_data[i], keypoint_names=['LBigToe', 'RBigToe'], toe_speed_below=toe_speed_below_px_frame)
+                            else:
+                                _, _, gait_direction = compute_floor_line(trc_data[i], keypoint_names=['LAnkle', 'RAnkle'], toe_speed_below=toe_speed_below_px_frame)
+                                logging.warning(f'The RBigToe and LBigToe are missing from your model. Gait direction will be determined from the ankle points.')
+                            visible_side_i = 'right' if gait_direction > 0.6 \
+                                                else 'left' if gait_direction < -0.6 \
+                                                else 'front'
+                        except:
+                            visible_side_i = 'none'
+                            logging.warning(f'Could not automatically find gait direction for person {i}. Please set visible_side to "front", "back", "left", or "right" for this person. Setting to "none".')
+
+                    # skip if none
+                    if visible_side_i == 'none':
+                        logging.info(f'Skipping because "visible_side" is set to none for person {i}.')
+                        continue
+                    
+                    # Convert to meters
+                    trc_data_m_i = pd.concat([convert_px_to_meters(trc_data[i][kpt_name], px_to_m_person_height_m, height_px, cx, cy, -floor_angle_estim, visible_side=visible_side_i) for kpt_name in keypoints_names], axis=1)
                     trc_data_m_i.insert(0, 't', all_frames_time)
-                    trc_data_unfiltered_m_i = pd.concat([convert_px_to_meters(trc_data_unfiltered[i][kpt_name], person_height_m, height_px, cx, cy, -floor_angle_estim) for kpt_name in keypoints_names], axis=1)
+                    trc_data_unfiltered_m_i = pd.concat([convert_px_to_meters(trc_data_unfiltered[i][kpt_name], px_to_m_person_height_m, height_px, cx, cy, -floor_angle_estim) for kpt_name in keypoints_names], axis=1)
                     trc_data_unfiltered_m_i.insert(0, 't', all_frames_time)
 
                     if to_meters and show_plots:
@@ -1676,7 +1495,9 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                     idx_path = selected_person_id if not multiperson and not calib_file else i
                     pose_path_person_m_i = (pose_output_path.parent / (pose_output_path_m.stem + f'_person{idx_path:02d}.trc'))
                     make_trc_with_trc_data(trc_data_m_i, pose_path_person_m_i, fps=fps)
-                    logging.info(f'Person {idx_path}: Pose in meters saved to {pose_path_person_m_i.resolve()}.')
+                    if make_c3d:
+                        c3d_path = convert_to_c3d(pose_path_person_m_i)
+                    logging.info(f'Person {idx_path}: Pose in meters saved to {pose_path_person_m_i.resolve()}. {"Also saved in c3d format." if make_c3d else ""}')
                     
                 
             
@@ -1694,7 +1515,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
 
 
                 # z = 3.0 # distance between the camera and the person. Required in the calibration file but simplified in the equations
-                # f = height_px / person_height_m * z
+                # f = height_px / px_to_m_person_height_m * z
 
 
                 # # Name
@@ -1728,7 +1549,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
 
     # Post-processing angles
     if save_angles and calculate_angles:
-        logging.info('\nPost-processing angles:')
+        logging.info('\nPost-processing angles (without inverse kinematics):')
         all_frames_angles = make_homogeneous(all_frames_angles)
         
         # unwrap angles
@@ -1805,3 +1626,18 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                 if show_plots:
                     all_frames_angles_person.insert(0, 't', all_frames_time)
                     angle_plots(all_frames_angles_person, angle_data, i) # i = current person
+
+
+    # # Run scaling and inverse kinematics
+    # if save_angles and calculate_angles and do_ik:
+    #     logging.info('\nPost-processing angles (with inverse kinematics):')
+    #     if not to_meters:
+    #         logging.error('IK requires positions in meters rather than in pixels. Set to_meters to True.')
+    #         raise ValueError('IK requires positions in meters rather than in pixels. Set to_meters to True.')
+        
+
+    #     marker_Z_positions
+    #     if 'none': No IK possible. 
+    #     visible_side=='auto'
+
+    #     convert_to_c3d(trc_path)