def estimate_r(ped_xy, vic_xy, dt):
"""
No vic in frame / strictly dominating vic => set r to this vic's index
Strictly dominating defined as
- dsq < any other vic's dsq
- tau < any other vic's tau
:param ped_xy: n_frames, n_ped, 2
:param vic_xy: n_frames, n_vic, 2
:param dt:
:return: n_frames-1, n_ped | index of dominating vic for frame,
i = R_NO_VIC if no vic
i = R_SENTINEL if not estimated (ie no dominating vic exists, or ped = nan in frame i/i+1)
"""
n_frames, n_ped = ped_xy.shape[:2]
n_vic = vic_xy.shape[1]
r = np.zeros((n_frames - 1, n_ped), dtype=np.int) + R_SENTINEL
for i in range(n_ped):
for j in range(n_frames-1):
ped_pv = np.concatenate([
ped_xy[[j], i, :], (ped_xy[[j + 1], i, :] - ped_xy[[j], i, :]) / dt], axis=1)
vic_pv = np.concatenate([
vic_xy[j, :, :], (vic_xy[j + 1, ...] - vic_xy[j, ...]) / dt], axis=1)
is_vic_nan = np.isnan(vic_pv).any(axis=1)
if is_vic_nan.sum() == n_vic:
r[j, i] = R_NO_VIC
continue
if np.isnan(ped_pv).any():
continue
dsq_tau = rt.dist2rt_v1(ped_pv, vic_pv[~is_vic_nan, :]) # (n_ped=1, n_vic, 2)
dsq_tau = dsq_tau[0, ...]
ind_dominating = find_dominating(-dsq_tau, sentinel=R_SENTINEL)
ind_dominating = np.arange(n_vic)[~is_vic_nan][ind_dominating]
r[j, i] = ind_dominating
return r
def sample_next(state, vic_pv, parameters):
"""
For pedestrians that can interact with no vic - ignore
For remaining, mask no-interaction vic and sample r, q
:param state: n_samples, n_ped, 6 | [x v r q]
:param vic_pv: n_vic, 4 | [x v]
:param parameters:
:return:
"""
n_samples, n_ped = state.shape[:2]
state_next = np.empty_like(state)
pe_dist, pa_dist = perp_dist.signed_pe_dist_particles_v1(
state[..., :4], vic_pv)
# n_samples, n_ped, n_vic
is_ignore = predict_utils.is_ignore(pe_dist, pa_dist,
parameters.mrx_grid[0])
ignore_n_mask = np.all(is_ignore, axis=-1)
n_ni = (~ignore_n_mask).sum()
state_next[ignore_n_mask, :2] = state[ignore_n_mask, :2] +\
state[ignore_n_mask, 2:4] * parameters.dt
state_next[ignore_n_mask, 2:4] = state[ignore_n_mask, 2:4] + \
np.random.randn(n_samples*n_ped - n_ni, 2) * parameters.sigma_v
if n_ni == 0:
# ignore all vic
return state_next
# remaining ped have at least 1 interacting vic
# n_ni, n_vic, 2
rt_all = rd.dist2rt_v1(state[~ignore_n_mask, :4], vic_pv)
rt_all[rt_all < EPS] = EPS
rt_all = np.log10(rt_all)
rt_all[..., 0] /= 2
# n_ni, n_vic, m
z_all = rt_grid.rt2enc_v1(rt_all, parameters.rt_grid)
# n_ni, n_vic
z_all_dot_beta = z_all.dot(-parameters.beta[1:]) - parameters.beta[0]
# n_ni,
r_s, q_s = sample_rq.sample_softmax_rq_v0(z_all_dot_beta,
is_ignore[~ignore_n_mask])
v_hat = u_grid.evaluate_v_v1(state[~ignore_n_mask, :4], vic_pv,
parameters.mrx_grid, parameters.u, q_s == 1,
r_s.astype(np.int))
state[~ignore_n_mask, 4] = r_s
state[~ignore_n_mask, 5] = q_s
state_next[
~ignore_n_mask, :2] = state[~ignore_n_mask, :2] + v_hat * parameters.dt
state_next[~ignore_n_mask, 2:4] = state[~ignore_n_mask, 2:4] +\
np.random.randn(n_ni, 2) * parameters.sigma_v
return state_next
def find_little_interaction_frames_v0(ped_xy, vic_xy, dt):
"""
little interaction if either of
- rt risk threshold met: for each vic, ped not on collision course, or vic is stationary
- perpendicular distance: even if collision course, distance is sufficiently far
- this relates to finite (perp) distance m(x)=u maps encode
:param ped_xy: n_frames, 2
:param vic_xy: n_frames, n_vic, 2
- frames with nan ignored
:param dt:
:return: frame_inds: inds of ped_xy *velocity* counted as observed, in sorted order
- ind i included if i,i+1 both observed or i has no vic
"""
tau_dist = 5 # meters away from any vic's last-velocity line
tau_v = 0.5 # threshold for stationary vics
M = 20 # threshold for d, tau - to check that ped is on collision course
frame_inds = []
n_frames = ped_xy.shape[0]
no_vic_frames = np.arange(n_frames)[
np.isnan(vic_xy).any(axis=2).all(axis=1)
]
# only for frames where i, i+1 not nan
is_ped_valid = ~np.isnan(ped_xy[:-1, 0]) & ~np.isnan(ped_xy[1:, 0])
for i in np.arange(n_frames-1)[is_ped_valid]:
if i in no_vic_frames:
continue
ped_pv = np.concatenate([ped_xy[[i], :], (ped_xy[[i+1], :] - ped_xy[[i], :])/dt], axis=1)
vic_pv = np.concatenate([vic_xy[i, :, :], (vic_xy[i+1, ...] - vic_xy[i, ...])/dt], axis=1)
is_vic_nan = np.isnan(vic_pv).any(axis=1)
vic_pv = vic_pv[~is_vic_nan, :]
dsq_tau = rt.dist2rt_v1(ped_pv, vic_pv) # (n_ped=1, n_vic, 2)
dsq_tau = dsq_tau[0, ...]
dsq_tau[:, 0] = np.sqrt(dsq_tau[:, 0])
is_intersect = np.all(dsq_tau < M, axis=1)
is_stationary = (vic_pv[:, 2:]**2).sum(axis=1) < tau_v**2
mask = is_intersect & ~is_stationary
if np.sum(mask) == 0:
frame_inds.append(i)
continue
pe_dist = perp.pe_dist_v0(ped_pv[:, :2], vic_pv[mask, :])[0]
if np.all(pe_dist > tau_dist):
frame_inds.append(i)
consec_inds = np.sort(np.hstack((frame_inds, no_vic_frames)).astype(np.int))
consec_inds = ge.select_consecutive_forward(consec_inds)
# add any removed no_vic frames back in
consec_inds = np.unique(np.hstack((consec_inds, no_vic_frames))).astype(np.int)
return consec_inds
def optimize_out_softmax_rq_v1(a_pvvh, b_pv, rt_grid, beta,
mrx_grid, u, sigma_x, dt):
"""
Calculate r, q, nll for a single frame (n=n_ped, k=n_vic)
Ignore k that are outside the range of grid, setting:
- r = R_IGNORED
- q = 1
:param a_pvvh: n, 6 | [x v \hat{v}], v is given, \hat{v} from differences
:param b_pv: k, 4
:param rt_grid: (lx, ly, dx, dy, nx, ny) | 6-tuple
:param beta: 1+nx*ny, | weights for rt_grid encoding
:param mrx_grid: (l, m) | 2-tuple
:param u: m, | weights for mrx_grid encoding
:param sigma_x:
:param dt:
:return: r, q that minimize total (r, q, x) nll given v
r: n,
q: n,
nll: n, | loss for each agent
"""
n = a_pvvh.shape[0]
k = b_pv.shape[0]
r = np.zeros(n, dtype=np.int) + R_IGNORED
q = np.ones(n, dtype=np.int)
nll = np.zeros(n)
pe_dist, pa_dist = perp.signed_pe_dist_v1(a_pvvh[:, :4], b_pv)
is_ignore = (pe_dist < -mrx_grid[0]) | (0 < pe_dist) | (pa_dist < 0)
ignore_n_mask = np.all(is_ignore, axis=1)
n_ni = (~ignore_n_mask).sum()
# is_ignore -> compute only x_q1 loss
scaling = 0.5 * (dt / sigma_x) ** 2
nll[ignore_n_mask] = ((a_pvvh[ignore_n_mask, 2:4] -
a_pvvh[ignore_n_mask, 4:6])**2).sum(axis=-1) * scaling
if n_ni == 0:
return r, q, nll
nll_all = np.zeros((n_ni, k, 2))
is_ignore_sub = is_ignore[~ignore_n_mask]
# *some* k may be valid for *some* n but not for others
# how to ignore bad k while still processing?
# in logsumexp use b=is_ignore_sub.astype(int)
ni_inds = np.arange(n)[~ignore_n_mask]
a_pvvh_ni = a_pvvh[~ignore_n_mask]
rt_all = rd.dist2rt_v1(a_pvvh_ni[:, :4], b_pv)
rt_all[rt_all < EPS] = EPS
rt_all = np.log10(rt_all)
rt_all[..., 0] /= 2
# n_ni, k, m
z_all = b_grid.rt2enc_v1(rt_all, rt_grid)
# n_ni, k
z_all_dot_beta = z_all.dot(-beta[1:]) - beta[0]
r_loss = -(z_all_dot_beta.T - sp.logsumexp(
z_all_dot_beta, b=(~is_ignore_sub).astype(np.int), axis=-1)).T
nll_all += r_loss[..., np.newaxis]
q1_loss = np.logaddexp(0 * z_all_dot_beta, z_all_dot_beta)
q0_loss = -z_all_dot_beta + q1_loss
nll_all[..., 0] += q0_loss
nll_all[..., 1] += q1_loss
# n_ni*k, 2
# a_pvvh_ni (n_ni, 4) -> (n_ni*k, 4)
v_slow = u_grid.evaluate_v_v1(
np.repeat(a_pvvh_ni[:, :4], k, axis=0), b_pv,
mrx_grid, u,
np.zeros(n_ni*k, dtype=np.int),
np.broadcast_to(np.arange(k, dtype=np.int), (n_ni, k)).reshape(-1),
)
# n_ni, k
x_q0_loss = ((v_slow.reshape(n_ni, k, 2) -
a_pvvh_ni[:, np.newaxis, 4:6]) ** 2).sum(axis=-1) * scaling
nll_all[..., 0] += x_q0_loss
# n_ni,
x_q1_loss = ((a_pvvh_ni[:, 2:4] - a_pvvh_ni[:, 4:6]) ** 2).sum(axis=-1) * scaling
nll_all[..., 1] += np.broadcast_to(x_q1_loss, (k, n_ni)).T
nll_all[is_ignore_sub] = np.inf
# flattened inds wrt [0, k*2)
min_inds = nll_all.reshape(n_ni, -1).argmin(axis=-1)
r[ni_inds], q[ni_inds] = np.unravel_index(min_inds, (k, 2))
nll_flat_inds = np.arange(0, n_ni*k*2, k*2) + min_inds
nll[~ignore_n_mask] = nll_all.flat[nll_flat_inds]
return r, q, nll
def optimize_out_softmax_rq_v0(a_pvvh, b_pv, rt_grid, beta,
mrx_grid, u, sigma_x, dt):
"""
Calculate r, q, nll for a single frame (n=n_ped, k=n_vic)
Ignore k that are outside the range of grid, setting:
- r = R_IGNORED
- q = 1
:param a_pvvh: n, 6 | [x v \hat{v}], v is given, \hat{v} from differences
:param b_pv: k, 4
:param rt_grid: (lx, ly, dx, dy, nx, ny) | 6-tuple
:param beta: 1+nx*ny, | weights for rt_grid encoding
:param mrx_grid: (l, m) | 2-tuple
:param u: m, | weights for mrx_grid encoding
:param sigma_x:
:param dt:
:return: r, q that minimize total (r, q, x) nll given v
r: n,
q: n,
nll: n, | loss for each agent
"""
n = a_pvvh.shape[0]
k = b_pv.shape[0]
r = np.zeros(n, dtype=np.int) + R_IGNORED
q = np.ones(n, dtype=np.int)
nll = np.zeros(n)
pe_dist, pa_dist = perp.signed_pe_dist_v1(a_pvvh[:, :4], b_pv)
is_ignore = (pe_dist < -mrx_grid[0]) | (0 < pe_dist) | (pa_dist < 0)
ignore_n_mask = np.all(is_ignore, axis=1)
n_ni = (~ignore_n_mask).sum()
# is_ignore -> compute only x_q1 loss
scaling = 0.5 * (dt / sigma_x) ** 2
nll[ignore_n_mask] = ((a_pvvh[ignore_n_mask, 2:4] -
a_pvvh[ignore_n_mask, 4:6])**2).sum(axis=-1) * scaling
if n_ni == 0:
return r, q, nll
nll_all = np.zeros((n_ni, k, 2))
# *some* k may be valid for *some* n but not for others
ni_inds = np.arange(n)[~ignore_n_mask]
for i, n_ind in enumerate(ni_inds):
k_s = (~is_ignore[n_ind, :]).sum()
if k_s == 0:
# ignore all vic
continue
# make 'x'
rt_all = rd.dist2rt_v1(a_pvvh[[n_ind], :4], b_pv[~is_ignore[n_ind, :], :])
rt_all[rt_all < EPS] = EPS
rt_all = np.log10(rt_all)
rt_all[..., 0] /= 2
# n=1, k_s, m
z_all = b_grid.rt2enc_v1(rt_all, rt_grid)
# k_s,
z_all_dot_beta = (z_all.dot(-beta[1:]) - beta[0])[0]
# k_s, : - log likelihood
r_loss = -(z_all_dot_beta - sp.logsumexp(z_all_dot_beta))
nll_all[i, ~is_ignore[n_ind, :], :] += np.broadcast_to(r_loss, (2, k_s)).T
# k_s,
q1_loss = np.logaddexp(0*z_all_dot_beta, z_all_dot_beta)
q0_loss = -z_all_dot_beta + q1_loss
nll_all[i, ~is_ignore[n_ind, :], 0] += q0_loss
nll_all[i, ~is_ignore[n_ind, :], 1] += q1_loss
# calculate v_slow
# - replicate a_pv to calculate it for all k_s vic at once
# k_s, 2
v_slow = u_grid.evaluate_v_v1(
np.broadcast_to(a_pvvh[[n_ind], :4], (k_s, 4)), b_pv[~is_ignore[n_ind, :], :],
mrx_grid, u,
np.zeros(k_s, dtype=np.int), np.arange(k_s, dtype=np.int)
)
x_q0_loss = ((v_slow - a_pvvh[n_ind, 4:6])**2).sum(axis=-1) * scaling
nll_all[i, ~is_ignore[n_ind, :], 0] += x_q0_loss
# 1,
x_q1_loss = ((a_pvvh[n_ind, 2:4] - a_pvvh[n_ind, 4:6])**2).sum() * scaling
nll_all[i, ~is_ignore[n_ind, :], 1] += x_q1_loss
nll_all[is_ignore[ni_inds]] = np.inf
# flattened inds wrt [0, k*2)
min_inds = nll_all.reshape(n_ni, -1).argmin(axis=-1)
r[ni_inds], q[ni_inds] = np.unravel_index(min_inds, (k, 2))
nll_flat_inds = np.arange(0, n_ni*k*2, k*2) + min_inds
nll[~ignore_n_mask] = nll_all.flat[nll_flat_inds]
return r, q, nll