19efec2723
* tests synchro * draft * further draft * affinity ok * proposals okay, need to incorporate in Pose2Sim+tests * will transfer sorting across frames in triangulation in next commit * Lasts tests need to be done but seems to work pretty well * should all work smoothly * update readme * last checks * fixed linting issues * getting tired of being forgetful
482 lines
18 KiB
Python
482 lines
18 KiB
Python
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
|
|
'''
|
|
#########################################
|
|
## SYNCHRONIZE CAMERAS ##
|
|
#########################################
|
|
|
|
Steps undergone in this script
|
|
0. Converting json files to pandas dataframe
|
|
1. Computing speeds (vertical)
|
|
2. Plotting paired correlations of speeds from one camera viewpoint to another (work on one single keypoint, or on all keypoints, or on a weighted selection of keypoints)
|
|
3.
|
|
Ideally, this should be done automatically for all views, checking pairs 2 by 2 with the highest correlation coefficient,
|
|
and ask for confirmation before deleting the frames in question (actually renamed .json.del - reset_sync option in Config.toml).
|
|
'''
|
|
|
|
|
|
## INIT
|
|
import numpy as np
|
|
import pandas as pd
|
|
import matplotlib.pyplot as plt
|
|
from scipy import signal
|
|
from scipy import interpolate
|
|
import json
|
|
import os
|
|
import fnmatch
|
|
import pickle as pk
|
|
import re
|
|
|
|
from Pose2Sim.filtering import loess_filter_1d
|
|
|
|
|
|
## AUTHORSHIP INFORMATION
|
|
__author__ = "HunMin Kim, David Pagnon"
|
|
__copyright__ = "Copyright 2021, Pose2Sim"
|
|
__credits__ = ["David Pagnon"]
|
|
__license__ = "BSD 3-Clause License"
|
|
__version__ = '0.7'
|
|
__maintainer__ = "David Pagnon"
|
|
__email__ = "contact@david-pagnon.com"
|
|
__status__ = "Development"
|
|
|
|
|
|
# FUNCTIONS
|
|
def convert_json2pandas(json_dir):
|
|
'''
|
|
Convert JSON files in a directory to a pandas DataFrame.
|
|
|
|
INPUTS:
|
|
- json_dir: str. The directory path containing the JSON files.
|
|
|
|
OUTPUT:
|
|
- df_json_coords: dataframe. Extracted coordinates in a pandas dataframe.
|
|
'''
|
|
|
|
json_files_names = fnmatch.filter(os.listdir(os.path.join(json_dir)), '*.json') # modified ( 'json' to '*.json' )
|
|
json_files_names.sort(key=lambda name: int(re.search(r'(\d+)\.json', name).group(1)))
|
|
json_files_path = [os.path.join(json_dir, j_f) for j_f in json_files_names]
|
|
json_coords = []
|
|
for i, j_p in enumerate(json_files_path):
|
|
with open(j_p) as j_f:
|
|
try:
|
|
json_data = json.load(j_f)['people'][0]['pose_keypoints_2d']
|
|
except:
|
|
print(f'No person found in {os.path.basename(json_dir)}, frame {i}')
|
|
json_data = [0]*75
|
|
json_coords.append(json_data)
|
|
df_json_coords = pd.DataFrame(json_coords)
|
|
return df_json_coords
|
|
|
|
|
|
def drop_col(df, col_nb):
|
|
'''
|
|
Drops every nth column from a DataFrame.
|
|
|
|
INPUTS:
|
|
- df: dataframe. The DataFrame from which columns will be dropped.
|
|
- col_nb: int. The column number to drop.
|
|
|
|
OUTPUT:
|
|
- dataframe: DataFrame with dropped columns.
|
|
'''
|
|
|
|
idx_col = list(range(col_nb-1, df.shape[1], col_nb))
|
|
df_dropped = df.drop(idx_col, axis=1)
|
|
df_dropped.columns = range(df_dropped.columns.size)
|
|
return df_dropped
|
|
|
|
|
|
def speed_vert(df, axis='y'):
|
|
'''
|
|
Calculate the vertical speed of a DataFrame along a specified axis.
|
|
|
|
Parameters:
|
|
- df: dataframe. DataFrame of 2D coordinates.
|
|
- axis (str): The axis along which to calculate the speed. Default is 'y'.
|
|
|
|
OUTPUT:
|
|
- DataFrame: The DataFrame containing the vertical speed values.
|
|
'''
|
|
|
|
axis_dict = {'x':0, 'y':1, 'z':2}
|
|
df_diff = df.diff()
|
|
df_diff = df_diff.fillna(df_diff.iloc[1]*2)
|
|
df_vert_speed = pd.DataFrame([df_diff.loc[:, 2*k + axis_dict[axis]] for k in range(int(df_diff.shape[1] / 2))]).T # modified ( df_diff.shape[1]*2 to df_diff.shape[1] / 2 )
|
|
df_vert_speed.columns = np.arange(len(df_vert_speed.columns))
|
|
return df_vert_speed
|
|
|
|
|
|
def speed_2D(df):
|
|
'''
|
|
Calculate the 2D speed of a DataFrame.
|
|
|
|
INPUTS:
|
|
- df: dataframe. DataFrame of 2D coordinates.
|
|
|
|
OUTPUT:
|
|
- DataFrame: The DataFrame containing the 2D speed values.
|
|
'''
|
|
|
|
df_diff = df.diff()
|
|
df_diff = df_diff.fillna(df_diff.iloc[1]*2)
|
|
df_2Dspeed = pd.DataFrame([np.sqrt(df_diff.loc[:,2*k]*2 + df_diff.loc[:,2*k+1]*2) for k in range(int(df_diff.shape[1]*2))]).T
|
|
return df_2Dspeed
|
|
|
|
|
|
def interpolate_zeros_nans(col, kind):
|
|
'''
|
|
Interpolate missing points (of value nan)
|
|
|
|
INPUTS
|
|
- col: pandas column of coordinates
|
|
- kind: 'linear', 'slinear', 'quadratic', 'cubic'. Default 'cubic'
|
|
|
|
OUTPUT
|
|
- col_interp: interpolated pandas column
|
|
'''
|
|
|
|
mask = ~(np.isnan(col) | col.eq(0)) # true where nans or zeros
|
|
idx_good = np.where(mask)[0]
|
|
try:
|
|
f_interp = interpolate.interp1d(idx_good, col[idx_good], kind=kind, bounds_error=False)
|
|
col_interp = np.where(mask, col, f_interp(col.index))
|
|
return col_interp
|
|
except:
|
|
print('No good values to interpolate')
|
|
return col
|
|
|
|
|
|
def find_highest_wrist_position(df_coords, wrist_index):
|
|
'''
|
|
Find the frame with the highest wrist position in a list of coordinate DataFrames.
|
|
Highest wrist position frame use for finding the fastest frame.
|
|
|
|
INPUT:
|
|
- df_coords (list): List of coordinate DataFrames.
|
|
- wrist_index (int): The index of the wrist in the keypoint list.
|
|
|
|
OUTPUT:
|
|
- list: The index of the frame with the highest wrist position.
|
|
'''
|
|
|
|
start_frames = []
|
|
min_y_coords = []
|
|
for df in df_coords:
|
|
# Wrist y-coordinate column index (2n where n is the keypoint index)
|
|
# Assuming wrist_index is a list and we want to use the first element
|
|
y_col_index = wrist_index[0] * 2 + 1
|
|
|
|
# Replace 0 with NaN to avoid considering them and find the index of the lowest y-coordinate value
|
|
min_y_coord = df.iloc[:, y_col_index].replace(0, np.nan).min()
|
|
min_y_index = df.iloc[:, y_col_index].replace(0, np.nan).idxmin()
|
|
if min_y_coord <= 100: # if the wrist is too high, it is likely to be an outlier
|
|
print("The wrist is too high. Please check the data for outliers.")
|
|
|
|
start_frames.append(min_y_index)
|
|
min_y_coords.append(min_y_coord)
|
|
|
|
return start_frames, min_y_coords
|
|
|
|
|
|
def find_motion_end(df_coords, wrist_index, start_frame, lowest_y, fps):
|
|
'''
|
|
Find the frame where hands down movement ends.
|
|
Hands down movement is defined as the time when the wrist moves down from the highest position.
|
|
|
|
INPUT:
|
|
- df_coord (DataFrame): The coordinate DataFrame of the reference camera.
|
|
- wrist_index (int): The index of the wrist in the keypoint list.
|
|
- start_frame (int): The frame where the hands down movement starts.
|
|
- fps (int): The frame rate of the cameras in Hz.
|
|
|
|
OUTPUT:
|
|
- int: The index of the frame where hands down movement ends.
|
|
'''
|
|
|
|
y_col_index = wrist_index * 2 + 1
|
|
wrist_y_values = df_coords.iloc[:, y_col_index].values # wrist y-coordinates
|
|
highest_y_value = lowest_y
|
|
highest_y_index = start_frame
|
|
|
|
# Find the highest y-coordinate value and its index
|
|
for i in range(highest_y_index + 1, len(wrist_y_values)):
|
|
if wrist_y_values[i] - highest_y_value >= 50:
|
|
start_increase_index = i
|
|
break
|
|
else:
|
|
raise ValueError("The wrist does not move down.")
|
|
|
|
start = start_increase_index - start_frame
|
|
time = (start + fps) / fps
|
|
|
|
return time
|
|
|
|
|
|
def find_fastest_frame(df_speed_list):
|
|
'''
|
|
Find the frame with the highest speed in a list of speed DataFrames.
|
|
Fastest frame should locate in after highest wrist position frame.
|
|
|
|
INPUT:
|
|
- df_speed_list (list): List of speed DataFrames.
|
|
- df_speed (DataFrame): The speed DataFrame of the reference camera.
|
|
- fps (int): The frame rate of the cameras in Hz.
|
|
- lag_time (float): The time lag in seconds.
|
|
|
|
OUTPUT:
|
|
- int: The index of the frame with the highest speed.
|
|
'''
|
|
|
|
for speed_series in df_speed_list:
|
|
max_speed = speed_series.abs().max()
|
|
max_speed_index = speed_series.abs().idxmax()
|
|
|
|
if max_speed < 10:
|
|
print(" !!Warning!! : The maximum speed is likely to be not representative of the actual movement. Consider increasing the time parameter in Config.toml.")
|
|
return max_speed_index, max_speed
|
|
|
|
|
|
def plot_time_lagged_cross_corr(camx, camy, ax, fps, lag_time):
|
|
'''
|
|
Calculate and plot the max correlation between two cameras with a time lag.
|
|
How it works:
|
|
1. Reference camera is camx and the other is camy. (Reference camera should record last. If not, the offset will be positive.)
|
|
2. The initial shift alppied to camy to match camx is calculated.
|
|
3. Additionally shift camy by max_lag frames to find the max correlation.
|
|
|
|
INPUT:
|
|
- camx: pd.Series. Speed series of the reference camera.
|
|
- camy: pd.Series). Speed series of the other camera.
|
|
- ax: plt.axis. Plot correlation on second axis.
|
|
- fps: int. Framerate of the cameras in Hz.
|
|
- lag_time: float. Time lag in seconds.
|
|
|
|
OUTPUT:
|
|
- offset: int. Offset value to apply to synchronize the cameras.
|
|
- max_corr: float. Maximum correlation value.
|
|
'''
|
|
|
|
max_lag = int(fps * lag_time)
|
|
pearson_r = []
|
|
lags = range(-max_lag, 1)
|
|
|
|
for lag in lags:
|
|
if lag < 0:
|
|
shifted_camy = camy.shift(lag).dropna() # shift the camy segment by lag
|
|
corr = camx.corr(shifted_camy) # calculate the correlation between the camx segment and the shifted camy segment
|
|
elif lag == 0:
|
|
corr = camx.corr(camy)
|
|
else:
|
|
continue
|
|
pearson_r.append(corr)
|
|
|
|
|
|
# Handle NaN values in pearson_r and find the max correlation ignoring NaNs
|
|
pearson_r = np.array(pearson_r)
|
|
max_corr = np.nanmax(pearson_r) # Use nanmax to ignore NaNs
|
|
offset = np.nanargmax(pearson_r) - max_lag # Use nanargmax to find the index of the max correlation ignoring NaNs
|
|
# real_offset = offset + initial_shift
|
|
|
|
# visualize
|
|
ax.plot(lags, pearson_r)
|
|
ax.axvline(offset, color='r', linestyle='--', label='Peak synchrony')
|
|
plt.annotate(f'Max correlation={np.round(max_corr,2)}', xy=(0.05, 0.9), xycoords='axes fraction')
|
|
# ax.set(title=f'Offset = {offset}{initial_shift} = {real_offset} frames', xlabel='Offset (frames)', ylabel='Pearson r')
|
|
ax.set(title=f'Offset = {offset} frames', xlabel='Offset (frames)', ylabel='Pearson r')
|
|
plt.legend()
|
|
|
|
return offset, max_corr
|
|
|
|
|
|
def apply_offset(offset, json_dirs, reset_sync, cam1_nb, cam2_nb):
|
|
'''
|
|
Apply the offset to synchronize the cameras.
|
|
Offset is always applied to the second camera.
|
|
Offset would be always negative if the first camera is the last to start recording.
|
|
Delete the camy json files from initial frame to offset frame.
|
|
|
|
INPUT:
|
|
- offset (int): The offset value to apply to synchronize the cameras.
|
|
- json_dirs (list): List of directories containing the JSON files for each camera.
|
|
- reset_sync (bool): Whether to reset the synchronization by deleting the .del files.
|
|
- cam1_nb (int): The number of the reference camera.
|
|
- cam2_nb (int): The number of the other camera.
|
|
'''
|
|
|
|
if offset == 0:
|
|
print(f"Cams {cam1_nb} and {cam2_nb} are already synchronized. No offset applied.")
|
|
json_dir_to_offset = json_dirs[cam2_nb]
|
|
elif offset > 0 and not reset_sync:
|
|
print(f"Consider adjusting the lag time.")
|
|
raise ValueError(f"Are you sure the reference camera is the last to start recording?")
|
|
else:
|
|
offset = abs(offset)
|
|
json_dir_to_offset = json_dirs[cam2_nb]
|
|
|
|
json_files = sorted(fnmatch.filter(os.listdir(json_dir_to_offset), '*.json'), key=lambda x: int(re.findall('\d+', x)[0]))
|
|
|
|
if reset_sync:
|
|
del_files = fnmatch.filter(os.listdir(json_dir_to_offset), '*.del')
|
|
for del_file in del_files:
|
|
os.rename(os.path.join(json_dir_to_offset, del_file), os.path.join(json_dir_to_offset, del_file[:-4]))
|
|
else:
|
|
for i in range(offset):
|
|
os.rename(os.path.join(json_dir_to_offset, json_files[i]), os.path.join(json_dir_to_offset, json_files[i] + '.del'))
|
|
|
|
|
|
def synchronize_cams_all(config_dict):
|
|
'''
|
|
|
|
'''
|
|
|
|
# get parameters from Config.toml
|
|
project_dir = config_dict.get('project').get('project_dir')
|
|
pose_dir = os.path.realpath(os.path.join(project_dir, 'pose'))
|
|
fps = config_dict.get('project').get('frame_rate') # frame rate of the cameras (Hz)
|
|
reset_sync = config_dict.get('synchronization').get('reset_sync') # Start synchronization over each time it is run
|
|
id_kpt = 4
|
|
weights_kpt = 1
|
|
filter_order = 4
|
|
filter_cutoff = 6
|
|
vmax = 20 # px/s
|
|
|
|
# List json files
|
|
pose_listdirs_names = next(os.walk(pose_dir))[1]
|
|
pose_listdirs_names.sort(key=lambda name: int(re.search(r'(\d+)', name).group(1)))
|
|
json_dirs_names = [k for k in pose_listdirs_names if 'json' in k]
|
|
json_dirs = [os.path.join(pose_dir, j_d) for j_d in json_dirs_names] # list of json directories in pose_dir
|
|
cam_nb = len(json_dirs)
|
|
|
|
# Extract, interpolate, and filter keypoint coordinates
|
|
df_coords = []
|
|
b, a = signal.butter(filter_order/2, filter_cutoff/(fps/2), 'low', analog = False)
|
|
for i, json_dir in enumerate(json_dirs):
|
|
df_coords.append(convert_json2pandas(json_dir))
|
|
df_coords[i] = drop_col(df_coords[i],3) # drop likelihood
|
|
df_coords[i] = df_coords[i].apply(interpolate_zeros_nans, axis=0, args = ['cubic'])
|
|
df_coords[i] = df_coords[i].apply(loess_filter_1d, axis=0, args = [30])
|
|
df_coords[i] = pd.DataFrame(signal.filtfilt(b, a, df_coords[i], axis=0))
|
|
|
|
|
|
|
|
|
|
# Save keypoint coordinates to pickle
|
|
with open(os.path.join(pose_dir, 'coords'), 'wb') as fp:
|
|
pk.dump(df_coords, fp)
|
|
# with open(os.path.join(pose_dir, 'coords'), 'rb') as fp:
|
|
# df_coords = pk.load(fp)
|
|
|
|
# Compute vertical speed
|
|
df_speed = []
|
|
for i in range(cam_nb):
|
|
df_speed.append(speed_vert(df_coords[i]))
|
|
# df_speed[i] = df_speed[i].where(abs(df_speed[i])<vmax, other=np.nan) # replaces by nan if jumps in speed
|
|
# df_speed[i] = df_speed[i].apply(interpolate_nans, axis=0, args = ['cubic'])
|
|
|
|
|
|
# Frame with maximum of the sum of absolute speeds
|
|
max_speed_frame = []
|
|
for i in range(cam_nb):
|
|
max_speed_frame += [np.argmax(abs(df_speed[i].sum(axis=1)))]
|
|
|
|
#############################################
|
|
# 2. PLOTTING PAIRED CORRELATIONS OF SPEEDS #
|
|
#############################################
|
|
|
|
# Do this on all cam pairs
|
|
# Choose pair with highest correlation
|
|
|
|
# on a particular point (typically the wrist on a vertical movement)
|
|
# or on a selection of weighted points
|
|
|
|
# find the lowest position of the wrist
|
|
lowest_frames, lowest_y_coords = find_highest_wrist_position(df_coords, id_kpt)
|
|
|
|
# set reference camera
|
|
nb_frames_per_cam = [len(d) for d in df_speed]
|
|
ref_cam_id = nb_frames_per_cam.index(min(nb_frames_per_cam))
|
|
|
|
|
|
max_speeds = []
|
|
|
|
|
|
cam_list = list(range(cam_nb))
|
|
cam_list.pop(ref_cam_id)
|
|
for cam_id in cam_list:
|
|
# find the highest wrist position for each camera
|
|
camx_start_frame = lowest_frames[ref_cam_id]
|
|
camy_start_frame = lowest_frames[cam_id]
|
|
|
|
camx_lowest_y = lowest_y_coords[ref_cam_id]
|
|
camy_lowest_y = lowest_y_coords[cam_id]
|
|
|
|
camx_time = find_motion_end(df_coords[ref_cam_id], id_kpt[0], camx_start_frame, camx_lowest_y, fps)
|
|
camy_time = find_motion_end(df_coords[cam_id], id_kpt[0], camy_start_frame, camy_lowest_y, fps)
|
|
|
|
camx_end_frame = camx_start_frame + int(camx_time * fps)
|
|
camy_end_frame = camy_start_frame + int(camy_time * fps)
|
|
|
|
camx_segment = df_speed[ref_cam_id].iloc[camx_start_frame:camx_end_frame+1, id_kpt[0]]
|
|
camy_segment = df_speed[cam_id].iloc[camy_start_frame:camy_end_frame+1, id_kpt[0]]
|
|
|
|
# Find the fastest speed and the frame
|
|
camx_max_speed_index, camx_max_speed = find_fastest_frame([camx_segment])
|
|
camy_max_speed_index, camy_max_speed = find_fastest_frame([camy_segment])
|
|
max_speeds.append(camx_max_speed)
|
|
max_speeds.append(camy_max_speed)
|
|
vmax = max(max_speeds)
|
|
|
|
# Find automatically the best lag time
|
|
lag_time = round((camy_max_speed_index - camx_max_speed_index) / fps + 1)
|
|
|
|
# FInd the fatest frame
|
|
camx_start_frame = camx_max_speed_index - (fps) * (lag_time)
|
|
if camx_start_frame < 0:
|
|
camx_start_frame = 0
|
|
else:
|
|
camx_start_frame = int(camx_start_frame)
|
|
camy_start_frame = camy_max_speed_index - (fps) * (lag_time)
|
|
camx_end_frame = camx_max_speed_index + (fps) * (lag_time)
|
|
camy_end_frame = camy_max_speed_index + (fps) * (lag_time)
|
|
|
|
if len(id_kpt) == 1 and id_kpt[0] != 'all':
|
|
camx = df_speed[ref_cam_id].iloc[camx_start_frame:camx_end_frame+1, id_kpt[0]]
|
|
camy = df_speed[cam_id].iloc[camy_start_frame:camy_end_frame+1, id_kpt[0]]
|
|
elif id_kpt == ['all']:
|
|
camx = df_speed[ref_cam_id].iloc[camx_start_frame:camx_end_frame+1].sum(axis=1)
|
|
camy = df_speed[cam_id].iloc[camy_start_frame:camy_end_frame+1].sum(axis=1)
|
|
elif len(id_kpt) == 1 and len(id_kpt) == len(weights_kpt):
|
|
dict_id_weights = {i:w for i, w in zip(id_kpt, weights_kpt)}
|
|
camx = df_speed[ref_cam_id] @ pd.Series(dict_id_weights).reindex(df_speed[ref_cam_id].columns, fill_value=0)
|
|
camy = df_speed[cam_id] @ pd.Series(dict_id_weights).reindex(df_speed[cam_id].columns, fill_value=0)
|
|
camx = camx.iloc[camx_start_frame:camx_end_frame+1]
|
|
camy = camy.iloc[camy_start_frame:camy_end_frame+1]
|
|
else:
|
|
raise ValueError('wrong values for id_kpt or weights_kpt')
|
|
|
|
# filter the speeds
|
|
camx = camx.where(lambda x: (x <= vmax) & (x >= -vmax), other=np.nan)
|
|
camy = camy.where(lambda x: (x <= vmax) & (x >= -vmax), other=np.nan)
|
|
|
|
f, ax = plt.subplots(2,1)
|
|
|
|
# speed
|
|
camx.plot(ax=ax[0], label = f'cam {ref_cam_id+1}')
|
|
camy.plot(ax=ax[0], label = f'cam {cam_id+1}')
|
|
ax[0].set(xlabel='Frame',ylabel='Speed (pxframe)')
|
|
ax[0].legend()
|
|
|
|
# time lagged cross-correlation
|
|
offset, max_corr = plot_time_lagged_cross_corr(camx, camy, ax[1], fps, lag_time, camx_max_speed_index, camy_max_speed_index)
|
|
f.tight_layout()
|
|
plt.show()
|
|
print(f'Using number{id_kpt} keypoint, synchronized camera {ref_cam_id+1} and camera {cam_id+1}, with an offset of {offset} and a max correlation of {max_corr}.')
|
|
|
|
# apply offset
|
|
apply_offset(offset, json_dirs, reset_sync, ref_cam_id, cam_id)
|
|
|
|
|