Make video (#457)

* fix lane line error

* true

* load once

* update safe env

* safe env

* add static obj option

* position bug

* move logger initialization pos

* fix bug

* disable aspect ratio

* batch convert

* point cloud

* update default pos

* fix bug

* update top down viewer

* depth

* camera position

* fix multi polygon

* update nuscenes dataset

* add assert

* fix test

* format

* format

metadrive/assets/shaders/depth_cam.frag.glsl

@@ -6,7 +6,8 @@ in vec4 color;
 out vec4 fragColor;
 
 void main() {
-  float base=8;
+  float base=5;
+  // distance=32
   float b = 16;
   float c = log(distanceToCamera/base)/log(b);
   fragColor = vec4(c, c, c, 0);

metadrive/base_class/base_object.py

@@ -488,6 +488,8 @@ def convert_to_world_coordinates(self, vector, origin):
         In a word, for calculating **points transformation** in different coordinates, origin is required. This is
         because vectors have no origin but origin is required to define a point.
         """
+        assert len(vector) == 2 or len(vector) == 3, "the vector should be in shape (2,) or (3,)"
+        vector = vector[:2]
         vector = LVector3(*vector, 0.)
         vector = self.engine.origin.getRelativeVector(self.origin, vector)
         project_on_x = vector[0]

metadrive/component/map/base_map.py

@@ -1,15 +1,12 @@
 import logging
-import geopandas as gpd
 import math
 
 import cv2
 import numpy as np
-from shapely import affinity
-from shapely.geometry import Polygon
-
 from metadrive.base_class.base_runnable import BaseRunnable
 from metadrive.constants import MapTerrainSemanticColor, MetaDriveType, DrivableAreaProperty
 from metadrive.engine.engine_utils import get_global_config
+from shapely.geometry import Polygon, MultiPolygon
 
 logger = logging.getLogger(__name__)
 
@@ -97,7 +94,11 @@ def num_blocks(self):
     def destroy(self):
         self.detach_from_world()
         if self._semantic_map is not None:
-            np.delete(self._semantic_map)
+            del self._semantic_map
+            self._semantic_map = None
+        if self._height_map is not None:
+            del self._height_map
+            self._height_map = None
 
         for block in self.blocks:
             block.destroy()
@@ -214,7 +215,7 @@ def get_height_map(
         height=1,
     ):
         """
-        Get semantics of the map
+        Get height of the map
         :param size: [m] length and width
         :param pixels_per_meter: the returned map will be in (size*pixels_per_meter * size*pixels_per_meter) size
         :param extension: If > 1, the returned height map's drivable region will be enlarged.
@@ -238,16 +239,21 @@ def get_height_map(
             for polygon in polygons:
                 if need_scale:
                     scaled_polygon = Polygon(polygon).buffer(extension, join_style=2)
-                    points = [
-                        [
-                            int(
-                                (scaled_polygon.exterior.coords.xy[0][index] - center_p[0]) * pixels_per_meter +
+                    if isinstance(scaled_polygon, MultiPolygon):
+                        scaled_polygons = scaled_polygon.geoms
+                    else:
+                        scaled_polygons = [scaled_polygon]
+                    for scaled_polygon in scaled_polygons:
+                        points = [
+                            [
+                                int(
+                                    (scaled_polygon.exterior.coords.xy[0][index] - center_p[0]) * pixels_per_meter +
+                                    size / 2
+                                ),
+                                int((scaled_polygon.exterior.coords.xy[1][index] - center_p[1]) * pixels_per_meter) +
                                 size / 2
-                            ),
-                            int((scaled_polygon.exterior.coords.xy[1][index] - center_p[1]) * pixels_per_meter) +
-                            size / 2
-                        ] for index in range(len(scaled_polygon.exterior.coords.xy[0]))
-                    ]
+                            ] for index in range(len(scaled_polygon.exterior.coords.xy[0]))
+                        ]
                 else:
                     points = [
                         [

metadrive/component/map/scenario_map.py

@@ -8,6 +8,7 @@
 from metadrive.engine.asset_loader import AssetLoader
 from metadrive.type import MetaDriveType
 from metadrive.scenario.scenario_description import ScenarioDescription
+from metadrive.utils.math import resample_polyline, get_polyline_length
 
 
 class ScenarioMap(BaseMap):
@@ -59,71 +60,74 @@ def get_boundary_line_vector(self, interval):
             if MetaDriveType.is_road_line(type):
                 if len(data[ScenarioDescription.POLYLINE]) <= 1:
                     continue
+                line = np.asarray(data[ScenarioDescription.POLYLINE])[..., :2]
+                length = get_polyline_length(line)
+                resampled = resample_polyline(line, interval) if length > interval * 2 else line
                 if MetaDriveType.is_broken_line(type):
                     ret[map_feat_id] = {
                         "type": MetaDriveType.LINE_BROKEN_SINGLE_YELLOW
                         if MetaDriveType.is_yellow_line(type) else MetaDriveType.LINE_BROKEN_SINGLE_WHITE,
-                        "polyline": np.asarray(data[ScenarioDescription.POLYLINE])
+                        "polyline": resampled
                     }
                 else:
                     ret[map_feat_id] = {
-                        "polyline": np.asarray(data[ScenarioDescription.POLYLINE]),
+                        "polyline": resampled,
                         "type": MetaDriveType.LINE_SOLID_SINGLE_YELLOW
                         if MetaDriveType.is_yellow_line(type) else MetaDriveType.LINE_SOLID_SINGLE_WHITE
                     }
             elif MetaDriveType.is_road_edge(type):
-                ret[map_feat_id] = {
-                    "polyline": np.asarray(data[ScenarioDescription.POLYLINE]),
-                    "type": MetaDriveType.BOUNDARY_LINE
-                }
+                line = np.asarray(data[ScenarioDescription.POLYLINE])[..., :2]
+                length = get_polyline_length(line)
+                resampled = resample_polyline(line, interval) if length > interval * 2 else line
+                ret[map_feat_id] = {"polyline": resampled, "type": MetaDriveType.BOUNDARY_LINE}
             elif type == MetaDriveType.LANE_SURFACE_STREET:
                 continue
             # else:
             # # for debug
             #     raise ValueError
         return ret
 
-    def get_map_features(self, interval=2):
-        """
-        TODO LQY: Consider removing it as I prefer a resampled one
-        """
-        map_features = super(ScenarioMap, self).get_map_features(interval=interval)
-
-        # Adding the information stored in original data to here
-        original_map_features = self.engine.data_manager.get_scenario(self.map_index)["map_features"]
-
-        for map_feat_key, old_map_feat in original_map_features.items():
-
-            if map_feat_key not in map_features:
-                # Discard the data for those map features that are not reconstructed by MetaDrive.
-                pass
-
-                # old_map_feat = copy.deepcopy(old_map_feat)
-                # old_map_feat["valid"] = False
-                #
-                # map_features[map_feat_key] = old_map_feat
-                #
-                # if "polyline" in old_map_feat:
-                #     map_features[map_feat_key]["polyline"] = convert_polyline_to_metadrive(
-                #         old_map_feat["polyline"], coordinate_transform=self.coordinate_transform
-                #     )
-                #
-                # if "position" in old_map_feat:
-                #     if self.coordinate_transform:
-                #         map_features[map_feat_key]["position"] = waymo_to_metadrive_vector(old_map_feat["position"])
-                #     else:
-                #         map_features[map_feat_key]["position"] = old_map_feat["position"]
-
-            else:
-                # This map features are in both original data and current data.
-                # We will check if in original data this map features contains some useful metadata.
-                # If so, copied it to the new data.
-                if "speed_limit_kmh" in old_map_feat:
-                    map_features[map_feat_key]["speed_limit_kmh"] = old_map_feat["speed_limit_kmh"]
-                if "speed_limit_mph" in old_map_feat:
-                    map_features[map_feat_key]["speed_limit_mph"] = old_map_feat["speed_limit_mph"]
-
-        return map_features
+    # def get_map_features(self, interval=2):
+    #     """
+    #
+    #     """
+    #     map_features = super(ScenarioMap, self).get_map_features(interval=interval)
+    #
+    #     # Adding the information stored in original data to here
+    #     original_map_features = self.engine.data_manager.get_scenario(self.map_index)["map_features"]
+    #
+    #     for map_feat_key, old_map_feat in original_map_features.items():
+    #
+    #         if map_feat_key not in map_features:
+    #             # Discard the data for those map features that are not reconstructed by MetaDrive.
+    #             pass
+    #
+    #             # old_map_feat = copy.deepcopy(old_map_feat)
+    #             # old_map_feat["valid"] = False
+    #             #
+    #             # map_features[map_feat_key] = old_map_feat
+    #             #
+    #             # if "polyline" in old_map_feat:
+    #             #     map_features[map_feat_key]["polyline"] = convert_polyline_to_metadrive(
+    #             #         old_map_feat["polyline"], coordinate_transform=self.coordinate_transform
+    #             #     )
+    #             #
+    #             # if "position" in old_map_feat:
+    #             #     if self.coordinate_transform:
+    #             #         map_features[map_feat_key]["position"] = waymo_to_metadrive_vector(old_map_feat["position"])
+    #             #     else:
+    #             #         map_features[map_feat_key]["position"] = old_map_feat["position"]
+    #
+    #         else:
+    #             # This map features are in both original data and current data.
+    #             # We will check if in original data this map features contains some useful metadata.
+    #             # If so, copied it to the new data.
+    #             if "speed_limit_kmh" in old_map_feat:
+    #                 map_features[map_feat_key]["speed_limit_kmh"] = old_map_feat["speed_limit_kmh"]
+    #             if "speed_limit_mph" in old_map_feat:
+    #                 map_features[map_feat_key]["speed_limit_mph"] = old_map_feat["speed_limit_mph"]
+    #
+    #     return map_features
 
 
 if __name__ == "__main__":

metadrive/component/scenario_block/scenario_block.py

@@ -54,9 +54,9 @@ def create_in_world(self):
         # draw
         for lane_id, data in self.map_data.items():
             type = data.get("type", None)
+            if ScenarioDescription.POLYLINE in data and len(data[ScenarioDescription.POLYLINE]) <= 1:
+                continue
             if MetaDriveType.is_road_line(type):
-                if len(data[ScenarioDescription.POLYLINE]) <= 1:
-                    continue
                 if MetaDriveType.is_broken_line(type):
                     self.construct_broken_line(
                         np.asarray(data[ScenarioDescription.POLYLINE]),

metadrive/component/static_object/traffic_object.py

@@ -100,7 +100,7 @@ def __init__(self, position, heading_theta, lane=None, static: bool = False, ran
             model = self.loader.loadModel(AssetLoader.file_path("models", "warning", "warning.gltf"))
             model.setScale(0.02)
             model.setH(-90)
-            model.setPos(0, 0, -self.HEIGHT / 2)
+            model.setPos(0, 0, -self.HEIGHT / 2 - 0.1)
             model.reparentTo(self.origin)
 
     @property

metadrive/component/vehicle_module/depth_camera.py

@@ -28,10 +28,11 @@ def __init__(self):
         cam = self.get_cam()
         lens = self.get_lens()
 
-        cam.lookAt(0, 2.4, 1.3)
+        # cam.lookAt(0, 2.4, 1.3)
+        cam.lookAt(0, 10.4, 1.6)
 
         lens.setFov(60)
-        lens.setAspectRatio(2.0)
+        # lens.setAspectRatio(2.0)
         if get_engine().mode == RENDER_MODE_NONE or not AssetLoader.initialized() or type(self)._singleton.init_num > 1:
             return
         # add shader for it

metadrive/component/vehicle_module/rgb_camera.py

@@ -19,7 +19,8 @@ def __init__(self):
         super(RGBCamera, self).__init__(True, cuda)
         cam = self.get_cam()
         lens = self.get_lens()
-        cam.lookAt(0, 2.4, 1.3)
+        # cam.lookAt(0, 2.4, 1.3)
+        cam.lookAt(0, 10.4, 1.6)
 
         lens.setFov(60)
-        lens.setAspectRatio(2.0)
+        # lens.setAspectRatio(2.0)

metadrive/engine/core/engine_core.py

@@ -133,6 +133,8 @@ def __init__(self, global_config):
         if self.use_render_pipeline:
             self.render_pipeline = RenderPipeline()
             self.render_pipeline.pre_showbase_init()
+            # disable it, as some model errors happen!
+            loadPrcFileData("", "gl-immutable-texture-storage 0")
         else:
             self.render_pipeline = None
 

metadrive/engine/core/main_camera.py

@@ -5,7 +5,7 @@
 
 import numpy as np
 from direct.controls.InputState import InputState
-from panda3d.core import Vec3, Point3, PNMImage, NodePath
+from panda3d.core import Vec3, Point3, PNMImage
 from panda3d.core import WindowProperties
 
 from metadrive.constants import CollisionGroup
@@ -227,9 +227,12 @@ def _chase_task(self, vehicle, task):
 
         if self.world_light is not None:
             self.world_light.step(current_pos)
-        # self.camera.reparentTo(vehicle.origin)
-        # self.camera.setPost(0., 0.8, 1.5)
-        # self.camera.lookAt(0, 2.4, 1.3)
+
+        # Don't use reparentTo()
+        # pos = vehicle.convert_to_world_coordinates([0.8, 0.], vehicle.position)
+        # look_at = vehicle.convert_to_world_coordinates([10.4, 0.], vehicle.position)
+        # self.camera.setPos(pos[0], pos[1], 1.5 + vehicle.origin.getZ())
+        # self.camera.lookAt(look_at[0], look_at[1], 1.6 + vehicle.origin.getZ())
         return task.cont
 
     @staticmethod

metadrive/engine/core/terrain.py

@@ -69,6 +69,11 @@ def _load_mesh_terrain_textures(self, anisotropic_degree=16, minfilter=SamplerSt
         self.ts_normal = TextureStage("normal")
         self.ts_normal.setMode(TextureStage.M_normal)
 
+        # basic height_field
+        heightfield_tex = self.loader.loadTexture(AssetLoader.file_path("textures", "terrain", "heightfield.png"))
+        heightfield_img = np.frombuffer(heightfield_tex.getRamImage().getData(), dtype=np.uint16)
+        self.heightfield_img = heightfield_img.reshape((heightfield_tex.getYSize(), heightfield_tex.getXSize(), 1))
+
         # grass
         self.grass_tex = self.loader.loadTexture(
             AssetLoader.file_path("textures", "grass2", "grass_path_2_diff_1k.png")
@@ -264,14 +269,11 @@ def reset(self, center_position):
             semantic_tex.setRamImage(semantics)
 
             # we will downsmaple the precision after this
-            heightfield_tex = self.loader.loadTexture(AssetLoader.file_path("textures", "terrain", "heightfield.png"))
-            heightfield_img = np.frombuffer(heightfield_tex.getRamImage().getData(), dtype=np.uint16)
-            heightfield_img = heightfield_img.reshape((heightfield_tex.getYSize(), heightfield_tex.getXSize(), 1))
             drivable_region = self.engine.current_map.get_height_map(
                 self._terrain_size, self._downsample_rate, self._drivable_region_extension
             )
-            drivable_region_height = np.mean(heightfield_img[np.where(drivable_region)]).astype(np.uint16)
-            heightfield_img = np.where(drivable_region, drivable_region_height, heightfield_img)
+            drivable_region_height = np.mean(self.heightfield_img[np.where(drivable_region)]).astype(np.uint16)
+            heightfield_img = np.where(drivable_region, drivable_region_height, self.heightfield_img)
 
             # set to zero height
             heightfield_img -= drivable_region_height

metadrive/envs/base_env.py

@@ -117,7 +117,7 @@
         show_lidar=False,
         mini_map=(84, 84, 250),  # buffer length, width
         rgb_camera=(84, 84),  # buffer length, width
-        depth_camera=(84, 84, False),  # buffer length, width, view_ground
+        depth_camera=(84, 84, True),  # buffer length, width, view_ground
         main_camera=None,  # buffer length, width
         show_side_detector=False,
         show_lane_line_detector=False,
@@ -211,7 +211,8 @@ def default_config(cls) -> Config:
         return Config(BASE_DEFAULT_CONFIG)
 
     # ===== Intialization =====
-    def __init__(self, config: Union[dict, None] = None):
+    def __init__(self, config: dict = None):
+        self.logger = logging.getLogger(self.logger_name)
         if config is None:
             config = {}
         merged_config = self._merge_extra_config(config)
@@ -220,7 +221,6 @@ def __init__(self, config: Union[dict, None] = None):
 
         self.config = global_config
         initialize_global_config(self.config)
-        self.logger = logging.getLogger(self.logger_name)
 
         # agent check
         self.num_agents = self.config["num_agents"]