diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml
index a4aa684a..fd745524 100644
--- a/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml
+++ b/elevation_mapping_cupy/config/setups/turtle_bot/plugin_config.yaml
@@ -42,19 +42,26 @@ semantic_filter:
enable: True
fill_nan: False
is_height_layer: False
- layer_name: "sem_fil"
+ layer_name: "max_categories"
extra_params:
- classes: ['grass','tree','fence','dirt']
+ classes: ['^sem_.*$']
semantic_traversability:
type: "semantic_traversability"
enable: False
fill_nan: False
is_height_layer: False
- layer_name: "sem_traversability"
+ layer_name: "semantic_traversability"
extra_params:
layers: ['traversability','robot_centric_elevation']
thresholds: [0.3,0.5]
type: ['traversability', 'elevation']
-
+features_pca:
+ type: "features_pca"
+ enable: True
+ fill_nan: False
+ is_height_layer: False
+ layer_name: "pca"
+ extra_params:
+ process_layer_names: ["^feat_.*$"]
\ No newline at end of file
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_image_semantics.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_image_semantics.yaml
deleted file mode 100644
index d4451a5d..00000000
--- a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_image_semantics.yaml
+++ /dev/null
@@ -1,57 +0,0 @@
-#### Plugins ########
-plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
-
-#### Subscribers ########
-subscribers:
-# sensor_name:
-# channels: ['feat_0','feat_1']
-# fusion: ['average','average']
-# topic_name: '/elevation_mapping/pointcloud_semantic'
- front_cam_2:
- channels: [ 'grass','tree',"person" ]
- fusion: [ 'exponential','exponential','exponential' ]
- topic_name_camera: '/elevation_mapping/semantic_front'
- topic_name_camera_info: '/camera/depth/camera_info'
- data_type: image
-
- color_cam: # for color camera
- channels: ['rgb']
- fusion: ['color']
- topic_name_camera: '/camera/rgb/image_raw'
- topic_name_camera_info: '/camera/depth/camera_info'
- data_type: image
-
- pointcloud:
- channels: []
- fusion: []
- topic_name: '/camera/depth/points'
- data_type: pointcloud
-
- front_cam:
- channels: ['grass','tree',"person"]
- fusion: ['exponential','exponential','exponential']
- topic_name_camera: '/elevation_mapping/semantic_front'
- topic_name_camera_info: '/camera/depth/camera_info'
- data_type: image
-
- semantic_segmentation: True
- # segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
- segmentation_model: 'lraspp_mobilenet_v3_large' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
- show_label_legend: False
- image_topic: "/camera/rgb/image_raw"
- image_info_topic: "/camera/depth/camera_info"
- resize: 0.5
-
- sem_seg_topic: "/elevation_mapping/semantic_front"
- sem_seg_image_topic: "/elevation_mapping/semantic_image_front"
-
-#### Publishers ########
-publishers:
- elevation_map_raw:
- layers: ['elevation', 'traversability', 'variance','rgb','sem_fil']
- basic_layers: ['elevation']
- fps: 5.0
- elevation_map_filter:
- layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','sem_fil']
- basic_layers: ['min_filter']
- fps: 3.0
\ No newline at end of file
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_image.yaml
similarity index 66%
rename from elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml
rename to elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_image.yaml
index 63d3b2b9..0fd7d215 100644
--- a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml
+++ b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_image.yaml
@@ -22,28 +22,23 @@ subscribers:
camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
channel_info_topic_name: '/front_cam/channel_info'
data_type: image
- # fixed_semantic_cam: # for semantic images
- # topic_name: '/front_cam/semantic_image'
- # camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
- # channels: ["test", "test2", "test3"]
- # data_type: image
front_cam_pointcloud:
topic_name: '/camera/depth/points'
data_type: pointcloud
feat_front:
- topic_name: /elevation_mapping/feat_f
- camera_info_topic_name: "/camera/depth/camera_info"
- channels: [ 'feat_0','feat_1','feat_2','feat_3','feat_4','feat_5','feat_6','feat_7','feat_8','feat_9' ]
+ topic_name: '/front_cam/semantic_seg_feat'
+ camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
+ channel_info_topic_name: '/front_cam/feat_channel_info'
data_type: image
#### Publishers ########
publishers:
elevation_map_raw:
- layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'chair', 'sofa']
+ layers: ['elevation', 'traversability', 'variance','rgb','max_categories', 'pca']
basic_layers: ['elevation']
fps: 5.0
elevation_map_filter:
- layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','sem_fil']
+ layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','max_categories']
basic_layers: ['min_filter']
fps: 3.0
\ No newline at end of file
diff --git a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml
new file mode 100644
index 00000000..19db1159
--- /dev/null
+++ b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics_pointcloud.yaml
@@ -0,0 +1,28 @@
+#### Plugins ########
+plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
+
+pointcloud_channel_fusions:
+ rgb: 'color'
+ default: 'average'
+
+image_channel_fusions:
+ rgb: 'color'
+ default: 'exponential'
+
+#### Subscribers ########
+subscribers:
+ front_cam_pointcloud:
+ topic_name: '/camera/depth/points'
+ data_type: pointcloud
+
+
+#### Publishers ########
+publishers:
+ elevation_map_raw:
+ layers: ['elevation', 'traversability', 'variance','rgb','max_categories']
+ basic_layers: ['elevation']
+ fps: 5.0
+ elevation_map_filter:
+ layers: ['min_filter', 'smooth', 'inpaint', 'elevation','rgb','max_categories']
+ basic_layers: ['min_filter']
+ fps: 3.0
\ No newline at end of file
diff --git a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch
index 386dfdff..b0fb53c9 100644
--- a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch
+++ b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch
@@ -1,7 +1,7 @@
-
-
+
+
-
+
\ No newline at end of file
diff --git a/elevation_mapping_cupy/launch/turtlesim_features_image_example.launch b/elevation_mapping_cupy/launch/turtlesim_features_image_example.launch
deleted file mode 100644
index d4b7af3c..00000000
--- a/elevation_mapping_cupy/launch/turtlesim_features_image_example.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/elevation_mapping_cupy/launch/turtlesim_segmentation_example.launch b/elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch
similarity index 100%
rename from elevation_mapping_cupy/launch/turtlesim_segmentation_example.launch
rename to elevation_mapping_cupy/launch/turtlesim_plane_decomposition_example.launch
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_example.launch
deleted file mode 100644
index d071ddbb..00000000
--- a/elevation_mapping_cupy/launch/turtlesim_semantic_example.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch
index 30741b50..05064d0a 100644
--- a/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch
+++ b/elevation_mapping_cupy/launch/turtlesim_semantic_image_example.launch
@@ -10,13 +10,10 @@
-
-
-
-
+
\ No newline at end of file
diff --git a/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch b/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch
index d672f6de..65142465 100644
--- a/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch
+++ b/elevation_mapping_cupy/launch/turtlesim_semantic_pointcloud_example.launch
@@ -6,14 +6,17 @@
-
-
+ -->
+
+
+
-
-
+
+
diff --git a/elevation_mapping_cupy/launch/turtlesim_example.launch b/elevation_mapping_cupy/launch/turtlesim_simple_example.launch
similarity index 100%
rename from elevation_mapping_cupy/launch/turtlesim_example.launch
rename to elevation_mapping_cupy/launch/turtlesim_simple_example.launch
diff --git a/elevation_mapping_cupy/rviz/turtle_example.rviz b/elevation_mapping_cupy/rviz/turtle_example.rviz
index fe4c79bb..0cd124c8 100644
--- a/elevation_mapping_cupy/rviz/turtle_example.rviz
+++ b/elevation_mapping_cupy/rviz/turtle_example.rviz
@@ -1,12 +1,14 @@
Panels:
- Class: rviz/Displays
- Help Height: 138
+ Help Height: 70
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
+ - /ElevationMapRaw1
+ - /Feature1
Splitter Ratio: 0.5768463015556335
- Tree Height: 229
+ Tree Height: 234
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -24,7 +26,7 @@ Panels:
- Class: rviz/Time
Name: Time
SyncMode: 0
- SyncSource: Image
+ SyncSource: Color
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -69,7 +71,7 @@ Visualization Manager:
Show Grid Lines: true
Topic: /elevation_mapping/elevation_map_raw
Unreliable: false
- Use Rainbow: true
+ Use Rainbow: false
Value: true
- Alpha: 1
Class: rviz/RobotModel
@@ -180,7 +182,7 @@ Visualization Manager:
Max Value: 1
Median window: 5
Min Value: 0
- Name: Image
+ Name: Color
Normalize Range: true
Queue Size: 2
Transport Hint: raw
@@ -188,11 +190,23 @@ Visualization Manager:
Value: true
- Class: rviz/Image
Enabled: true
- Image Topic: /front_cam/elevation_mapping/semantic_image_front
+ Image Topic: /front_cam/semantic_image_debug
Max Value: 1
Median window: 5
Min Value: 0
- Name: Image
+ Name: Segmentation
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /front_cam/semantic_seg_feat_im
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Feature
Normalize Range: true
Queue Size: 2
Transport Hint: raw
@@ -242,19 +256,23 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.5147963762283325
+ Pitch: 0.4647962152957916
Target Frame: base_footprint
- Yaw: 3.1033706665039062
+ Yaw: 3.113370418548584
Saved: ~
Window Geometry:
+ Color:
+ collapsed: false
Displays:
collapsed: false
- Height: 1043
+ Feature:
+ collapsed: false
+ Height: 1306
Hide Left Dock: false
Hide Right Dock: true
- Image:
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f700000464fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000016b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0043006f006c006f007201000001ac000000fd0000001600fffffffb00000018005300650067006d0065006e0074006100740069006f006e01000002af000000e40000001600fffffffb0000000e00460065006100740075007200650100000399000001060000001600ffffff000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000046400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Segmentation:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001f70000035dfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001eb000000d30000001600fffffffb0000000a0049006d00610067006501000002c4000000d40000001600ffffff000000010000015f0000053bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000560000053b000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000005830000035d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -265,4 +283,4 @@ Window Geometry:
collapsed: true
Width: 1920
X: 2280
- Y: 105
+ Y: 68
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
index e7011529..42c81c04 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py
@@ -731,7 +731,7 @@ def get_map_with_name_ref(self, name, data):
self.elevation_map,
self.layer_names,
self.semantic_map.semantic_map,
- self.semantic_map.param,
+ self.semantic_map.layer_names,
self.base_rotation,
self.semantic_map.elements_to_shift,
)
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/features_pca.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/features_pca.py
index ad1236bf..8ea2d54a 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/features_pca.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/features_pca.py
@@ -5,6 +5,7 @@
import cupy as cp
import numpy as np
from typing import List
+import re
from elevation_mapping_cupy.plugins.plugin_manager import PluginBase
from sklearn.decomposition import PCA
@@ -20,10 +21,26 @@ class FeaturesPca(PluginBase):
"""
def __init__(
self,
+ cell_n: int = 100,
+ process_layer_names: List[str]=[],
**kwargs,
):
super().__init__()
- self.indices = []
+ self.process_layer_names = process_layer_names
+
+ def get_layer_indices(self, layer_names: List[str]) -> List[int]:
+ """ Get the indices of the layers that are to be processed using regular expressions.
+ Args:
+ layer_names (List[str]): List of layer names.
+ Returns:
+ List[int]: List of layer indices.
+ """
+ indices = []
+ for i, layer_name in enumerate(layer_names):
+ if any(re.match(pattern, layer_name) for pattern in self.process_layer_names):
+ indices.append(i)
+ return indices
+
def __call__(
self,
@@ -31,8 +48,8 @@ def __call__(
layer_names: List[str],
plugin_layers: cp.ndarray,
plugin_layer_names: List[str],
- semantic_map,
- semantic_params,
+ semantic_map: cp.ndarray,
+ semantic_layer_names: List[str],
*args,
) -> cp.ndarray:
"""
@@ -49,31 +66,33 @@ def __call__(
cupy._core.core.ndarray:
"""
# get indices of all layers that contain semantic features information
- layer_indices = cp.array([], dtype=cp.int32)
- for it, fusion_alg in enumerate(semantic_params.fusion_algorithms):
- if fusion_alg in ["average", "bayesian_inference", "exponential"]:
- layer_indices = cp.append(layer_indices, it).astype(cp.int32)
-
- n_c = semantic_map[layer_indices].shape[1]
- comp_img = np.zeros((n_c, n_c, 3), dtype=np.float32)
- # check which has the highest value
- if len(layer_indices) > 0:
- data = cp.reshape(semantic_map[layer_indices], (len(layer_indices), -1)).T.get()
- # data = np.clip(data, -1, 1)
+ data = []
+ for m, layer_names in zip([elevation_map, plugin_layers, semantic_map],
+ [layer_names, plugin_layer_names, semantic_layer_names]):
+ layer_indices = self.get_layer_indices(layer_names)
+ if len(layer_indices) > 0:
+ n_c = m[layer_indices].shape[1]
+ data_i = cp.reshape(m[layer_indices], (len(layer_indices), -1)).T.get()
+ data_i = np.clip(data_i, -1, 1)
+ data.append(data_i)
+ if len(data) > 0:
+ data = np.concatenate(data, axis=1)
+ # check which has the highest value
n_components = 3
pca = PCA(n_components=n_components).fit(data)
pca_descriptors = pca.transform(data)
img_pca = pca_descriptors.reshape(n_c, n_c, n_components)
comp = img_pca # [:, :, -3:]
- var = comp.std(axis=(0, 1))
comp_min = comp.min(axis=(0, 1))
comp_max = comp.max(axis=(0, 1))
if (comp_max - comp_min).any() != 0:
comp_img = np.divide((comp - comp_min), (comp_max - comp_min))
- map = (comp_img * 255).astype(np.uint8)
- r = np.asarray(map[:, :, 0], dtype=np.uint32)
- g = np.asarray(map[:, :, 1], dtype=np.uint32)
- b = np.asarray(map[:, :, 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)
+ pca_map = (comp_img * 255).astype(np.uint8)
+ r = np.asarray(pca_map[:, :, 0], dtype=np.uint32)
+ g = np.asarray(pca_map[:, :, 1], dtype=np.uint32)
+ b = np.asarray(pca_map[:, :, 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)
+ else:
+ return cp.zeros_like(elevation_map[0])
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py
index b5c2d313..01e3aea8 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py
@@ -103,7 +103,7 @@ def load_plugin_settings(self, file_path: str):
is_height_layer=v["is_height_layer"],
)
)
- extra_params.append(v["extra_params"])
+ extra_params.append(v["extra_params"])
self.init(plugin_params, extra_params)
print("Loaded plugins are ", *self.plugin_names)
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/robot_centric_elevation.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/robot_centric_elevation.py
index e9b43266..334085f0 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/robot_centric_elevation.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/robot_centric_elevation.py
@@ -88,8 +88,8 @@ def __call__(
layer_names: List[str],
plugin_layers: cp.ndarray,
plugin_layer_names: List[str],
- semantic_map,
- semantic_params,
+ semantic_map: cp.ndarray,
+ semantic_layer_names: List[str],
rotation,
*args,
) -> cp.ndarray:
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_filter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_filter.py
index 3331afe7..7085f558 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_filter.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_filter.py
@@ -5,6 +5,7 @@
import cupy as cp
import numpy as np
from typing import List
+import re
from elevation_mapping_cupy.plugins.plugin_manager import PluginBase
@@ -28,7 +29,17 @@ def __init__(
self.classes = classes
self.color_encoding = self.transform_color()
- def color_map(self, N=256, normalized=False):
+ def color_map(self, N:int =256, normalized: bool=False):
+ """
+ Creates a color map with N different colors.
+
+ Args:
+ N (int, optional): The number of colors in the map. Defaults to 256.
+ normalized (bool, optional): If True, the colors are normalized to the range [0,1]. Defaults to False.
+
+ Returns:
+ np.ndarray: The color map.
+ """
def bitget(byteval, idx):
return (byteval & (1 << idx)) != 0
@@ -52,6 +63,12 @@ def bitget(byteval, idx):
return cmap[1:]
def transform_color(self):
+ """
+ Transforms the color map into a format that can be used for semantic filtering.
+
+ Returns:
+ cp.ndarray: The transformed color map.
+ """
color_classes = self.color_map(255)
r = np.asarray(color_classes[:, 0], dtype=np.uint32)
g = np.asarray(color_classes[:, 1], dtype=np.uint32)
@@ -60,6 +77,19 @@ def transform_color(self):
rgb_arr.dtype = np.float32
return cp.asarray(rgb_arr)
+ def get_layer_indices(self, layer_names: List[str]) -> List[int]:
+ """ Get the indices of the layers that are to be processed using regular expressions.
+ Args:
+ layer_names (List[str]): List of layer names.
+ Returns:
+ List[int]: List of layer indices.
+ """
+ indices = []
+ for i, layer_name in enumerate(layer_names):
+ if any(re.match(pattern, layer_name) for pattern in self.classes):
+ indices.append(i)
+ return indices
+
def __call__(
self,
elevation_map: cp.ndarray,
@@ -67,7 +97,7 @@ def __call__(
plugin_layers: cp.ndarray,
plugin_layer_names: List[str],
semantic_map: cp.ndarray,
- semantic_params,
+ semantic_layer_names: List[str],
rotation,
elements_to_shift,
*args,
@@ -86,28 +116,18 @@ def __call__(
cupy._core.core.ndarray:
"""
# get indices of all layers that contain semantic class information
- layer_indices = cp.array([], dtype=cp.int32)
- max_idcs = cp.array([], dtype=cp.int32)
- for it, fusion_alg in enumerate(semantic_params.fusion_algorithms):
- if fusion_alg in ["class_bayesian", "class_average", "exponential"]:
- layer_indices = cp.append(layer_indices, it).astype(cp.int32)
- # we care only for the first max in the display
- if fusion_alg in ["class_max"] and len(max_idcs) < 1:
- max_idcs = cp.append(max_idcs, it).astype(cp.int32)
-
- # check which has the highest value
- if len(layer_indices) > 0:
- class_map = cp.amax(semantic_map[layer_indices], axis=0)
- class_map_id = cp.argmax(semantic_map[layer_indices], axis=0)
- else:
- class_map = cp.zeros_like(semantic_map[0])
- class_map_id = cp.zeros_like(semantic_map[0], dtype=cp.int32)
- if "class_max" in semantic_params.fusion_algorithms:
- max_map = cp.amax(semantic_map[max_idcs], axis=0)
- max_map_id = elements_to_shift["id_max"][max_idcs]
- map = cp.where(max_map > class_map, max_map_id, class_map_id)
+ data = []
+ for m, layer_names in zip([elevation_map, plugin_layers, semantic_map],
+ [layer_names, plugin_layer_names, semantic_layer_names]):
+ layer_indices = self.get_layer_indices(layer_names)
+ if len(layer_indices) > 0:
+ data.append(m[layer_indices])
+ if len(data) > 0:
+ data = cp.concatenate(data, axis=0)
+ class_map = cp.amax(data, axis=0)
+ class_map_id = cp.argmax(data, axis=0)
else:
- map = class_map_id
- # create color coding
- enc = self.color_encoding[map]
- return enc
+ class_map = cp.zeros_like(elevation_map[0])
+ class_map_id = cp.zeros_like(elevation_map[0], dtype=cp.int32)
+ enc = self.color_encoding[class_map_id]
+ return enc
\ No newline at end of file
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_traversability.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_traversability.py
index 0c9af601..842d3b16 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_traversability.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/semantic_traversability.py
@@ -38,8 +38,8 @@ def __call__(
layer_names: List[str],
plugin_layers: cp.ndarray,
plugin_layer_names: List[str],
- semantic_map,
- semantic_params,
+ semantic_map: cp.ndarray,
+ semantic_layer_names: List[str],
*args,
) -> cp.ndarray:
"""
@@ -62,9 +62,9 @@ def __call__(
if name in layer_names:
idx = layer_names.index(name)
tempo = elevation_map[idx]
- elif name in semantic_params.additional_layers:
- idx = semantic_params.additional_layers.index(name)
- tempo = semantic_map[idx]
+ # elif name in semantic_params.additional_layers:
+ # idx = semantic_params.additional_layers.index(name)
+ # tempo = semantic_map[idx]
elif name in plugin_layer_names:
idx = plugin_layer_names.index(name)
tempo = plugin_layers[idx]
diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py
index 6b7291ca..1d2eb838 100644
--- a/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py
+++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py
@@ -173,8 +173,7 @@ def get_fusion(self, channels: List[str], channel_fusions: Dict[str, str], layer
layer_specs[channel] = matched_fusion
self.update_fusion_setting()
x = layer_specs[channel]
- if x not in fusion_list:
- fusion_list.append(x)
+ fusion_list.append(x)
process_channels.append(channel)
return process_channels, fusion_list
@@ -287,12 +286,8 @@ def update_layers_image(
image_width:
"""
- # additional_fusion = self.get_fusion_of_pcl(channels)
process_channels, fusion_methods = self.get_fusion(channels, self.param.image_channel_fusions, self.layer_specs_image)
self.new_map[self.delete_new_layers] = 0.0
- # config = self.param.subscriber_cfg[sub_key]
-
- # for j, (fusion, channel) in enumerate(zip(config["fusion"], config["channels"])):
for j, (fusion, channel) in enumerate(zip(fusion_methods, process_channels)):
if channel not in self.layer_names:
print(f"Layer {channel} not found, adding it to the semantic map")
@@ -303,8 +298,6 @@ def update_layers_image(
print(f"Layer {channel} not found!")
return
- # which layers need to be updated with this fusion algorithm
- # pcl_ids, layer_ids = self.get_indices_fusion(channels, fusion)
# update the layers with the fusion algorithm
self.fusion_manager.execute_image_plugin(
fusion,
diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp
index 21412e95..4a72e8c6 100644
--- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp
+++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp
@@ -86,6 +86,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
boost::function f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key);
ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, f);
pointcloudSubs_.push_back(sub);
+ ROS_INFO_STREAM("Subscribed to PointCloud2 topic: " << pointcloud_topic);
}
else if (type == "image") {
std::string camera_topic = subscriber.second["topic_name"];
@@ -122,6 +123,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub);
sync->registerCallback(boost::bind(&ElevationMappingNode::imageChannelCallback, this, _1, _2, _3));
cameraChannelSyncs_.push_back(sync);
+ ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ", Channel info topic: " << channel_info_topic);
}
else {
// If there is channels setting, we use it. Otherwise, we use rgb as default.
@@ -135,7 +137,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh)
else {
channels_[key].push_back("rgb");
}
- ROS_INFO_STREAM("Channel info topic not found for " << camera_topic << ". Using channels: " << boost::algorithm::join(channels_[key], ", "));
+ ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ". Channel info topic: " << (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")) : channel_info_topic));
CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub);
sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key));
cameraSyncs_.push_back(sync);
diff --git a/sensor_processing/semantic_sensor/config/sensor_parameter.yaml b/sensor_processing/semantic_sensor/config/sensor_parameter.yaml
index 4f3f9ebf..c497564c 100644
--- a/sensor_processing/semantic_sensor/config/sensor_parameter.yaml
+++ b/sensor_processing/semantic_sensor/config/sensor_parameter.yaml
@@ -38,9 +38,9 @@ front_cam_pointcloud:
show_label_legend: False
data_type: pointcloud
- cam_info_topic: "/camera/depth/camera_info"
- image_topic: "/camera/rgb/image_raw"
- depth_topic: "/camera/depth/image_raw"
+ cam_info_topic: "camera/depth/camera_info"
+ image_topic: "camera/rgb/image_raw"
+ depth_topic: "camera/depth/image_raw"
cam_frame: "camera_rgb_optical_frame"
front_cam_image:
@@ -53,9 +53,9 @@ front_cam_image:
data_type: image
semantic_segmentation: True
- # segmentation_model: 'detectron_coco_panoptic_fpn_R_101_3x' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
+ feature_extractor: True
segmentation_model: 'lraspp_mobilenet_v3_large' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
show_label_legend: False
- image_topic: "/camera/rgb/image_raw"
- image_info_topic: "/camera/depth/camera_info"
+ image_topic: "camera/rgb/image_raw"
+ camera_info_topic: "camera/depth/camera_info"
resize: 0.5
\ No newline at end of file
diff --git a/sensor_processing/semantic_sensor/script/semantic_sensor/image_node.py b/sensor_processing/semantic_sensor/script/semantic_sensor/image_node.py
index 8a89c81e..d59840ba 100755
--- a/sensor_processing/semantic_sensor/script/semantic_sensor/image_node.py
+++ b/sensor_processing/semantic_sensor/script/semantic_sensor/image_node.py
@@ -22,7 +22,7 @@
from semantic_sensor.networks import resolve_model
from sklearn.decomposition import PCA
-from elevation_map_msgs.msg import FusionInfo
+from elevation_map_msgs.msg import ChannelInfo
class SemanticSegmentationNode:
@@ -71,9 +71,9 @@ def register_sub_pub(self):
node_name = rospy.get_name()
# subscribers
- if self.param.image_info_topic is not None and self.param.resize is not None:
- rospy.Subscriber(self.param.image_info_topic, CameraInfo, self.image_info_callback)
- self.feat_im_info_pub = rospy.Publisher(node_name + "/" + self.param.image_info_topic + "_resized", CameraInfo, queue_size=2)
+ if self.param.camera_info_topic is not None and self.param.resize is not None:
+ rospy.Subscriber(self.param.camera_info_topic, CameraInfo, self.image_info_callback)
+ self.feat_im_info_pub = rospy.Publisher(node_name + "/" + self.param.camera_info_topic + "_resized", CameraInfo, queue_size=2)
if "compressed" in self.param.image_topic:
self.compressed = True
@@ -94,8 +94,9 @@ def register_sub_pub(self):
if self.param.feature_extractor:
self.feature_pub = rospy.Publisher(node_name + "/" + self.param.feature_topic, Image, queue_size=2)
self.feat_im_pub = rospy.Publisher(node_name + "/" + self.param.feat_image_topic, Image, queue_size=2)
+ self.feat_channel_info_pub = rospy.Publisher(node_name + "/" + self.param.feat_channel_info_topic, ChannelInfo, queue_size=2)
- self.fusion_info_pub = rospy.Publisher(node_name + "/" + self.param.fusion_info_topic, FusionInfo, queue_size=2)
+ self.channel_info_pub = rospy.Publisher(node_name + "/" + self.param.channel_info_topic, ChannelInfo, queue_size=2)
def color_map(self, N=256, normalized=False):
"""Create a color map for the class labels.
@@ -177,24 +178,23 @@ def image_callback(self, rgb_msg):
if self.param.semantic_segmentation:
self.publish_segmentation()
self.publish_segmentation_image()
- self.publish_fusion_info()
+ self.publish_channel_info([f"sem_{c}" for c in self.param.channels], self.channel_info_pub)
if self.param.feature_extractor:
self.publish_feature()
self.publish_feature_image(self.features)
- self.publish_fusion_info()
+ self.publish_channel_info([f"feat_{i}" for i in range(self.features.shape[0])], self.feat_channel_info_pub)
if self.param.resize is not None:
self.pub_info()
def pub_info(self):
self.feat_im_info_pub.publish(self.info)
- def publish_fusion_info(self):
+ def publish_channel_info(self, channels, pub):
"""Publish fusion info."""
- info = FusionInfo()
+ info = ChannelInfo()
info.header = self.header
- info.channels = self.param.channels
- info.fusion_methods = self.param.fusion_methods
- self.fusion_info_pub.publish(info)
+ info.channels = channels
+ pub.publish(info)
def process_image(self, image):
"""Depending on setting generate color, semantic segmentation or feature channels.
diff --git a/sensor_processing/semantic_sensor/script/semantic_sensor/image_parameters.py b/sensor_processing/semantic_sensor/script/semantic_sensor/image_parameters.py
index 014bd9b9..aacb152c 100644
--- a/sensor_processing/semantic_sensor/script/semantic_sensor/image_parameters.py
+++ b/sensor_processing/semantic_sensor/script/semantic_sensor/image_parameters.py
@@ -23,19 +23,20 @@ class ImageParameter(Serializable):
semantic_segmentation: bool = True
# sem_seg_topic: str = "semantic_seg"
# sem_seg_image_topic: str = "semantic_seg_im"
- publish_topic: str = "semantic_seg"
- publish_image_topic: str = "semantic_seg_img"
# publish_camera_info_topic: str = "semantic_seg/camera_info"
segmentation_model: str = "detectron_coco_panoptic_fpn_R_101_3x"
show_label_legend: bool = False
channels: list = field(default_factory=lambda: ["grass", "road", "tree", "sky"])
- fusion_methods: list = field(default_factory=lambda: ["class_average", "class_average", "class_average", "class_average"])
+
+ publish_topic: str = "semantic_seg"
+ publish_image_topic: str = "semantic_seg_img"
+ channel_info_topic: str = "channel_info"
feature_extractor: bool = False
feature_config: FeatureExtractorParameter = FeatureExtractorParameter
# feature_config.input_size: list = field(default_factory=lambda: [80, 160])
feature_topic: str = "semantic_seg_feat"
feat_image_topic: str = "semantic_seg_feat_im"
- fusion_info_topic: str = "fusion_info"
+ feat_channel_info_topic: str = "feat_channel_info"
resize: float = None
- image_info_topic: str = "image_info"
\ No newline at end of file
+ camera_info_topic: str = "camera_info"
\ No newline at end of file
diff --git a/sensor_processing/semantic_sensor/script/semantic_sensor/networks.py b/sensor_processing/semantic_sensor/script/semantic_sensor/networks.py
index df7c578c..0c666a3e 100644
--- a/sensor_processing/semantic_sensor/script/semantic_sensor/networks.py
+++ b/sensor_processing/semantic_sensor/script/semantic_sensor/networks.py
@@ -118,17 +118,20 @@ def resolve_categories(self):
indices.append(class_to_idx[chan])
channels.append(chan)
self.actual_channels.append(chan)
- elif self.param.fusion_methods[it] in ["class_average", "class_bayesian"]:
- print(chan, " is not in the semantic segmentation model.")
- for it, chan in enumerate(self.param.channels):
- if self.param.fusion_methods[it] in ["class_max"]:
- self.actual_channels.append(chan)
- print(
- chan,
- " is not in the semantic segmentation model but is a max channel.",
- )
else:
- pass
+ self.actual_channels.append(chan)
+ # elif self.param.fusion_methods[it] in ["class_average", "class_bayesian"]:
+ # print(chan, " is not in the semantic segmentation model.")
+ # for it, chan in enumerate(self.param.channels):
+ # self.actual_channels.append(chan)
+ # if self.param.fusion_methods[it] in ["class_max"]:
+ # self.actual_channels.append(chan)
+ # print(
+ # chan,
+ # " is not in the semantic segmentation model but is a max channel.",
+ # )
+ # else:
+ # pass
self.stuff_categories = dict(zip(channels, indices))
self.segmentation_channels = dict(zip(channels, indices))
@@ -152,14 +155,14 @@ def __call__(self, image, *args, **kwargs):
selected_masks = cp.asarray(normalized_masks[list(self.stuff_categories.values())])
# get values of max, first remove the ones we already have
normalized_masks[list(self.stuff_categories.values())] = 0
- for i in range(self.param.fusion_methods.count("class_max")):
- maxim, index = torch.max(normalized_masks, dim=0)
- mer = encode_max(maxim, index)
- selected_masks = cp.concatenate((selected_masks, cp.expand_dims(mer, axis=0)), axis=0)
- x = torch.arange(0, index.shape[0])
- y = torch.arange(0, index.shape[1])
- c = torch.meshgrid(x, y, indexing="ij")
- normalized_masks[index, c[0], c[1]] = 0
+ # for i in range(self.param.fusion_methods.count("class_max")):
+ # maxim, index = torch.max(normalized_masks, dim=0)
+ # mer = encode_max(maxim, index)
+ # selected_masks = cp.concatenate((selected_masks, cp.expand_dims(mer, axis=0)), axis=0)
+ # x = torch.arange(0, index.shape[0])
+ # y = torch.arange(0, index.shape[1])
+ # c = torch.meshgrid(x, y, indexing="ij")
+ # normalized_masks[index, c[0], c[1]] = 0
assert len(self.actual_channels) == selected_masks.shape[0]
return cp.asarray(selected_masks)
@@ -260,7 +263,7 @@ def __init__(self, weights, cfg):
self.device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
self.model.to(self.device)
self.model.eval()
- self.shrink = Resize(size=(self.cfg.input_size[0], self.cfg.input_size[1]))
+ self.shrink = Resize(size=(self.cfg.input_size[0], self.cfg.input_size[1]), antialias=True)
def to_tensor(self, data):
data = data.astype(np.float32)
@@ -277,7 +280,7 @@ def __call__(self, image, *args, **kwargs):
# image = torch.as_tensor(image, device="cuda").permute(2, 0, 1).unsqueeze(0)
image = self.to_tensor(image).unsqueeze(0)
# if self.cfg.pcl:
- reset_size = Resize(image.shape[-2:], interpolation=TF.InterpolationMode.NEAREST)
+ reset_size = Resize(image.shape[-2:], interpolation=TF.InterpolationMode.NEAREST, antialias=True)
im_size = image.shape[-2:]
image = self.shrink(image)
image = TF.normalize(image, mean=(0.485, 0.456, 0.406), std=(0.229, 0.224, 0.225))