added universal checker

Author: gerni17

elevation_mapping_cupy/config/plugin_config.yaml

@@ -52,6 +52,16 @@ semantic_filter:
   is_height_layer: False
   layer_name: "sem_fil"
   extra_params:
-#    TODO make a better link between classes and color
     classes: ['grass','tree','fence','person']
-    colors: [[0,255,0],[120,120,0],[170,0,20],[0,0,255],[255,0,0]]
+    colors: [[0,255,0],[120,120,0],[170,0,20],[0,0,255],[255,0,0]]
+
+semantic_traversability:
+  type: "semantic_traversability"
+  enable: False
+  fill_nan: False
+  is_height_layer: False
+  layer_name: "sem_traversability"
+  extra_params:
+    layers: ['traversability','robot_centric_elevation']
+    thresholds: [0.7,0.5]
+    type: ['traversability', 'elevation']

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -595,6 +595,22 @@ def get_normal_ref(self, normal_x_data, normal_y_data, normal_z_data):
         normal_y_data[...] = xp.asnumpy(maps[1], stream=self.stream)
         normal_z_data[...] = xp.asnumpy(maps[2], stream=self.stream)
 
+    def get_layer(self,name):
+        if name in self.layer_names:
+            idx = self.layer_names.index(name)
+            map = self.elevation_map[idx]
+        elif name in self.semantic_map.param.additional_layers:
+            idx = self.semantic_map.param.additional_layers.index(name)
+            map= self.semantic_map.map[idx]
+        elif name in self.plugin_manager.layer_names:
+            self.plugin_manager.update_with_name(name, self.elevation_map, self.layer_names, self.semantic_map,
+                                                 self.base_rotation)
+            map = self.plugin_manager.get_map_with_name(name)
+        else:
+            print("Layer {} is not in the map, returning traversabiltiy!".format(name))
+            return
+        return map
+
     def get_polygon_traversability(self, polygon, result):
         """ Check if input polygons are traversable.
 
@@ -626,11 +642,12 @@ def get_polygon_traversability(self, polygon, result):
             self.mask,
             size=(self.cell_n * self.cell_n),
         )
-        masked, masked_isvalid = get_masked_traversability(self.elevation_map, self.mask)
+        map = self.get_layer(self.param.checker_layer)
+        masked, masked_isvalid = get_masked_traversability(self.elevation_map, self.mask, map)
         if masked_isvalid.sum() > 0:
             t = masked.sum() / masked_isvalid.sum()
         else:
-            t = cp.asarray(0.0,dtype=self.data_type)
+            t = cp.asarray(0.0, dtype=self.data_type)
         is_safe, un_polygon = is_traversable(
             masked, self.param.safe_thresh, self.param.safe_min_thresh, self.param.max_unsafe_n
         )
@@ -697,14 +714,14 @@ def initialize_map(self, points, method="cubic"):
     layers = ["elevation", "variance", "traversability", "min_filter", "smooth", "inpaint","rgb"]
     points = xp.random.rand(100000, len(layers))
 
-    channels = ['x','y','z']+ param.additional_layers
+    channels = ['x', 'y', 'z'] + param.additional_layers
     print(channels)
     data = np.zeros((elevation.cell_n - 2, elevation.cell_n - 2), dtype=np.float32)
     for i in range(20):
         elevation.input(points,channels, R, t, 0, 0)
         elevation.update_normal(elevation.elevation_map[0])
         pos = np.array([i * 0.01, i * 0.02, i * 0.01])
-        elevation.move_to(pos,R)
+        elevation.move_to(pos, R)
         for layer in layers:
             elevation.get_map_with_name_ref(layer, data)
         print(i)

elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py

@@ -10,13 +10,13 @@
 
 @dataclass
 class Parameter(Serializable):
-    resolution: float = 0.02
+    resolution: float = 0.04
     additional_layers: list = ['feat_0'].copy
     fusion_algorithms: list = ['average'].copy
     cell_n: int = None
     data_type: str = np.float32
 
-    map_length: float = 10.0
+    map_length: float = 8.0
     sensor_noise_factor: float = 0.05
     mahalanobis_thresh: float = 2.0
     outlier_variance: float = 0.01
@@ -45,6 +45,7 @@ class Parameter(Serializable):
     safe_thresh: float = 0.5
     safe_min_thresh: float = 0.5
     max_unsafe_n: int = 20
+    checker_layer: str = 'traversability'
 
     min_filter_size: int = 5
     min_filter_iteration: int = 3

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_traversability.py

@@ -0,0 +1,71 @@
+#
+# 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
+import numpy as np
+from typing import List
+
+from elevation_mapping_cupy.plugins.plugin_manager import PluginBase
+
+
+class SemanticTraversability(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,
+        layers: list = ["traversability"],
+        thresholds: list = [0.5],
+        type: list = ["traversability"],
+        **kwargs,
+    ):
+        super().__init__()
+        self.layers = layers
+        self.thresholds = cp.asarray(thresholds)
+        self.type = type
+
+    def __call__(
+        self,
+        elevation_map: cp.ndarray,
+        layer_names: List[str],
+        plugin_layers: cp.ndarray,
+        plugin_layer_names: List[str],
+        semantic_map,
+        *args,
+    ) -> cp.ndarray:
+        # get indices of all layers that
+        map = cp.zeros(elevation_map[2].shape, np.float32)
+        tempo = cp.zeros(elevation_map[2].shape, np.float32)
+        for it, name in enumerate(self.layers):
+            if name in layer_names:
+                idx = layer_names.index(name)
+                tempo = elevation_map[idx]
+            elif name in semantic_map.param.additional_layers:
+                idx = semantic_map.param.additional_layers.index(name)
+                tempo = semantic_map.map[idx]
+            elif name in plugin_layer_names:
+                idx = plugin_layer_names.index(name)
+                tempo = plugin_layers[idx]
+            else:
+                print(
+                    "Layer {} is not in the map, returning traversabiltiy!".format(name)
+                )
+                return
+            if self.type[it] == "traversability":
+                tempo = cp.where(tempo <= self.thresholds[it], 1, 0)
+                map += tempo
+            else:
+                tempo = cp.where(tempo >= self.thresholds[it], 1, 0)
+                map += tempo
+        map = cp.where(map <= 0.9, 0.1,1)
+
+        return map

elevation_mapping_cupy/script/elevation_mapping_cupy/tests/plugin_config.yaml

@@ -0,0 +1,66 @@
+# Settings of the plugins. (The plugins should be stored in script/plugins)
+
+# min_filter fills in minimum value around the invalid cell.
+min_filter:                                   
+  enable: True                                # weather to load this plugin
+  fill_nan: False                             # Fill nans to invalid cells of elevation layer.
+  is_height_layer: True                       # If this is a height layer (such as elevation) or not (such as traversability)
+  layer_name: "min_filter"                    # The layer name.
+  extra_params:                               # This params are passed to the plugin class on initialization.
+    dilation_size: 1                         # The patch size to apply
+    iteration_n: 30                           # The number of iterations
+# Apply smoothing.
+smooth_filter:                                
+  enable: True
+  fill_nan: False
+  is_height_layer: True
+  layer_name: "smooth"
+  extra_params:
+    input_layer_name: "min_filter"
+# Apply inpainting using opencv
+inpainting:
+  enable: True
+  fill_nan: False
+  is_height_layer: True
+  layer_name: "inpaint"
+  extra_params:
+    method: "telea"                           # telea or ns
+# Apply smoothing for inpainted layer
+smooth_filter_1:
+  type: "smooth_filter"
+  enable: True
+  fill_nan: False
+  is_height_layer: True
+  layer_name: "smooth_1"
+  extra_params:
+    input_layer_name: "inpaint"
+robot_centric_elevation:                                  # Use the same name as your file name.
+#  type: "robot_centric_elevation"
+  enable: True                                # weather to load this plugin
+  fill_nan: False                              # Fill nans to invalid cells of elevation layer.
+  is_height_layer: True                       # If this is a height layer (such as elevation) or not (such as traversability)
+  layer_name: "robot_centric_elevation"                 # The layer name.
+  extra_params:                               # This params are passed to the plugin class on initialization.
+#    add_value: 2.0                            # Example param
+    resolution: 0.04
+    threshold: 1.1
+    use_threshold: True
+semantic_filter:
+  type: "semantic_filter"
+  enable: True
+  fill_nan: False
+  is_height_layer: False
+  layer_name: "sem_fil"
+  extra_params:
+    classes: ['grass','tree','fence','person']
+    colors: [[0,255,0],[120,120,0],[170,0,20],[0,0,255],[255,0,0]]
+semantic_traversability:
+  type: "semantic_traversability"
+  enable: True
+  fill_nan: False
+  is_height_layer: False
+  layer_name: "sem_traversability"
+  extra_params:
+    layers: ['traversability','robot_centric_elevation']
+    thresholds: [0.7,0.5]
+    type: ['traversability', 'elevation']

elevation_mapping_cupy/script/elevation_mapping_cupy/tests/test_elevation_mapping.py

@@ -94,6 +94,6 @@ def test_initialize_map(self, elmap_ex):
 
     def test_plugins(self, elmap_ex):
         layers = elmap_ex.plugin_manager.layer_names
-        data= np.zeros((500,500),dtype=np.float32)
+        data = np.zeros((200, 200), dtype=np.float32)
         for layer in layers:
-            elmap_ex.get_map_with_name_ref(layer, data)
+            elmap_ex.get_map_with_name_ref(layer, data)

elevation_mapping_cupy/script/elevation_mapping_cupy/tests/test_plugins.py

@@ -4,7 +4,7 @@
 import numpy as np
 from elevation_mapping_cupy.plugins.plugin_manager import PluginManager, PluginParams
 
-plugin_path = "../../../config/plugin_config.yaml"
+plugin_path = "plugin_config.yaml"
 
 
 @pytest.fixture()
@@ -28,19 +28,19 @@ def semmap_ex(add_lay, fusion_alg):
 @pytest.mark.parametrize(
     "add_lay, fusion_alg,channels",
     [
-        (["feat_0", "feat_1"], ["average", "average"], ["feat_0"]),
-        (["feat_0", "feat_1"], ["average", "average"], []),
-        (
-            ["feat_0", "feat_1", "rgb"],
-            ["average", "average", "color"],
-            ["rgb", "feat_0"],
-        ),
+        (['grass','tree','fence','person'], ["class_average", "class_average", "class_average", "class_average"], ["grass"]),
+        (['grass','tree'], ["class_average", "class_average"], ['grass']),
+        # (
+        #     ["feat_0", "feat_1", "rgb"],
+        #     ["average", "average", "color"],
+        #     ["rgb", "feat_0"],
+        # ),
     ],
 )
 def test_plugin_manager(semmap_ex, channels):
-    manager = PluginManager(200)
+    manager = PluginManager(202)
     manager.load_plugin_settings(plugin_path)
-    elevation_map = cp.zeros((7, 200, 200)).astype(cp.float32)
+    elevation_map = cp.zeros((7, 202, 202)).astype(cp.float32)
     rotation = cp.eye(3,dtype=cp.float32)
     layer_names = [
         "elevation",
@@ -51,13 +51,14 @@ def test_plugin_manager(semmap_ex, channels):
         "upper_bound",
         "is_upper_bound",
     ]
-    elevation_map[0] = cp.random.randn(200, 200)
-    elevation_map[2] = cp.abs(cp.random.randn(200, 200))
+    elevation_map[0] = cp.random.randn(202, 202)
+    elevation_map[2] = cp.abs(cp.random.randn(202, 202))
     elevation_map[0]
     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("semantic_filter", elevation_map, layer_names,semmap_ex,rotation)
+    manager.update_with_name("semantic_traversability", elevation_map, layer_names,semmap_ex)
     manager.get_map_with_name("smooth")
     for lay in manager.get_layer_names():
         manager.update_with_name(lay,elevation_map,layer_names,semmap_ex,rotation)

elevation_mapping_cupy/script/elevation_mapping_cupy/traversability_polygon.py

@@ -7,8 +7,8 @@
 from shapely.geometry import Polygon, MultiPoint
 
 
-def get_masked_traversability(map_array, mask):
-    traversability = map_array[3][1:-1, 1:-1]
+def get_masked_traversability(map_array, mask, traversability):
+    traversability = traversability[1:-1, 1:-1]
     is_valid = map_array[2][1:-1, 1:-1]
     mask = mask[1:-1, 1:-1]