diff --git a/mpi_sim/components/occupany_grid_mapping.py b/mpi_sim/components/occupany_grid_mapping.py index 3a2b4587a7178d968e8acea555a36f3a5ecf357c..419f91e42733244560bd56a132bc90a3189b4a25 100644 --- a/mpi_sim/components/occupany_grid_mapping.py +++ b/mpi_sim/components/occupany_grid_mapping.py @@ -127,9 +127,6 @@ class OccupancyGridMapping(SensorComponent): resolution=self._resolution ) - # Initialize the transformation - self.transform = self.config.transform - def _add_to_simulation(self): @@ -155,11 +152,14 @@ class OccupancyGridMapping(SensorComponent): ) else : imported_global_map = np.load(file=self.config.imported_global_map_path) + if self.config.transform: + imported_global_map=self.config.transform(imported_global_map) self.state.global_map = mpi_sim.utils.OccupancyGridMap( map=imported_global_map, area=self._global_map_area, resolution=self._resolution ) + return self.state.global_map.map @@ -177,16 +177,12 @@ class OccupancyGridMapping(SensorComponent): ) else : # Same but we need to relocate the position from the center of the imported room - coord_of_imported_map_center = mpi_sim.utils.transform_map_coordinate_to_position( - map_coordinate=self.config.imported_global_map_px_coord_center, - map_area=self.config.imported_global_map_area, - map_resolution=self.config.imported_global_map_resolution - ) self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map( global_map=self.state.global_map, - position=self.object.position + coord_of_imported_map_center, + position=self.object.position, orientation=constraint_angle(self.object.orientation), depth=self.config.depth, + from_given_center = self.config.imported_global_map_px_coord_center ) return self.state.local_map.map diff --git a/mpi_sim/utils/occupancy_grid_map.py b/mpi_sim/utils/occupancy_grid_map.py index 8e1228056b6b10323570540069fef1445cb1f181..67a5870ce78219b9d5cc13feb12fb3db9fa56c9a 100644 --- a/mpi_sim/utils/occupancy_grid_map.py +++ b/mpi_sim/utils/occupancy_grid_map.py @@ -180,7 +180,7 @@ def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_fil return map -def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0): +def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None): r"""Creates a local perspective of an occupancy grid map. Args: @@ -201,12 +201,18 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position= # TODO: there should be a way to avoid the transpose operations, by choosing a different points that should be local_map_length = int(depth * 2 / global_map.resolution) + + if from_given_center is None : + pos_in_map = transform_position_to_map_coordinate( + position, + map_area=global_map.area, + map_resolution=global_map.resolution + ) + else : + center_x, center_y = from_given_center + position_px = position/global_map.resolution + pos_in_map = np.array([center_x - np.array(position_px)[1], center_y + np.array(position_px)[0]]) - pos_in_map = transform_position_to_map_coordinate( - position, - map_area=global_map.area, - map_resolution=global_map.resolution - ) orientation = mpi_sim.utils.constraint_angle(orientation) # rotation in for cv operator is in opposite direction @@ -221,7 +227,6 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position= rect_ur = pos_in_map + 0.5 * hvect * local_map_length + 0.5 * vvect * local_map_length rect_lr = pos_in_map + 0.5 * hvect * local_map_length - 0.5 * vvect * local_map_length src_pts = np.vstack([rect_ll, rect_ul, rect_ur, rect_lr]).astype(np.float32) - # src_pts = src_pts # print('src_pts', src_pts) # destination points in pixel space