def optimal_proposal(graph: MultiDiGraph, particle: np.ndarray, new_observation: Union[None, np.ndarray], time_interval: float, mm_model: MapMatchingModel, full_smoothing: bool = True, d_refine: float = 1., d_max: float = None, d_max_fail_multiplier: float = 1.5, d_max_threshold: tuple = (0.9, 0.1), num_inter_cut_off: int = None, only_norm_const: bool = False, store_norm_quants: bool = False, resample_fails: bool = True) -> Union[Tuple[Union[None, np.ndarray], float, Union[float, np.ndarray]], float]: """ Samples a single particle from the (distance discretised) optimal proposal. :param graph: encodes road network, simplified and projected to UTM :param particle: single element of MMParticles.particles :param new_observation: cartesian coordinate in UTM :param time_interval: time between last observation and newly received observation :param mm_model: MapMatchingModel :param full_smoothing: if True returns full trajectory otherwise returns only x_t-1 to x_t :param d_refine: metres, resolution of distance discretisation :param d_max: optional override of d_max = mm_model.d_max(time_interval) :param d_max_fail_multiplier: extension of d_max in case all probs are 0 :param d_max_threshold: tuple defining when to extend d_max extend if total sample prob of distances > d_max * d_max_threshold[0] larger than d_max_threshold[1] :param num_inter_cut_off: maximum number of intersections to cross in the time interval :param only_norm_const: if true only return prior normalising constant (don't sample) :param store_norm_quants: whether to additionally return quantities needed for gradient EM step assuming deviation prior is used :param resample_fails: whether to return None (and induce later resampling of whole trajectory) if proposal fails to find route with positive probability if False assume distance=0 :return: (particle, unnormalised weight, prior_norm) or (particle, unnormalised weight, dev_norm_quants) """ if particle is None: return 0. if only_norm_const else (None, 0., 0.) if isinstance(new_observation, list): new_observation = np.array(new_observation) if num_inter_cut_off is None: num_inter_cut_off = max(int(time_interval / 1.5), 10) if d_max is None: d_max = mm_model.d_max(time_interval) # Extract all possible routes from previous position start_position = particle[-1:].copy() start_position[0, -1] = 0 possible_routes = get_all_possible_routes_overshoot(graph, start_position, d_max, num_inter_cut_off=num_inter_cut_off) # Get all possible positions on each route discretised_routes_indices_list = [] discretised_routes_list = [] for i, route in enumerate(possible_routes): # All possible end positions of route discretised_edge_matrix = discretise_edge(graph, route[-1, 1:4], d_refine) if route.shape[0] == 1: discretised_edge_matrix = discretised_edge_matrix[discretised_edge_matrix[:, 0] >= particle[-1, 4]] discretised_edge_matrix[:, -1] -= discretised_edge_matrix[-1, -1] else: discretised_edge_matrix[:, -1] += route[-2, -1] discretised_edge_matrix = discretised_edge_matrix[discretised_edge_matrix[:, -1] < d_max + 1e-5] # Track route index and append to list if discretised_edge_matrix is not None and len(discretised_edge_matrix) > 0: discretised_routes_indices_list += [np.ones(discretised_edge_matrix.shape[0], dtype=int) * i] discretised_routes_list += [discretised_edge_matrix] # Concatenate into numpy.ndarray discretised_routes_indices = np.concatenate(discretised_routes_indices_list) discretised_routes = np.concatenate(discretised_routes_list) if len(discretised_routes) == 0 or (len(discretised_routes) == 1 and discretised_routes[0][-1] == 0): if only_norm_const: return 0 if resample_fails: return None, 0., 0. else: sampled_dis_route = discretised_routes[0] # Append sampled route to old particle sampled_route = possible_routes[0] proposal_out = process_proposal_output(particle, sampled_route, sampled_dis_route, time_interval, full_smoothing) return proposal_out, 0., 0. # Distance prior evals distances = discretised_routes[:, -1] distance_prior_evals = mm_model.distance_prior_evaluate(distances, time_interval) # Deviation prior evals deviation_prior_evals = mm_model.deviation_prior_evaluate(particle[-1, 5:7], discretised_routes[:, 1:3], discretised_routes[:, -1]) # Normalise prior/transition probabilities prior_probs = distance_prior_evals * deviation_prior_evals prior_probs_norm_const = prior_probs.sum() if only_norm_const: if store_norm_quants: deviations = np.sqrt(np.sum((particle[-1, 5:7] - discretised_routes[:, 1:3]) ** 2, axis=1)) deviations = np.abs(deviations - discretised_routes[:, -1]) # Z, dZ/d(dist_params), dZ/d(deviation_beta) dev_norm_quants = np.array([prior_probs_norm_const, *np.sum(mm_model.distance_prior_gradient(distances, time_interval) .reshape(len(mm_model.distance_params), len(distances)) * deviation_prior_evals, axis=-1), -np.sum(deviations * distance_prior_evals * deviation_prior_evals) ]) return dev_norm_quants else: return prior_probs_norm_const prior_probs /= prior_probs_norm_const # Likelihood evaluations likelihood_evals = mm_model.likelihood_evaluate(discretised_routes[:, 1:3], new_observation) # Calculate sample probabilities sample_probs = prior_probs[likelihood_evals > 0] * likelihood_evals[likelihood_evals > 0] # sample_probs = prior_probs * likelihood_evals # p(y_m | x_m-1^j) prop_weight = sample_probs.sum() model_d_max = mm_model.d_max(time_interval) if prop_weight < 1e-100 \ or (np.sum(sample_probs[np.where(distances[likelihood_evals > 0] > (d_max * d_max_threshold[0]))[0]])/prop_weight > d_max_threshold[1]\ and (not d_max > model_d_max)): if (d_max - np.max(distances)) < d_refine + 1e-5 \ and d_max_fail_multiplier > 1 and (not d_max > model_d_max): return optimal_proposal(graph, particle, new_observation, time_interval, mm_model, full_smoothing, d_refine, d_max=d_max * d_max_fail_multiplier, num_inter_cut_off=num_inter_cut_off, only_norm_const=only_norm_const, store_norm_quants=store_norm_quants, resample_fails=resample_fails) if resample_fails: proposal_out = None else: sampled_dis_route_index = np.where(discretised_routes[:, -1] == 0)[0][0] sampled_dis_route = discretised_routes[sampled_dis_route_index] # Append sampled route to old particle sampled_route = possible_routes[discretised_routes_indices[sampled_dis_route_index]] proposal_out = process_proposal_output(particle, sampled_route, sampled_dis_route, time_interval, full_smoothing) prop_weight = 0. else: # Sample an edge and distance sampled_dis_route_index = np.random.choice(len(sample_probs), 1, p=sample_probs / prop_weight)[0] sampled_dis_route = discretised_routes[likelihood_evals > 0][sampled_dis_route_index] # Append sampled route to old particle sampled_route = possible_routes[discretised_routes_indices[likelihood_evals > 0][sampled_dis_route_index]] proposal_out = process_proposal_output(particle, sampled_route, sampled_dis_route, time_interval, full_smoothing) if store_norm_quants: deviations = np.sqrt(np.sum((particle[-1, 5:7] - discretised_routes[:, 1:3]) ** 2, axis=1)) deviations = np.abs(deviations - discretised_routes[:, -1]) # Z, dZ/d(dist_params), dZ/d(deviation_beta) dev_norm_quants = np.array([prior_probs_norm_const, *np.sum(mm_model.distance_prior_gradient(distances, time_interval) .reshape(len(mm_model.distance_params), len(distances)) * deviation_prior_evals, axis=-1), -np.sum(deviations * distance_prior_evals * deviation_prior_evals) ]) return proposal_out, prop_weight, dev_norm_quants else: return proposal_out, prop_weight, prior_probs_norm_const
def sample_route( graph: MultiDiGraph, timestamps: Union[float, np.ndarray], num_obs: int = None, mm_model: MapMatchingModel = ExponentialMapMatchingModel(), d_refine: float = 1., start_position: np.ndarray = None, num_inter_cut_off: int = None) -> Tuple[np.ndarray, np.ndarray]: """ Runs offline map-matching. I.e. receives a full polyline and returns an equal probability collection of trajectories. Forward-filtering backward-simulation implementation - no fixed-lag approximation needed for offline inference. :param graph: encodes road network, simplified and projected to UTM :param timestamps: seconds either float if all times between observations are the same, or a series of timestamps in seconds/UNIX timestamp :param num_obs: int length of observed polyline to generate :param mm_model: MapMatchingModel :param d_refine: metres, resolution of distance discretisation :param start_position: optional start position; array (u, v, k, alpha) :param num_inter_cut_off: maximum number of intersections to cross in the time interval :return: tuple with sampled route (array with same shape as a single MMParticles) and polyline (array with shape (num_obs, 2)) """ if isinstance(timestamps, np.ndarray): num_obs = len(timestamps) + 1 time_interval_arr = get_time_interval_array(timestamps, num_obs) if start_position is None: start_position = random_positions(graph, 1)[0] start_geom = edges.get_geometry(graph, start_position) start_coords = edges.edge_interpolate(start_geom, start_position[-1]) full_sampled_route = np.concatenate([[0.], start_position, start_coords, [0.]])[np.newaxis] for k in range(num_obs - 1): time_interval = time_interval_arr[k] d_max = mm_model.d_max(time_interval) num_inter_cut_off_i = max( int(time_interval / 1.5), 10) if num_inter_cut_off is None else num_inter_cut_off prev_pos = full_sampled_route[-1:].copy() prev_pos[0, 0] = 0. prev_pos[0, -1] = 0. possible_routes = proposal.get_all_possible_routes_overshoot( graph, prev_pos, d_max, num_inter_cut_off=num_inter_cut_off_i) # Get all possible positions on each route discretised_routes_indices_list = [] discretised_routes_list = [] for i, route in enumerate(possible_routes): # All possible end positions of route discretised_edge_matrix = edges.discretise_edge( graph, route[-1, 1:4], d_refine) if route.shape[0] == 1: discretised_edge_matrix = discretised_edge_matrix[ discretised_edge_matrix[:, 0] >= full_sampled_route[-1, 4]] discretised_edge_matrix[:, -1] -= discretised_edge_matrix[-1, -1] else: discretised_edge_matrix[:, -1] += route[-2, -1] discretised_edge_matrix = discretised_edge_matrix[ discretised_edge_matrix[:, -1] < d_max + 1e-5] # Track route index and append to list if discretised_edge_matrix is not None and len( discretised_edge_matrix) > 0: discretised_routes_indices_list += [ np.ones(discretised_edge_matrix.shape[0], dtype=int) * i ] discretised_routes_list += [discretised_edge_matrix] # Concatenate into numpy.ndarray discretised_routes_indices = np.concatenate( discretised_routes_indices_list) discretised_routes = np.concatenate(discretised_routes_list) if len(discretised_routes) == 0 or (len(discretised_routes) == 1 and discretised_routes[0][-1] == 0): warnings.warn('sample_route exited prematurely') break # Distance prior evals distances = discretised_routes[:, -1] distance_prior_evals = mm_model.distance_prior_evaluate( distances, time_interval) # Deviation prior evals deviation_prior_evals = mm_model.deviation_prior_evaluate( full_sampled_route[-1, 5:7], discretised_routes[:, 1:3], discretised_routes[:, -1]) # Normalise prior/transition probabilities prior_probs = distance_prior_evals * deviation_prior_evals prior_probs_norm_const = prior_probs.sum() sampled_dis_route_index = np.random.choice(len(prior_probs), 1, p=prior_probs / prior_probs_norm_const)[0] sampled_dis_route = discretised_routes[sampled_dis_route_index] # Append sampled route to old particle sampled_route = possible_routes[ discretised_routes_indices[sampled_dis_route_index]] full_sampled_route = proposal.process_proposal_output( full_sampled_route, sampled_route, sampled_dis_route, time_interval, True) obs_indices = edges.observation_time_indices(full_sampled_route[:, 0]) polyline = full_sampled_route[obs_indices, 5:7] \ + mm_model.gps_sd * np.random.normal(size=(obs_indices.sum(), 2)) return full_sampled_route, polyline