sports2d 0.6.2__py3-none-any.whl → 0.7.0__py3-none-any.whl

Sports2D/process.py

@@ -56,10 +56,13 @@ import sys
 import logging
 import json
 import ast
+import shutil
+import os
 from functools import partial
 from datetime import datetime
 import itertools as it
 from tqdm import tqdm
+from collections import defaultdict
 from anytree import RenderTree
 
 import numpy as np
@@ -69,11 +72,14 @@ 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
+import opensim as osim
 
 from Sports2D.Utilities import filter
 from Sports2D.Utilities.common import *
 from Sports2D.Utilities.skeletons import *
 
+DEFAULT_MASS = 70
+DEFAULT_HEIGHT = 1.7
 
 ## AUTHORSHIP INFORMATION
 __author__ = "David Pagnon, HunMin Kim"
@@ -592,8 +598,8 @@ def load_pose_file(Q_coords):
     - scores_all: np.array. The scores in the format (Nframes, 1, Nmarkers)
     '''
 
-    Z_cols = [3*i+2 for i in range(len(Q_coords.columns)//3)]
-    Q_coords_xy = Q_coords.drop(Q_coords.columns[Z_cols], axis=1)
+    Z_cols = np.array([[3*i,3*i+1] for i in range(len(Q_coords.columns)//3)]).ravel()
+    Q_coords_xy = Q_coords.iloc[:,Z_cols]
     kpt_number = len(Q_coords_xy.columns)//2
 
     # shape (Nframes, 2*Nmarkers) --> (Nframes, 1, Nmarkers, 2)
@@ -787,9 +793,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
@@ -802,6 +808,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)
@@ -810,36 +817,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]]
+    angle = -np.arctan(floor_line[0]) # angle of the floor line in degrees
+    xy_origin = [0, floor_line[1]] # origin of the floor line
 
-    # Compute the angle of the floor line in degrees
-    angle = -np.arctan(floor_line[0])
-
-    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
@@ -848,10 +864,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))
 
-    Q_coords_kpt_m = pd.DataFrame(np.array([X, Y, np.zeros_like(X)]).T, columns=Q_coords_kpt.columns)
+    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, Z]).T, columns=Q_coords_kpt.columns)
 
     return Q_coords_kpt_m
 
@@ -902,11 +925,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')
@@ -942,23 +967,18 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     
     # 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]    
     xy_origin = [float(o) for o in xy_origin] if xy_origin != ['auto'] else 'auto'
 
-    fastest_frames_to_remove_percent = config_dict.get('px_to_meters_conversion').get('fastest_frames_to_remove_percent')
-    large_hip_knee_angles = config_dict.get('px_to_meters_conversion').get('large_hip_knee_angles')
-    trimmed_extrema_percent = config_dict.get('px_to_meters_conversion').get('trimmed_extrema_percent')
-    close_to_zero_speed_px = config_dict.get('px_to_meters_conversion').get('close_to_zero_speed_px')
-
     keypoint_likelihood_threshold = config_dict.get('pose').get('keypoint_likelihood_threshold')
     average_likelihood_threshold = config_dict.get('pose').get('average_likelihood_threshold')
     keypoint_number_threshold = config_dict.get('pose').get('keypoint_number_threshold')
@@ -991,11 +1011,6 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                            butterworth_filter_order, butterworth_filter_cutoff, frame_rate,
                            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')
-
     # Create output directories
     if video_file == "webcam":
         current_date = datetime.now().strftime("%Y%m%d_%H%M%S")
@@ -1014,6 +1029,32 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     if save_img:
         img_output_dir.mkdir(parents=True, exist_ok=True)
 
+    # Inverse kinematics settings
+    do_ik = config_dict.get('kinematics').get('do_ik')
+    use_augmentation = config_dict.get('kinematics').get('use_augmentation')
+    participant_masses = config_dict.get('kinematics').get('participant_mass')
+    participant_masses = participant_masses if isinstance(participant_masses, list) else [participant_masses]
+    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_px = config_dict.get('kinematics').get('close_to_zero_speed_px')
+    close_to_zero_speed_m = config_dict.get('kinematics').get('close_to_zero_speed_m')
+    if do_ik:
+        from Pose2Sim.markerAugmentation import augment_markers_all
+        from Pose2Sim.kinematics import kinematics_all
+        # Create a Pose2Sim dictionary and fill in missing keys
+        recursivedict = lambda: defaultdict(recursivedict)
+        Pose2Sim_config_dict = recursivedict()
+        # Fill Pose2Sim dictionary (height and mass will be filled later)
+        Pose2Sim_config_dict['project']['project_dir'] = str(output_dir)
+        Pose2Sim_config_dict['markerAugmentation']['make_c3d'] = make_c3d
+        Pose2Sim_config_dict['kinematics'] = config_dict.get('kinematics')
+        # Temporarily recreate Pose2Sim file hierarchy
+        pose3d_dir = Path(output_dir) / 'pose-3d'
+        pose3d_dir.mkdir(parents=True, exist_ok=True)
+        kinematics_dir = Path(output_dir) / 'kinematics'
+        kinematics_dir.mkdir(parents=True, exist_ok=True)
+
 
     # Set up video capture
     if video_file == "webcam":
@@ -1036,7 +1077,11 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
         model_name = 'HALPE_26'
         ModelClass = BodyWithFeet # 26 keypoints(halpe26)
         logging.info(f"Using HALPE_26 model (body and feet) for pose estimation.")
-    elif pose_model.upper() in ('COCO_133', 'WHOLE_BODY', 'WHOLE_BODY_WRIST'):
+    elif pose_model.upper() == 'WHOLE_BODY_WRIST':
+        model_name = 'COCO_133_WRIST'
+        ModelClass = Wholebody
+        logging.info(f"Using COCO_133 model (body, feet, 2 hand points) for pose estimation.")
+    elif pose_model.upper() in ('COCO_133', 'WHOLE_BODY'):
         model_name = 'COCO_133'
         ModelClass = Wholebody
         logging.info(f"Using COCO_133 model (body, feet, hands, and face) for pose estimation.")
@@ -1077,15 +1122,15 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
     
 
     # 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):
+        for pre, _, node in RenderTree(pose_model):
             if node.name in keypoints_names:
                 node.id = keypoints_names.index(node.name)
     
@@ -1137,7 +1182,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
@@ -1163,7 +1208,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
 
 
             # 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]
@@ -1184,7 +1229,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             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]
@@ -1210,7 +1255,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
                         person_X_flipped = flip_left_right_direction(person_X, L_R_direction_idx, keypoints_names, keypoints_ids)
                     else:
                         person_X_flipped = person_X.copy()
-                        
+                    
                     # Compute angles
                     person_angles = []
                     # Add Neck and Hip if not provided
@@ -1293,8 +1338,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])
 
@@ -1312,7 +1357,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
 
             # Delete person if less than 4 valid frames
             pose_nan_count = len(np.where(all_frames_X_person.sum(axis=1)==0)[0])
-            if frame_count - pose_nan_count <= 4:
+            if frame_count - frame_range[0] - pose_nan_count <= 4:
                 trc_data_i = pd.DataFrame(0, index=all_frames_X_person.index, columns=np.array([[c]*3 for c in all_frames_X_person.columns]).flatten())
                 trc_data_i.insert(0, 't', all_frames_time)
                 trc_data.append(trc_data_i)
@@ -1367,7 +1412,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()}.')
 
@@ -1380,11 +1425,12 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             
 
         # Convert px to meters
+        trc_data_m = []
         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)
@@ -1393,53 +1439,81 @@ 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}] px.')
                 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]})"}.')
+                             f'xy_origin: {xy_origin if not xy_origin=="auto" else f"auto (estimation: {[round(c) for c in xy_origin_estim]})"} px.')
 
             # Coordinates in m
             for i in range(len(trc_data)):
                 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'
+                            logging.info(f'- Person {i}: Seen from the {visible_side_i}.')
+                        except:
+                            visible_side_i = 'none'
+                            logging.warning(f'- Person {i}: Could not automatically find gait direction. Please set visible_side to "front", "back", "left", or "right" for this person. Setting to "none".')
+                    # skip if none
+                    elif visible_side_i == 'none':
+                        logging.info(f'- Person {i}: Keeping output in 2D because "visible_side" is set to "none" for person {i}.')
+                    else:
+                        logging.info(f'- Person {i}: Seen from the {visible_side_i}.')
+                    
+                    # 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:
                         pose_plots(trc_data_unfiltered_m_i, trc_data_m_i, i)
                     
                     # Write to trc file
+                    trc_data_m.append(trc_data_m_i)
                     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'Pose in meters saved to {pose_path_person_m_i.resolve()}. {"Also saved in c3d format." if make_c3d else ""}')
                     
                 
             
@@ -1447,9 +1521,6 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
 
 
 
-
-
-
                 # # plt.plot(trc_data_m.iloc[:,0], trc_data_m.iloc[:,1])
                 # # plt.ylim([0,2])
                 # # plt.show()
@@ -1457,7 +1528,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
@@ -1484,14 +1555,14 @@ 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
@@ -1508,7 +1579,7 @@ def process_fun(config_dict, video_file, time_range, frame_rate, result_dir):
             
             # Delete person if less than 4 valid frames
             angle_nan_count = len(np.where(all_frames_angles_person.sum(axis=1)==0)[0])
-            if frame_count - angle_nan_count <= 4:
+            if frame_count - frame_range[0] - angle_nan_count <= 4:
                 logging.info(f'- Person {i}: Less than 4 valid frames. Deleting person.')
 
             else:
@@ -1568,3 +1639,81 @@ 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
+
+
+    # OpenSim inverse kinematics (and optional marker augmentation)
+    if do_ik or use_augmentation:
+        logging.info('\nPost-processing angles (with inverse kinematics):')
+        if not to_meters:
+            logging.warning('Skipping marker augmentation and inverse kinematics as to_meters was set to False.')
+        else:
+            # move all trc files containing _m_ string to pose3d_dir
+            for trc_file in output_dir.glob('*_m_*.trc'):
+                if (pose3d_dir/trc_file.name).exists():
+                    os.remove(pose3d_dir/trc_file.name)
+                shutil.move(trc_file, pose3d_dir)
+
+            heights_m, masses = [], []
+            for i in range(len(trc_data_m)):
+                if do_ik and not use_augmentation:
+                    logging.info(f'- Person {i}: Running scaling and inverse kinematics without marker augmentation. Set use_augmentation to True if you need it.') 
+                elif not do_ik and use_augmentation:
+                    logging.info(f'- Person {i}: Running marker augmentation without inverse kinematics. Set do_ik to True if you need it.')
+                else:
+                    logging.info(f'- Person {i}: Running marker augmentation and inverse kinematics.')
+
+                # Delete person if less than 4 valid frames
+                pose_path_person = pose_output_path.parent / (pose_output_path.stem + f'_person{i:02d}.trc')
+                all_frames_X_person = pd.DataFrame(all_frames_X_homog[:,i,:], columns=keypoints_names)
+                pose_nan_count = len(np.where(all_frames_X_person.sum(axis=1)==0)[0])
+                if frame_count - frame_range[0] - pose_nan_count <= 4:
+                    # heights_m.append(DEFAULT_HEIGHT)
+                    # masses.append(DEFAULT_MASS)
+                    logging.info(f'Less than 4 valid frames. Deleting person.')
+                else:
+                    if visible_side[i] == 'none':
+                        logging.info(f'Skipping marker augmentation and inverse kinematics because visible_side is "none".')
+                        # heights_m.append(DEFAULT_HEIGHT)
+                        # masses.append(DEFAULT_MASS)
+                    else:
+                        # Provide missing data to Pose2Sim_config_dict
+                        height_m_i = compute_height(trc_data_m_i.iloc[:,1:], keypoints_names,
+                            fastest_frames_to_remove_percent=fastest_frames_to_remove_percent, close_to_zero_speed=close_to_zero_speed_m, large_hip_knee_angles=large_hip_knee_angles, trimmed_extrema_percent=trimmed_extrema_percent)
+                        mass_i = participant_masses[i] if len(participant_masses)>i else 70
+                        if len(participant_masses)<=i:
+                            logging.warning(f'No mass provided. Using 70 kg as default.')
+                        heights_m.append(height_m_i)
+                        masses.append(mass_i)
+                
+            Pose2Sim_config_dict['project']['participant_height'] = heights_m
+            Pose2Sim_config_dict['project']['participant_mass'] = masses
+            Pose2Sim_config_dict['pose']['pose_model'] = pose_model_name.upper()
+            Pose2Sim_config_dict = to_dict(Pose2Sim_config_dict)
+
+            # Marker augmentation
+            if use_augmentation:
+                logging.info('Running marker augmentation...')
+                augment_markers_all(Pose2Sim_config_dict)
+                logging.info(f'Augmented trc results saved to {pose3d_dir.resolve()}.\n')
+
+            if do_ik:
+                if not save_angles or not calculate_angles:
+                    logging.warning(f'Skipping inverse kinematics because save_angles or calculate_angles is set to False.')
+                else:
+                    logging.info('Running inverse kinematics...')
+                    kinematics_all(Pose2Sim_config_dict)
+                    for mot_file in kinematics_dir.glob('*.mot'):
+                        if (mot_file.parent/(mot_file.stem+'_ik.mot')).exists():
+                            os.remove(mot_file.parent/(mot_file.stem+'_ik.mot'))
+                        os.rename(mot_file, mot_file.parent/(mot_file.stem+'_ik.mot'))
+                    logging.info(f'.osim model and .mot motion file results saved to {kinematics_dir.resolve()}.\n')
+                            
+            # Move all files in pose-3d and kinematics to the output_dir
+            osim.Logger.removeFileSink()
+            for directory in [pose3d_dir, kinematics_dir]:
+                for file in directory.glob('*'):
+                    if (output_dir/file.name).exists():
+                        os.remove(output_dir/file.name)
+                    shutil.move(file, output_dir)
+            pose3d_dir.rmdir()
+            kinematics_dir.rmdir()