def generate_scene(cls, quads: List[List[QuadKey]], others: List[PEMDynamicActor], egos: List[PEMDynamicActor], with_occupant: bool) -> PEMTrafficScene: grid: PEMOccupancyGrid = PEMOccupancyGrid(cells=[]) idx: int = random.randint(0, len(egos) - 1) ego: PEMDynamicActor = egos[idx] quadkeys: List[QuadKey] = quads[idx] for qk in quadkeys: state: GridCellState = random.choice(STATES) occupant: Union[PEMDynamicActor, None] = random.choice( others ) if with_occupant and state == GridCellState.occupied() else None grid.cells.append( PEMGridCell(hash=qk.to_quadint(), state=PEMRelation(cls.rand_prob(), state), occupant=PEMRelation(cls.rand_prob(), occupant))) scene: PEMTrafficScene = PEMTrafficScene(timestamp=time.time(), measured_by=ego, occupancy_grid=grid) return scene
def split_by_level( cls, observations: List[PEMTrafficSceneObservation] ) -> List[PEMTrafficSceneObservation]: scenes: Dict[str, List[PEMTrafficSceneObservation]] = { } # parent tile to scene for o in observations: contained_parents: Set[str] = set() for cell in o.value.occupancy_grid.cells: parent_str: str = cls.cache_get( cell.hash)[:REMOTE_GRID_TILE_LEVEL] contained_parents.add(parent_str) if parent_str not in scenes: scenes[parent_str] = [] for parent in contained_parents: scenes[parent].append( PEMTrafficSceneObservation( timestamp=o.timestamp, scene=PEMTrafficScene( **{ 'timestamp': o.value.timestamp, 'min_timestamp': o.value.min_timestamp, 'max_timestamp': o.value.max_timestamp, 'occupancy_grid': PEMOccupancyGrid( **{ 'cells': [ c for c in o.value.occupancy_grid.cells if cls.cache_get( c.hash).startswith(parent) ] }) }), meta={ 'parent': QuadKey(parent), **o.meta })) return list(itertools.chain(*list(scenes.values())))
def _on_remote_graph(self, msg: bytes): in_time: float = time.time() try: scene: PEMTrafficScene = PEMTrafficScene.from_bytes(msg) obs: PEMTrafficSceneObservation = PEMTrafficSceneObservation(time.time(), scene, meta={'sender': int(self.ego_id)}) if self.alive and self.recording and self.remote_grid_sink: if OBS_GRAPH_REMOTE not in self.remote_grid_sink.accumulator: self.remote_grid_sink.push(OBS_GRAPH_REMOTE, [obs]) else: self.remote_grid_sink.accumulator[OBS_GRAPH_REMOTE].append(obs) self.timings.start('d6', custom_time=in_time) self.timings.stop('d6') except KeyError: return self.timings.start('d5', custom_time=scene.last_timestamp) self.timings.stop('d5', custom_time=in_time)
MOCK_ACTUAL_1: List[Ogtc] = [ Ogtc(tile=REF_PARENT_1, occupied_cells=frozenset([REF_TILE_1_1]), ts=REF_TIME_1) ] MOCK_LOCAL_1: List[PEMTrafficSceneObservation] = [ PEMTrafficSceneObservation( timestamp=REF_TIME_1 + 1.0, scene=PEMTrafficScene( timestamp=REF_TIME_1 + 0.15, occupancy_grid=PEMOccupancyGrid(cells=[ PEMGridCell( hash=REF_TILE_1_1.to_quadint(), state=PEMRelation(confidence=.5, object=GridCellState.occupied())), PEMGridCell(hash=REF_TILE_1_2.to_quadint(), state=PEMRelation(confidence=.5, object=GridCellState.free())) ])), meta={ 'sender': 1, 'parent': REF_PARENT_1 }), PEMTrafficSceneObservation(timestamp=REF_TIME_1 + 2.0, scene=PEMTrafficScene(), meta={ 'sender': 1, 'parent': REF_PARENT_1 })
def on_graph(message: bytes): try: graph: PEMTrafficScene = PEMTrafficScene.from_bytes(message) graph_queue.append(graph2json(graph)) except: print('Failed to parse graph.')
def _on_grid(self, obs: OccupancyGridObservation): _ts = time.monotonic() ts1, grid = obs.timestamp, obs.value actors_ego_obs = self.om.latest(OBS_ACTOR_EGO) actors_others_obs = self.om.latest(OBS_ACTORS_RAW) if not grid or not actors_ego_obs or not actors_others_obs or not actors_ego_obs.value or len(actors_ego_obs.value) < 1: return ts: float = min([ts1, actors_ego_obs.timestamp, actors_others_obs.timestamp]) self.timings.start('d0', custom_time=ts) self.timings.stop('d0') ts2: float = time.time() ego_actor: DynamicActor = actors_ego_obs.value[0] # Turned off for evaluation, because occupants are not fused anyway # visible_actors: Dict[str, DynamicActor] = get_occupied_cells_multi_map(actors_others_obs.value + [ego_actor]) visible_actors: Dict[str, DynamicActor] = {} # Generate PEM complex object attributes pem_ego = map_pem_actor(ego_actor) pem_grid = PEMOccupancyGrid(cells=[]) for cell in grid.cells: group_key = f'cell_occupant_{cell.quad_key.key}' state_relation: PEMRelation[GridCellState] = PEMRelation(cell.state.confidence, GridCellState(cell.state.value)) occupant_relation: PEMRelation[Optional[PEMDynamicActor]] = PEMRelation(0, None) if cell.quad_key.key in visible_actors: actor = visible_actors[cell.quad_key.key] self.tracker.track(group_key, str(actor.id)) occupant_relation = PEMRelation(self.tracker.get(group_key, str(actor.id)), map_pem_actor(actor)) else: occupant_relation = PEMRelation(cell.state.confidence, None) self.tracker.cycle_group(group_key) # Consistency between state and occupant presence if occupant_relation.object and state_relation.object != GridCellState.occupied(): if occupant_relation.confidence > state_relation.confidence: state_relation = PEMRelation(occupant_relation.confidence, GridCellState.occupied()) else: occupant_relation = PEMRelation(state_relation.confidence, None) pem_grid.cells.append(PEMGridCell( hash=self._get_quadint(cell.quad_key), state=state_relation, occupant=occupant_relation )) now = time.time() # Generate PEM graph graph = PEMTrafficScene(timestamp=ts, last_timestamp=now, measured_by=pem_ego, occupancy_grid=pem_grid) encoded_msg = graph.to_bytes() self.timings.start('d1', custom_time=ts2) self.timings.stop('d1') obs: PEMTrafficSceneObservation = PEMTrafficSceneObservation(now, graph, meta={'sender': int(self.ego_id)}) self.inbound.publish(OBS_GRAPH_LOCAL, obs) if self.alive and self.recording and self.local_grid_sink: if OBS_GRAPH_LOCAL not in self.local_grid_sink.accumulator: self.local_grid_sink.push(OBS_GRAPH_LOCAL, [obs]) else: self.local_grid_sink.accumulator[OBS_GRAPH_LOCAL].append(obs) if self.tss.active: self.tss.publish_graph(encoded_msg) # Debug logging self.tsdiffhistory3.append(time.monotonic() - self.last_publish) self.last_publish = time.monotonic() # logging.debug(f'PUBLISH: {np.mean(self.tsdiffhistory3)}') self.tsdiffhistory2.append(time.monotonic() - _ts)
def get_empty_observation(with_ts: float) -> PEMTrafficSceneObservation: return PEMTrafficSceneObservation( timestamp=with_ts, scene=PEMTrafficScene(occupancy_grid=PEMOccupancyGrid(cells=[])), meta={'empty': True})
def read_data( self, downsample_ground_truth: float = 1., downsample_observations: float = 1. ) -> Tuple[List[PEMTrafficSceneObservation], List[PEMTrafficSceneObservation], List[Ogtc]]: assert downsample_ground_truth <= 1 and downsample_observations <= 1 logging.debug( f'downsample_ground_truth={downsample_ground_truth}, downsample_observations={downsample_observations}' ) logging.debug('Reading directory info.') files_actual: List[str] = list( filter(lambda s: s.startswith(self.file_prefix), os.listdir(self.data_dir_actual))) files_observed: List[str] = list( filter(lambda s: s.startswith(self.file_prefix), os.listdir(self.data_dir_observed))) occupancy_ground_truth: List[Ogtc] = [] occupancy_observations_local: List[PEMTrafficSceneObservation] = [] occupancy_observations_remote: List[PEMTrafficSceneObservation] = [] logging.debug('Reading ground truth.') for file_name in files_actual: with open(os.path.join(self.data_dir_actual, file_name), 'rb') as f: try: occupancy_ground_truth += pickle.load(f) except EOFError: logging.warning(f'File {file_name} corrupt.') logging.debug( f'Got {len(occupancy_ground_truth)} ground truth data points.') logging.debug(f'Reading and decoding observations.') for file_name in files_observed: with open(os.path.join(self.data_dir_observed, file_name), 'rb') as f: try: if 'remote' not in file_name: occupancy_observations_local.extend(pickle.load(f)) else: data = pickle.load(f) assert isinstance(data, list) if len(data) < 1: continue if isinstance(data[0], RawBytesObservation): for obs in data: try: occupancy_observations_remote.append( PEMTrafficSceneObservation( timestamp=obs.timestamp, scene=PEMTrafficScene.from_bytes( obs.value), meta=obs.meta)) except KeyError: continue elif isinstance(data[0], PEMTrafficSceneObservation): occupancy_observations_remote.extend(data) except EOFError: logging.warning(f'File {file_name} corrupt.') logging.debug( f'Got {len(occupancy_observations_local)} local and {len(occupancy_observations_remote)} remote observations.' ) occupancy_ground_truth = sorted(random.sample( occupancy_ground_truth, k=math.floor( len(occupancy_ground_truth) * downsample_ground_truth)), key=attrgetter('ts')) occupancy_observations_local = sorted(random.sample( occupancy_observations_local, k=math.floor( len(occupancy_observations_local) * downsample_observations)), key=attrgetter('timestamp')) occupancy_observations_remote = sorted(random.sample( occupancy_observations_remote, k=math.floor( len(occupancy_observations_remote) * downsample_observations)), key=attrgetter('timestamp')) return occupancy_observations_local, occupancy_observations_remote, occupancy_ground_truth
def fuse(cls, local_observation: PEMTrafficSceneObservation, remote_observation: PEMTrafficSceneObservation, ref_time: float) -> PEMTrafficSceneObservation: assert local_observation.meta['parent'] == remote_observation.meta[ 'parent'] cells: List[PEMGridCell] = [] ts1: float = local_observation.value.timestamp ts2: float = remote_observation.value.min_timestamp + ( remote_observation.value.max_timestamp - remote_observation.value.min_timestamp) / 2 weights: Tuple[float, float] = (cls._decay(ts1, ref_time), cls._decay(ts2, ref_time)) states: List[GridCellState] = GridCellState.options() local_cells: Dict[QuadKey, PEMGridCell] = { QuadKey(cls.cache_get(c.hash)): c for c in local_observation.value.occupancy_grid.cells } remote_cells: Dict[QuadKey, PEMGridCell] = { QuadKey(cls.cache_get(c.hash)): c for c in remote_observation.value.occupancy_grid.cells } keys: Set[QuadKey] = set().union(set(local_cells.keys()), set(remote_cells.keys())) for qk in keys: if qk in local_cells and qk not in remote_cells: cells.append(local_cells[qk]) elif qk not in local_cells and qk in remote_cells: cells.append(remote_cells[qk]) else: weight_sum: float = sum(weights) new_cell: PEMGridCell = PEMGridCell(hash=local_cells[qk].hash) state_vec: List[float] = [0] * GridCellState.N for i in range(len(state_vec)): v1: float = local_cells[qk].state.confidence * weights[ 0] if local_cells[qk].state.object == states[i] else 0 v2: float = remote_cells[qk].state.confidence * weights[ 1] if remote_cells[qk].state.object == states[i] else 0 # Override unknown if i == Gss.UNKNOWN and sum(state_vec[:Gss.UNKNOWN]) > 0: if v1 > 0: v1 = 0 weight_sum -= weights[0] elif v2 > 0: v2 = 0 weight_sum -= weights[1] state_vec[i] = v1 + v2 state_vec = [c / weight_sum for c in state_vec] max_conf: float = max(state_vec) max_state: int = state_vec.index(max_conf) new_cell.state = PEMRelation( object=GridCellState(value=Gss(max_state)), confidence=max_conf) cells.append(new_cell) return PEMTrafficSceneObservation( timestamp=ref_time, # shouldn't matter scene=PEMTrafficScene( **{'occupancy_grid': PEMOccupancyGrid(cells=cells)}), meta=local_observation.meta)
PEMRelation[GridCellState](confidence=0.65, object=GridCellState.occupied()), 'occupant': PEMRelation[PEMDynamicActor](confidence=0.91, object=ego2) }), PEMGridCell( **{ 'hash': 2222, 'state': PEMRelation[GridCellState](confidence=0.88, object=GridCellState.free()), 'occupant': None }) ] grid1.cells = cells scene1: PEMTrafficScene = PEMTrafficScene() scene1.occupancy_grid = grid1 scene1.measured_by = ego1 encoded_msg: bytes = scene1.to_bytes() scene1: PEMTrafficScene = PEMTrafficScene.from_bytes(encoded_msg) print(scene1.measured_by.id) print(scene1.measured_by.position.object.x) print(scene1.occupancy_grid.cells[0].state.confidence) print(scene1.occupancy_grid.cells[0].state.object) print(scene1.occupancy_grid.cells[0].occupant.object.id)