def shr_compute(df_row):
st_contrib = df_row['traj_interp'].st.contribs[:, 1]
gh_contrib = df_row['traj_interp'].gh.contribs[:, 1]
ht_traj = df_row['ht']
st_traj = df_row['st']
gh_traj = df_row['gh']
up_down = df_row['up_down_analysis']
start_idx = up_down.max_run_up_start_idx
end_idx = up_down.max_run_up_end_idx
sec = slice(start_idx, end_idx + 1)
st_elev = -np.rad2deg(st_contrib[sec] - st_contrib[start_idx])
run_vals_new, run_starts_new, run_lengths_new = find_runs(st_elev > 1)
first_gt_1_new = run_starts_new[np.nonzero(
np.logical_and(run_lengths_new > 10, run_vals_new))[0][0]]
st_elev_diff = np.rad2deg(-(st_traj.euler.st_isb[sec, 1] -
st_traj.euler.st_isb[start_idx, 1]))
run_vals_old, run_starts_old, run_lengths_old = find_runs(st_elev_diff > 1)
first_gt_1_old = run_starts_old[np.nonzero(
np.logical_and(run_lengths_old > 10, run_vals_old))[0][0]]
start_idx_comp = max(first_gt_1_new, first_gt_1_old)
ht_elev_diff = np.rad2deg(-(ht_traj.euler.ht_isb[sec, 1] -
ht_traj.euler.ht_isb[start_idx, 1]))
gh_elev_diff = np.rad2deg(-(gh_traj.euler.gh_isb[sec, 1] -
gh_traj.euler.gh_isb[start_idx, 1]))
ht_elev_range = ht_elev_diff[start_idx_comp:] - ht_elev_diff[start_idx_comp]
old_shr = gh_elev_diff[start_idx_comp:] / st_elev_diff[start_idx_comp:]
new_shr = ((gh_contrib[sec] - gh_contrib[start_idx])[start_idx_comp:] /
(st_contrib[sec] - st_contrib[start_idx])[start_idx_comp:])
ht_interp_range = np.arange(0, 100 + 0.25, 0.25)
old_shr_interp = np.interp(ht_interp_range,
ht_elev_range,
old_shr,
left=np.nan,
right=np.nan)
new_shr_interp = np.interp(ht_interp_range,
ht_elev_range,
new_shr,
left=np.nan,
right=np.nan)
return old_shr, new_shr, old_shr_interp, new_shr_interp
def sig_string(x: np.ndarray, sig: np.ndarray) -> str:
"""Create a string describing signifiance based on the domain x and boolean vector of signifiance (sig)."""
run_values, run_starts, run_lengths = find_runs(sig)
run_starts_sign = run_starts[run_values]
run_lengths_sign = run_lengths[run_values]
sec = []
for run, length in zip(run_starts_sign, run_lengths_sign):
sec.append('{:.2f} - {:.2f}'.format(x[run], x[run + length - 1]))
return '\n'.join(sec)
def extract_sig(spm_test: _SPM0Dinference, x: np.ndarray) -> str:
"""Create a string describing where spm_test is significant in the domain of x."""
sig = np.logical_or(spm_test.z < -spm_test.zstar, spm_test.z > spm_test.zstar)
run_values, run_starts, run_lengths = find_runs(sig)
run_starts_sign = run_starts[run_values]
run_lengths_sign = run_lengths[run_values]
sec = []
for run, length in zip(run_starts_sign, run_lengths_sign):
sec.append('{:.2f} - {:.2f}'.format(x[run], x[run + length - 1]))
return '\n'.join(sec)
def analyze_up_down(ht_traj: PoseTrajectory) -> UpDownAnalysis:
"""Performing an analysis of ht_traj to determine where minimum and maximal HT elevation is achieved."""
ht_elev = np.rad2deg(-ht_traj.euler.ht_isb[:, 1])
num_frames = ht_elev.size
max_elev = ht_elev.max()
max_elev_idx = np.argmax(ht_elev)
# first level of analysis - determine minimum for up and down
up = ht_elev[:max_elev_idx+1]
down = np.flip(ht_elev[max_elev_idx:])
min_elev_up = up.min()
min_elev_down = down.min()
min_elev_up_idx = np.argmin(up)
min_elev_down_idx = np.argmin(down)
# second level of analysis - determine longest run
up_diff = np.diff(up)
down_diff = np.diff(down)
up_run_vals, up_run_starts, up_run_lengths = find_runs(up_diff >= 0)
down_run_vals, down_run_starts, down_run_lengths = find_runs(down_diff >= 0)
up_run_starts_inc = up_run_starts[up_run_vals]
up_run_lengths_inc = up_run_lengths[up_run_vals]
down_run_starts_inc = down_run_starts[down_run_vals]
down_run_lengths_inc = down_run_lengths[down_run_vals]
# note that the down indices are for the flipped trajectory
max_run_up_run_idx = np.argmax(up_run_lengths_inc)
max_run_down_run_idx = np.argmax(down_run_lengths_inc)
max_run_up_start_idx = up_run_starts_inc[max_run_up_run_idx]
max_run_up_end_idx = max_run_up_start_idx + up_run_lengths_inc[max_run_up_run_idx]
max_run_down_start_idx = down_run_starts_inc[max_run_down_run_idx]
max_run_down_end_idx = max_run_down_start_idx + down_run_lengths_inc[max_run_down_run_idx]
max_run_up_start_val = up[max_run_up_start_idx]
max_run_up_end_val = up[max_run_up_end_idx]
max_run_down_start_val = down[max_run_down_start_idx]
max_run_down_end_val = down[max_run_down_end_idx]
return UpDownAnalysis(max_elev, max_elev_idx, min_elev_up, min_elev_down, min_elev_up_idx,
num_frames - 1 - min_elev_down_idx, max_run_up_start_idx, max_run_up_end_idx,
num_frames - 1 - max_run_down_start_idx, num_frames - 1 - max_run_down_end_idx,
max_run_up_start_val, max_run_up_end_val, max_run_down_start_val, max_run_down_end_val)
def kf_filter_marker_piecewise(
marker_pos_labeled: np.ndarray,
marker_pos_filled: np.ndarray,
dt: float,
max_gap: int = 75,
max_gap_secondary: Tuple[int, int] = (30, 10),
min_length: int = 75,
white_noise_var: float = 10000
) -> Tuple[Sequence[FilterStep], Sequence[FilterStep]]:
"""Filter raw (labeled) Vicon marker data, accounting for gaps.
There are two conditions that create a gap:
1. The marker is not visible for more than or equal to max_gap frames
2. Periods where marker is not visible for >= max_gap_secondary[0] frames are separated by an interval where the
marker is visible for at most max_gap_secondary[1] frames
Subsequently, all gaps are combined.
Raises
------
biplane_kine.smoothing.kalman_filtering.InsufficientDataError
"""
start_idx, stop_idx = init_point(marker_pos_labeled, marker_pos_filled)
nans_labeled = ~np.isnan(marker_pos_labeled[start_idx:stop_idx, 0])
runs = find_runs(nans_labeled)
# primary gaps - no data for longer than max_gap
primary_runs_gaps_idx_start = np.nonzero(
((~runs[0]) & (runs[2] >= max_gap)))[0]
primary_runs_gaps_idx_end = primary_runs_gaps_idx_start + 1
# secondary gaps - gaps of max_gap_secondary[0] separated by spaces where at most max_gap_secondary[1] data exists
runs_secondary_gaps_idx = np.nonzero(
((~runs[0]) & (runs[2] >= max_gap_secondary[0])))[0]
secondary_runs_gaps_idx_start = []
secondary_runs_gaps_idx_end = []
for i in range(runs_secondary_gaps_idx.size - 1):
if np.sum(runs[0][runs_secondary_gaps_idx[i] +
1:runs_secondary_gaps_idx[i + 1]] * runs[2]
[runs_secondary_gaps_idx[i] +
1:runs_secondary_gaps_idx[i + 1]]) < max_gap_secondary[1]:
secondary_runs_gaps_idx_start.append(runs_secondary_gaps_idx[i])
secondary_runs_gaps_idx_end.append(runs_secondary_gaps_idx[i + 1] +
1)
# now let's combine the gaps
all_runs_gaps_idx = \
list(itertools.chain.from_iterable([zip(primary_runs_gaps_idx_start, primary_runs_gaps_idx_end),
zip(secondary_runs_gaps_idx_start, secondary_runs_gaps_idx_end)]))
def gaps_overlap(gap1: Tuple[int, int], gap2: Tuple[int, int]) -> bool:
"""Do the gaps overlap?"""
return (gap1[0] < gap2[1]) and (gap2[0] < gap1[1])
def combine_gaps(gap1: Tuple[int, int],
gap2: Tuple[int, int]) -> Tuple[int, int]:
"""Combined the two gaps."""
min_start = min(gap1, gap2, key=itemgetter(0))
max_end = max(gap1, gap2, key=itemgetter(1))
return min_start[0], max_end[1]
# this only works if the list is sorted by the start index!
def recursive_combine(initial_gap_list, combined_gap_list):
# if there are no more gaps to combine then return the combined_gap_list
if not initial_gap_list:
return combined_gap_list
# if we can combine the current gap (combined_gap_list[-1]) with the next gap in the list to process
# (initial_gap_list[0])
if gaps_overlap(combined_gap_list[-1], initial_gap_list[0]):
# combine the gaps and update the current gap
combined = combine_gaps(combined_gap_list[-1], initial_gap_list[0])
combined_gap_list[-1] = combined
else:
# can't combine so add the considered gap becomes the current gap
combined_gap_list.append(initial_gap_list[0])
# either way we have taken care of this gap so remove it from the list of gaps to be considered
del (initial_gap_list[0])
# march forward
return recursive_combine(initial_gap_list, combined_gap_list)
def recursive_combine_start(initial_gap_list):
# no gaps
if not initial_gap_list:
return []
# the first combination is easy, it's just the first gap by itself
initial_gap_list_copy = initial_gap_list.copy()
combined_gap_list = [initial_gap_list_copy[0]]
del (initial_gap_list_copy[0])
return recursive_combine(initial_gap_list_copy, combined_gap_list)
# first sort by the start index
all_runs_gaps_idx.sort(key=itemgetter(0))
all_runs_gaps_ids_merged = recursive_combine_start(all_runs_gaps_idx)
# break the list of tuples apart into two lists
runs_gaps_idx_start_final = [gap[0] for gap in all_runs_gaps_ids_merged]
runs_gaps_idx_end_final = [gap[1] for gap in all_runs_gaps_ids_merged]
# number of pieces to filter will always be one greater than the number of gaps
num_pieces = len(all_runs_gaps_ids_merged) + 1
pieces_end_idx = np.full((num_pieces, ), stop_idx)
pieces_start_idx = np.full((num_pieces, ), start_idx)
# there may not be any gaps so check first
if all_runs_gaps_ids_merged:
# interior pieces run from the end index of a gap to the start index of the next gap
pieces_end_idx[:-1] = runs[1][runs_gaps_idx_start_final] + start_idx
pieces_start_idx[1:] = runs[1][runs_gaps_idx_end_final] + start_idx
filtered_pieces = []
smoothed_pieces = []
# filter each piece
for i in range(num_pieces):
if (pieces_end_idx[i] - pieces_start_idx[i]) < min_length:
log.info('Skipping Filtering piece %d running from %d to %d', i,
pieces_start_idx[i], pieces_end_idx[i])
continue
log.info('Filtering piece %d running from %d to %d ', i,
pieces_start_idx[i], pieces_end_idx[i])
piece_filtered, piece_smoothed = kf_filter_marker_piece(
marker_pos_labeled, marker_pos_filled, pieces_start_idx[i],
pieces_end_idx[i], dt, white_noise_var)
filtered_pieces.append(piece_filtered)
smoothed_pieces.append(piece_smoothed)
if not filtered_pieces:
raise InsufficientDataError('No resulting segments to filter.')
return filtered_pieces, smoothed_pieces
ht_traj = df_row['ht']
st_traj = df_row['st']
gh_traj = df_row['gh']
up_down = df_row['up_down_analysis']
start_idx = up_down.max_run_up_start_idx
end_idx = up_down.max_run_up_end_idx
sec = slice(start_idx, end_idx + 1)
fig = plt.figure()
ax = fig.subplots()
ax.xaxis.set_major_locator(plticker.MultipleLocator(base=10.0))
plot_utils.style_axes(ax, 'HT Elevation Angle', 'SHR')
st_elev = np.rad2deg(st_contrib[sec] - st_contrib[start_idx])
run_vals_new, run_starts_new, run_lengths_new = find_runs(
-st_elev > 1)
first_gt_1_new = run_starts_new[np.nonzero(
np.logical_and(run_lengths_new > 10, run_vals_new))[0][0]]
st_elev_diff = np.rad2deg(
-(st_traj.euler.st_isb[sec, 1] -
st_traj.euler.st_isb[start_idx, 1]))
run_vals_old, run_starts_old, run_lengths_old = find_runs(
st_elev_diff > 1)
first_gt_1_old = run_starts_old[np.nonzero(
np.logical_and(run_lengths_old > 10, run_vals_old))[0][0]]
start_idx_comp = max(first_gt_1_new, first_gt_1_old)
ht_elev = np.rad2deg(-ht_traj.euler.ht_isb[sec, 1])
ht_elev_diff = np.rad2deg(
-(ht_traj.euler.ht_isb[sec, 1] -