first

Author: gerni17

elevation_mapping_cupy/config/plugin_config.yaml

@@ -26,7 +26,7 @@ inpainting:
   extra_params:
     method: "telea"                           # telea or ns
 # Apply smoothing for inpainted layer
-smooth_filter_1:                                
+smooth_filter_1:
   type: "smooth_filter"
   enable: True
   fill_nan: False

elevation_mapping_cupy/config/sensor_parameter.yaml

@@ -4,10 +4,10 @@ subscribers:
 #    fusion: ['average','average']
 #    topic_name: '/elevation_mapping/pointcloud_semantic'
   front_cam:
-    channels: ['rgb','feat_0','feat_1','person','grass']
-    fusion: ['color','average','average','class_average']
+    channels: []
+    fusion: []
     topic_name: '/elvation_mapping/pointcloud_semantic'
-    semantic_segmentation: True
+    semantic_segmentation: False
     segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x'
     feature_config:
       name: 'DINO'
@@ -26,6 +26,7 @@ subscribers:
     cam_frame: "zed2i_right_camera_optical_frame"
     confidence_topic: "/zed2i/zed_node/confidence/confidence_map"
     confidence_threshold: 10
+    feature_extractor: False
 
 
 

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/inpainting.py

@@ -38,6 +38,7 @@ def __call__(
         layer_names: List[str],
         plugin_layers: cp.ndarray,
         plugin_layer_names: List[str],
+        **kwargs,
     ) -> cp.ndarray:
         mask = cp.asnumpy((elevation_map[2] < 0.5).astype("uint8"))
         if (mask < 1).any():

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/min_filter.py

@@ -94,6 +94,7 @@ def __call__(
         layer_names: List[str],
         plugin_layers: cp.ndarray,
         plugin_layer_names: List[str],
+        **kwargs,
     ) -> cp.ndarray:
         self.min_filtered = elevation_map[0].copy()
         self.min_filtered_mask = elevation_map[2].copy()

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py

@@ -9,6 +9,7 @@
 import inspect
 from dataclasses import dataclass
 from ruamel.yaml import YAML
+from inspect import signature
 
 
 @dataclass
@@ -38,6 +39,7 @@ def __call__(
         layer_names: List[str],
         plugin_layers: cp.ndarray,
         plugin_layer_names: List[str],
+        **kwargs,
     ) -> cp.ndarray:
         """This gets the elevation map data and plugin layers as a cupy array.
 
@@ -75,9 +77,15 @@ def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]):
 
         self.plugins = []
         for param, extra_param in zip(plugin_params, extra_params):
-            m = importlib.import_module("." + param.name, package="elevation_mapping_cupy.plugins")  # -> 'module'
+            m = importlib.import_module(
+                "." + param.name, package="elevation_mapping_cupy.plugins"
+            )  # -> 'module'
             for name, obj in inspect.getmembers(m):
-                if inspect.isclass(obj) and issubclass(obj, PluginBase) and name != "PluginBase":
+                if (
+                    inspect.isclass(obj)
+                    # and issubclass(obj, PluginBase)
+                    and name != "PluginBase"
+                ):
                     # Add cell_n to params
                     extra_param["cell_n"] = self.cell_n
                     self.plugins.append(obj(**extra_param))
@@ -102,6 +110,7 @@ def load_plugin_settings(self, file_path: str):
                     )
                 )
             extra_params.append(v["extra_params"])
+        print(plugin_params)
         self.init(plugin_params, extra_params)
         print("Loaded plugins are ", *self.plugin_names)
 
@@ -133,10 +142,38 @@ def get_layer_index_with_name(self, name: str) -> int:
             print("Error with layer {}: {}".format(name, e))
             return None
 
-    def update_with_name(self, name: str, elevation_map: cp.ndarray, layer_names: List[str]):
+    def update_with_name(
+        self,
+        name: str,
+        elevation_map: cp.ndarray,
+        layer_names: List[str],
+        semantic_map=None,
+        transform=None,
+    ):
         idx = self.get_layer_index_with_name(name)
         if idx is not None:
-            self.layers[idx] = self.plugins[idx](elevation_map, layer_names, self.layers, self.layer_names)
+            n_param = len(signature(self.plugins[idx]).parameters)
+            if n_param == 5:
+                self.layers[idx] = self.plugins[idx](
+                    elevation_map, layer_names, self.layers, self.layer_names
+                )
+            elif n_param == 6:
+                self.layers[idx] = self.plugins[idx](
+                    elevation_map,
+                    layer_names,
+                    self.layers,
+                    self.layer_names,
+                    semantic_map,
+                )
+            else:
+                self.layers[idx] = self.plugins[idx](
+                    elevation_map,
+                    layer_names,
+                    self.layers,
+                    self.layer_names,
+                    semantic_map,
+                    transform,
+                )
 
     def get_map_with_name(self, name: str) -> cp.ndarray:
         idx = self.get_layer_index_with_name(name)
@@ -154,17 +191,28 @@ def get_param_with_name(self, name: str) -> PluginParams:
         PluginParams(name="min_filter", layer_name="min_filter"),
         PluginParams(name="smooth_filter", layer_name="smooth"),
     ]
-    extra_params = [{"dilation_size": 5, "iteration_n": 5}, {"input_layer_name": "elevation2"}]
+    extra_params = [
+        {"dilation_size": 5, "iteration_n": 5},
+        {"input_layer_name": "elevation2"},
+    ]
     manager = PluginManager(200)
-    manager.load_plugin_settings("config/plugin_config.yaml")
+    manager.load_plugin_settings("../config/plugin_config.yaml")
     print(manager.layer_names)
     print(manager.plugin_names)
     elevation_map = cp.zeros((7, 200, 200)).astype(cp.float32)
-    layer_names = ["elevation", "variance", "is_valid", "traversability", "time", "upper_bound", "is_upper_bound"]
+    layer_names = [
+        "elevation",
+        "variance",
+        "is_valid",
+        "traversability",
+        "time",
+        "upper_bound",
+        "is_upper_bound",
+    ]
     elevation_map[0] = cp.random.randn(200, 200)
     elevation_map[2] = cp.abs(cp.random.randn(200, 200))
     print("map", elevation_map[0])
     print("layer map ", manager.layers[0])
     manager.update_with_name("min_filter", elevation_map, layer_names)
-    manager.update_with_name("smooth_filter", elevation_map, layer_names)
+    manager.update_with_name("smooth", elevation_map, layer_names)
     print(manager.get_map_with_name("smooth"))

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_map.py

@@ -0,0 +1,53 @@
+#
+# Copyright (c) 2022, Takahiro Miki. All rights reserved.
+# Licensed under the MIT license. See LICENSE file in the project root for details.
+#
+import cupy as cp
+from typing import List
+import cupyx.scipy.ndimage as ndimage
+
+from .plugin_manager import PluginBase
+from elevation_mapping_cupy.semantic_map import SemanticMap
+
+class SemanticFilter(PluginBase):
+    """This is a filter to create colors
+
+    ...
+
+    Attributes
+    ----------
+    cell_n: int
+        width and height of the elevation map.
+    """
+
+    def __init__(self, cell_n: int = 100, input_layer_name: str = "elevation", **kwargs):
+        super().__init__()
+        self.input_layer_name = input_layer_name
+        self.indices = []
+
+    def __call__(
+        self,
+        elevation_map: cp.ndarray,
+        layer_names: List[str],
+        plugin_layers: cp.ndarray,
+        plugin_layer_names: List[str],
+        semantic_map: SemanticMap,
+        **kwargs,
+    ) -> cp.ndarray:
+        # get indices of all layers that
+        semantic_map.param.additional_layers
+         # check which has the highest value
+        # create color coding
+        ##################
+        if self.input_layer_name in layer_names:
+            idx = layer_names.index(self.input_layer_name)
+            h = elevation_map[idx]
+        elif self.input_layer_name in plugin_layer_names:
+            idx = plugin_layer_names.index(self.input_layer_name)
+            h = plugin_layers[idx]
+        else:
+            print("layer name {} was not found. Using elevation layer.".format(self.input_layer_name))
+            h = elevation_map[0]
+        hs1 = ndimage.uniform_filter(h, size=3)
+        hs1 = ndimage.uniform_filter(hs1, size=3)
+        return hs1

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/smooth_filter.py

@@ -30,6 +30,7 @@ def __call__(
         layer_names: List[str],
         plugin_layers: cp.ndarray,
         plugin_layer_names: List[str],
+        **kwargs,
     ) -> cp.ndarray:
         if self.input_layer_name in layer_names:
             idx = layer_names.index(self.input_layer_name)