changed to float32, added the color plugin for the classes and added first plugin tests

Author: gerni17

elevation_mapping_cupy/config/parameters.yaml

@@ -81,7 +81,7 @@ publishers:
     basic_layers: ['elevation', 'traversability']
     fps: 2.0
   elevation_map_filter:
-    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb','person']
+    layers: ['min_filter', 'smooth', 'inpaint', 'upper_bound','rgb','sem_fil']
     basic_layers: ['min_filter']
     fps: 3.0
 

elevation_mapping_cupy/config/plugin_config.yaml

@@ -18,7 +18,7 @@ smooth_filter:
   extra_params:
     input_layer_name: "min_filter"
 # Apply inpainting using opencv
-inpainting:                                   
+inpainting:
   enable: True
   fill_nan: False
   is_height_layer: True
@@ -34,3 +34,14 @@ smooth_filter_1:
   layer_name: "smooth_1"
   extra_params:
     input_layer_name: "inpaint"
+
+semantic_filter:
+  type: "semantic_filter"
+  enable: False
+  fill_nan: False
+  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]]

elevation_mapping_cupy/config/sensor_parameter.yaml

@@ -4,8 +4,8 @@ subscribers:
 #    fusion: ['average','average']
 #    topic_name: '/elevation_mapping/pointcloud_semantic'
   front_cam:
-    channels: []
-    fusion: []
+    channels: ['rgb','feat_0','feat_1','grass','tree','fence','person']
+    fusion: ['color','average','average','class_average','class_average','class_average','class_average']
     topic_name: '/elvation_mapping/pointcloud_semantic'
     semantic_segmentation: False
     segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x'

elevation_mapping_cupy/rviz/lonomy_single.rviz

@@ -55,8 +55,8 @@ Visualization Manager:
       Autocompute Intensity Bounds: true
       Class: grid_map_rviz_plugin/GridMap
       Color: 200; 200; 200
-      Color Layer: person
-      Color Transformer: IntensityLayer
+      Color Layer: rgb
+      Color Transformer: ColorLayer
       Enabled: true
       Height Layer: smooth
       Height Transformer: GridMapLayer
@@ -449,9 +449,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5547983050346375
+      Pitch: 0.6447982788085938
       Target Frame: base_footprint
-      Yaw: 1.4138503074645996
+      Yaw: 1.8688498735427856
     Saved: ~
 Window Geometry:
   Displays:

elevation_mapping_cupy/script/elevation_mapping_cupy/custom_kernels.py

@@ -665,7 +665,7 @@ def sum_kernel(
     # input the list of layers, amount of channels can slo be input through kernel
     sum_kernel = cp.ElementwiseKernel(
         in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels",
-        out_params="raw U map, raw T newmap",
+        out_params="raw U map, raw U newmap",
         preamble=string.Template(
             """
                 __device__ int get_map_idx(int idx, int layer_n) {
@@ -681,7 +681,7 @@ def sum_kernel(
             U inside = p[i * pcl_channels[0] + 2];
             if (valid) {
                 if (inside) {
-                    for ( int it=0;it<pcl_channels[1];it++){
+                    for ( W it=0;it<pcl_channels[1];it++){
                         U feat = p[i * pcl_channels[0] + pcl_chan[it]];
                         atomicAdd(&newmap[get_map_idx(idx, map_lay[it])], feat);
                     }
@@ -864,7 +864,8 @@ def color_average_kernel(
                     //    unsigned int b = prev_b/2 + color_map[get_map_idx(i, it*3+2)]/(2*cnt);
                     //}
                     unsigned int rgb = (r<<16) + (g << 8) + b;
-                    map[get_map_idx(i,  map_lay[it])] = rgb;
+                    float rgb_ = __uint_as_float(rgb);
+                    map[get_map_idx(i,  map_lay[it])] = rgb_;
                 }
             }
             """

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -36,7 +36,6 @@
 pool = cp.cuda.MemoryPool(cp.cuda.malloc_managed)
 cp.cuda.set_allocator(pool.malloc)
 
-
 class ElevationMap:
     """
     Core elevation mapping class.
@@ -49,16 +48,16 @@ def __init__(self, param: Parameter):
             param (elevation_mapping_cupy.parameter.Parameter):
         """
         self.param = param
-
+        self.data_type = self.param.data_type
         self.resolution = param.resolution
-        self.center = xp.array([0, 0, 0], dtype=float)
+        self.center = xp.array([0, 0, 0], dtype=self.data_type)
         self.map_length = param.map_length
         self.cell_n = param.cell_n
 
         self.map_lock = threading.Lock()
         self.additional_layers = dict(zip(self.param.additional_layers, self.param.fusion_algorithms))
         self.semantic_map = SemanticMap(self.param, self.additional_layers)
-        self.elevation_map = xp.zeros((7, self.cell_n, self.cell_n))
+        self.elevation_map = xp.zeros((7, self.cell_n, self.cell_n),dtype=self.data_type)
         self.layer_names = [
             "elevation",
             "variance",
@@ -71,7 +70,7 @@ def __init__(self, param: Parameter):
 
         # buffers
         self.traversability_buffer = xp.full((self.cell_n, self.cell_n), xp.nan)
-        self.normal_map = xp.zeros((3, self.cell_n, self.cell_n))
+        self.normal_map = xp.zeros((3, self.cell_n, self.cell_n),dtype=self.data_type)
         # Initial variance
         self.initial_variance = param.initial_variance
         self.elevation_map[1] += self.initial_variance
@@ -223,12 +222,12 @@ def compile_kernels(self):
 
         """
         # Compile custom cuda kernels.
-        self.new_map = cp.zeros((self.elevation_map.shape[0], self.cell_n, self.cell_n))
-        self.traversability_input = cp.zeros((self.cell_n, self.cell_n))
-        self.traversability_mask_dummy = cp.zeros((self.cell_n, self.cell_n))
-        self.min_filtered = cp.zeros((self.cell_n, self.cell_n))
-        self.min_filtered_mask = cp.zeros((self.cell_n, self.cell_n))
-        self.mask = cp.zeros((self.cell_n, self.cell_n))
+        self.new_map = cp.zeros((self.elevation_map.shape[0], self.cell_n, self.cell_n),dtype=self.data_type)
+        self.traversability_input = cp.zeros((self.cell_n, self.cell_n),dtype=self.data_type)
+        self.traversability_mask_dummy = cp.zeros((self.cell_n, self.cell_n),dtype=self.data_type)
+        self.min_filtered = cp.zeros((self.cell_n, self.cell_n),dtype=self.data_type)
+        self.min_filtered_mask = cp.zeros((self.cell_n, self.cell_n),dtype=self.data_type)
+        self.mask = cp.zeros((self.cell_n, self.cell_n),dtype=self.data_type)
         self.add_points_kernel = add_points_kernel(
             self.resolution,
             self.cell_n,
@@ -302,8 +301,8 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
             self.error_counting_kernel(
                 self.elevation_map,
                 points,
-                cp.array([0.0]),
-                cp.array([0.0]),
+                cp.array([0.0],dtype=self.data_type),
+                cp.array([0.0],dtype=self.data_type),
                 R,
                 t,
                 self.new_map,
@@ -324,8 +323,8 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
                 if np.abs(self.mean_error) < self.param.max_drift:
                     self.elevation_map[0] += self.mean_error * self.param.drift_compensation_alpha
             self.add_points_kernel(
-                cp.array([0.0]),
-                cp.array([0.0]),
+                cp.array([0.0],dtype=self.data_type),
+                cp.array([0.0],dtype=self.data_type),
                 R,
                 t,
                 self.normal_map,
@@ -337,7 +336,7 @@ def update_map_with_kernel(self, points_all, channels, R, t, position_noise, ori
             self.average_map_kernel(self.new_map, self.elevation_map, size=(self.cell_n * self.cell_n))
 
             # self.update_additional_layers(additional_fusion, points_all, channels, R, t)
-            self.semantic_map.update_layers(points_all, channels, R, t,self.new_map)
+            self.semantic_map.update_layers(points_all, channels, R, t, self.new_map)
 
             if self.param.enable_overlap_clearance:
                 self.clear_overlap_map(t)
@@ -405,10 +404,10 @@ def input(self, raw_points: cp._core.core.ndarray, channels: List[str], R: cp._c
             None: 
         """
         # Update elevation map using point cloud input.
-        raw_points = cp.asarray(raw_points)
+        raw_points = cp.asarray(raw_points, dtype=self.data_type)
         additional_channels = channels[3:]
         raw_points = raw_points[~cp.isnan(raw_points).any(axis=1)]
-        self.update_map_with_kernel(raw_points, additional_channels, cp.asarray(R), cp.asarray(t), position_noise, orientation_noise)
+        self.update_map_with_kernel(raw_points, additional_channels, cp.asarray(R,dtype=self.data_type), cp.asarray(t,dtype=self.data_type), position_noise, orientation_noise)
 
     def update_normal(self, dilated_map):
         """ Clear the normal map and then apply the normal kernel with dilated map as input.
@@ -561,7 +560,7 @@ def get_map_with_name_ref(self, name, data):
             elif name in self.additional_layers.keys():
                 m = self.semantic_map.get_map_with_name(name)
             elif name in self.plugin_manager.layer_names:
-                self.plugin_manager.update_with_name(name, self.elevation_map, self.layer_names)
+                self.plugin_manager.update_with_name(name, self.elevation_map, self.layer_names, self.semantic_map)
                 m = self.plugin_manager.get_map_with_name(name)
                 p = self.plugin_manager.get_param_with_name(name)
                 xp = self.xp_of_array(m)
@@ -607,7 +606,7 @@ def get_polygon_traversability(self, polygon, result):
         """
         polygon = xp.asarray(polygon)
         area = calculate_area(polygon)
-        polygon = polygon.astype(np.float64)
+        polygon = polygon.astype(self.data_type)
         pmin = self.center[:2] - self.map_length / 2 + self.resolution
         pmax = self.center[:2] + self.map_length / 2 - self.resolution
         polygon[:, 0] = polygon[:, 0].clip(pmin[0], pmax[0])
@@ -630,7 +629,7 @@ def get_polygon_traversability(self, polygon, result):
         if masked_isvalid.sum() > 0:
             t = masked.sum() / masked_isvalid.sum()
         else:
-            t = cp.asarray(0.0)
+            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
         )
@@ -662,7 +661,7 @@ def initialize_map(self, points, method="cubic"):
         """
         self.clear()
         with self.map_lock:
-            points = cp.asarray(points)
+            points = cp.asarray(points,dtype=self.data_type)
             indices = transform_to_map_index(points[:, :2], self.center[:2], self.cell_n, self.resolution)
             points[:, :2] = indices.astype(points.dtype)
             points[:, 2] -= self.center[2]
@@ -708,7 +707,7 @@ def initialize_map(self, points, method="cubic"):
         for layer in layers:
             elevation.get_map_with_name_ref(layer, data)
         print(i)
-        polygon = cp.array([[0, 0], [2, 0], [0, 2]], dtype=np.float64)
+        polygon = cp.array([[0, 0], [2, 0], [0, 2]], dtype=param.data_type)
         result = np.array([0, 0, 0])
         elevation.get_polygon_traversability(polygon, result)
         print(result)

elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py

@@ -14,6 +14,7 @@ class Parameter(Serializable):
     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
     sensor_noise_factor: float = 0.05

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py

@@ -89,8 +89,9 @@ def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]):
                     # Add cell_n to params
                     extra_param["cell_n"] = self.cell_n
                     self.plugins.append(obj(**extra_param))
-
-        self.layers = cp.zeros((len(self.plugins), self.cell_n, self.cell_n))
+        self.layers = cp.zeros(
+            (len(self.plugins), self.cell_n, self.cell_n), dtype=cp.float32
+        )
         self.layer_names = self.get_layer_names()
         self.plugin_names = self.get_plugin_names()
 
@@ -212,7 +213,8 @@ def get_param_with_name(self, name: str) -> PluginParams:
     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])
+    print("layer map ", manager.layers)
     manager.update_with_name("min_filter", elevation_map, layer_names)
-    manager.update_with_name("smooth", elevation_map, layer_names)
+    manager.update_with_name("smooth_filter", elevation_map, layer_names)
+    # manager.update_with_name("sem_fil", elevation_map, layer_names, semantic_map=semantic_map)
     print(manager.get_map_with_name("smooth"))

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_filter.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 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,
+        classes: list = ["person", "grass"],
+        colors: list = [
+            [0, 255, 0],
+            [0, 255, 255],
+            [0, 0, 255],
+            [255, 255, 0],
+            [255, 0, 0],
+        ],
+        **kwargs,
+    ):
+        super().__init__()
+        self.indices = []
+        self.classes = classes
+        color_classes = np.array(colors)
+        self.color_encoding = self.transform_color(color_classes)
+
+    def transform_color(self, color_classes):
+        r = np.asarray(color_classes[:, 0], dtype=np.uint32)
+        g = np.asarray(color_classes[:, 1], dtype=np.uint32)
+        b = np.asarray(color_classes[:, 2], dtype=np.uint32)
+        rgb_arr = np.array((r << 16) | (g << 8) | (b << 0), dtype=np.uint32)
+        rgb_arr.dtype = np.float32
+        return cp.asarray(rgb_arr)
+
+    def __call__(
+        self,
+        elevation_map: cp.ndarray,
+        layer_names: List[str],
+        plugin_layers: cp.ndarray,
+        plugin_layer_names: List[str],
+        semantic_map,
+        **kwargs,
+    ) -> cp.ndarray:
+        # get indices of all layers that
+        layer_indices = cp.array([], dtype=cp.int32)
+        for it, fusion_alg in enumerate(semantic_map.param.fusion_algorithms):
+            if fusion_alg == "class_average" and (
+                semantic_map.param.additional_layers[it] in self.classes
+            ):
+                layer_indices = cp.append(layer_indices, it).astype(cp.int32)
+
+        # check which has the highest value
+        class_map = cp.argmax(semantic_map.map[layer_indices], axis=0)
+        # create color coding
+        enc = self.color_encoding[class_map]
+        return enc