Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
    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())))
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
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
                               })
Ejemplo n.º 5
0
def on_graph(message: bytes):
    try:
        graph: PEMTrafficScene = PEMTrafficScene.from_bytes(message)
        graph_queue.append(graph2json(graph))
    except:
        print('Failed to parse graph.')
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
 def get_empty_observation(with_ts: float) -> PEMTrafficSceneObservation:
     return PEMTrafficSceneObservation(
         timestamp=with_ts,
         scene=PEMTrafficScene(occupancy_grid=PEMOccupancyGrid(cells=[])),
         meta={'empty': True})
Ejemplo n.º 8
0
    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
Ejemplo n.º 9
0
    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)
Ejemplo n.º 10
0
                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)