def __init__(self, joint_cs: Callable, static_marker_pos: Mapping,
tracking_marker_names: Sequence[str]):
self.cs = joint_cs(
*[static_marker_pos[marker] for marker in joint_cs.markers])
self.static_markers = np.stack(
[static_marker_pos[marker] for marker in tracking_marker_names], 0)
self.static_markers_intrinsic = vec_transform(
ht_inv(self.cs), make_vec_hom(self.static_markers))[:, :3]
def process_trial(static_markers,
tracking_markers,
traj_func,
base_frame_inv=None):
torso_traj = traj_func(static_markers, tracking_markers)
present_frames = np.nonzero(~np.any(np.isnan(torso_traj), (-2,
-1)))[0]
if present_frames.size == 0:
num_frames = tracking_markers.shape[1]
torso_pos = np.full((num_frames, 3), np.nan)
torso_eul = np.full((num_frames, 3), np.nan)
base_frame_inv = None
else:
if base_frame_inv is None:
base_frame_inv = ht_inv(torso_traj[present_frames[0]])
torso_intrinsic = change_cs(base_frame_inv, torso_traj)
torso_pos = torso_intrinsic[:, :3, 3]
torso_eul = np.rad2deg(zxy_intrinsic(torso_intrinsic))
return (torso_pos, torso_eul), base_frame_inv
def get_trajs(trial: BiplaneViconTrial, dt: float, torso_def: str, scap_lateral: str = 'GC') \
-> Tuple[PoseTrajectory, PoseTrajectory, PoseTrajectory]:
"""Return HT, GH, and ST trajectory from a given trial."""
torso, scap, hum = trajectories_from_trial(trial, dt, torso_def=torso_def)
# this is not efficient because I recompute the scapula CS for each trial, but it can be done at the subject level
# however, in the grand scheme of things, this computation is trivial
scap_gc = scapula_cs_isb(trial.subject.scapula_landmarks['GC'],
trial.subject.scapula_landmarks['IA'],
trial.subject.scapula_landmarks['TS'])
scap_lat = scapula_cs_isb(trial.subject.scapula_landmarks[scap_lateral],
trial.subject.scapula_landmarks['IA'],
trial.subject.scapula_landmarks['TS'])
scap_gc_lat = ht_inv(scap_gc) @ scap_lat
scap = PoseTrajectory.from_ht(scap.ht @ scap_gc_lat[np.newaxis, ...],
dt=scap.dt,
frame_nums=scap.frame_nums)
ht = hum.in_trajectory(torso)
gh = hum.in_trajectory(scap)
st = scap.in_trajectory(torso)
return ht, gh, st
def trajectories_from_trial(trial: BiplaneViconTrial, dt: float, smoothed: bool = True, base_cs: str = 'vicon',
torso_def: str = 'isb', frame_sync: bool = True) -> Tuple[PoseTrajectory, PoseTrajectory,
PoseTrajectory]:
"""Create torso, scapula, and humerus trajectories from a trial."""
assert(np.array_equal(trial.humerus_frame_nums, trial.scapula_frame_nums))
scap_quat_field = 'scapula_quat_fluoro_avg_smooth' if smoothed else 'scapula_quat_fluoro'
scap_pos_field = 'scapula_pos_fluoro_avg_smooth' if smoothed else 'scapula_pos_fluoro'
hum_quat_field = 'humerus_quat_fluoro_avg_smooth' if smoothed else 'humerus_quat_fluoro'
hum_pos_field = 'humerus_pos_fluoro_avg_smooth' if smoothed else 'humerus_pos_fluoro'
def get_torso_pos_quat(t, thorax_def):
if thorax_def == 'isb':
return t.torso_pos_vicon, t.torso_quat_vicon
elif thorax_def == 'v3d':
return t.torso_v3d_pos_vicon, t.torso_v3d_quat_vicon
else:
raise ValueError('torso_def must be either isb or v3d.')
if base_cs == 'vicon':
# scapula
scap_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(getattr(trial, scap_quat_field)))
scap_traj_fluoro = ht_r(scap_rot_mat, getattr(trial, scap_pos_field))
scap_traj_vicon = change_cs(ht_inv(trial.subject.f_t_v), scap_traj_fluoro)
scap_traj = PoseTrajectory.from_ht(scap_traj_vicon, dt, trial.scapula_frame_nums)
# humerus
hum_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(getattr(trial, hum_quat_field)))
hum_traj_fluoro = ht_r(hum_rot_mat, getattr(trial, hum_pos_field))
hum_traj_vicon = change_cs(ht_inv(trial.subject.f_t_v), hum_traj_fluoro)
hum_traj = PoseTrajectory.from_ht(hum_traj_vicon, dt, trial.humerus_frame_nums)
# torso
torso_pos_vicon, torso_quat_vicon = get_torso_pos_quat(trial, torso_def)
if frame_sync:
torso_pos_vicon_sync = (torso_pos_vicon[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_quat_vicon_sync = (torso_quat_vicon[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_traj = PoseTrajectory.from_quat(torso_pos_vicon_sync, torso_quat_vicon_sync, dt,
trial.humerus_frame_nums)
else:
torso_traj = PoseTrajectory.from_quat(torso_pos_vicon, torso_quat_vicon, dt,
np.arange(torso_pos_vicon.shape[0]) + 1)
elif base_cs == 'fluoro':
scap_traj = PoseTrajectory.from_quat(getattr(trial, scap_pos_field), getattr(trial, scap_quat_field), dt,
trial.scapula_frame_nums)
hum_traj = PoseTrajectory.from_quat(getattr(trial, hum_pos_field), getattr(trial, hum_quat_field), dt,
trial.humerus_frame_nums)
torso_pos_vicon, torso_quat_vicon = get_torso_pos_quat(trial, torso_def)
torso_rot_mat = quaternion.as_rotation_matrix(quaternion.from_float_array(torso_quat_vicon))
torso_traj_vicon = ht_r(torso_rot_mat, torso_pos_vicon)
torso_traj_fluoro = change_cs(trial.subject.f_t_v, torso_traj_vicon)
if frame_sync:
torso_traj_fluoro_sync = (torso_traj_fluoro[trial.vicon_endpts[0]:
trial.vicon_endpts[1]])[trial.humerus_frame_nums - 1]
torso_traj = PoseTrajectory.from_ht(torso_traj_fluoro_sync, dt, trial.humerus_frame_nums)
else:
torso_traj = PoseTrajectory.from_ht(torso_traj_fluoro, dt, np.arange(torso_traj_fluoro.shape[0]) + 1)
else:
raise ValueError('base_cs must be either vicon or fluoro.')
return torso_traj, scap_traj, hum_traj