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.yaml index 4cffcde2..566fc4d3 100644 --- a/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml +++ b/elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml @@ -1,10 +1,17 @@ #### Plugins ######## plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml' +pointcloud_channel_fusion: + rgb: 'pointcloud_color' + default: 'pointcloud_average' + sheep: 'pointcloud_average' + sofa: 'pointcloud_class_average' + person: 'pointcloud_class_average' + #### Subscribers ######## subscribers: front_cam: - channels: ['rgb', 'grass','tree',"person" ] + channels: ['rgb', 'sheep','sofa',"person" ] fusion: [ 'color','class_average','class_average','class_average' ] topic_name: '/elevation_mapping/pointcloud_semantic' semantic_segmentation: True @@ -22,7 +29,7 @@ subscribers: #### Publishers ######## publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance','rgb','sem_fil'] + layers: ['elevation', 'traversability', 'variance','rgb','sem_fil', 'person', 'sheep', 'sofa'] basic_layers: ['elevation'] fps: 5.0 elevation_map_filter: diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 5811b05b..4345f04e 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -74,8 +74,9 @@ class ElevationMappingNode { void readParameters(); void setupMapPublishers(); void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key); - void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const std::string& key); + void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, + const std::vector& channels, const std::vector& fusion_methods); + void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); void publishAsPointCloud(const grid_map::GridMap& map) const; bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index ff92db66..b6d633da 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -48,8 +48,8 @@ class ElevationMappingWrapper { void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); - void input_image(const std::string& key, const std::vector& image, const RowMatrixXd& R, const Eigen::VectorXd& t, - const RowMatrixXd& cameraMatrix, int height, int width); + void input_image(const std::vector& multichannel_image, const std::vector& channels, const std::vector& fusion_methods, const RowMatrixXd& R, + const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, int height, int width); void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R); void clear(); void update_variance(); 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 c76bff7b..0f926c9a 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -452,6 +452,7 @@ def input_pointcloud( """ raw_points = cp.asarray(raw_points, dtype=self.data_type) additional_channels = channels[3:] + print("additional_channels", additional_channels) raw_points = raw_points[~cp.isnan(raw_points).any(axis=1)] self.update_map_with_kernel( raw_points, @@ -464,8 +465,9 @@ def input_pointcloud( def input_image( self, - sub_key: str, image: List[cp._core.core.ndarray], + channels: List[str], + fusion_methods: List[str], R: cp._core.core.ndarray, t: cp._core.core.ndarray, K: cp._core.core.ndarray, @@ -509,6 +511,9 @@ def input_image( self.uv_correspondence *= 0 self.valid_correspondence[:, :] = False + print("channels", channels) + print("fusion_methods", fusion_methods) + with self.map_lock: self.image_to_map_correspondence_kernel( self.elevation_map, @@ -524,8 +529,10 @@ def input_image( size=int(self.cell_n * self.cell_n), ) self.semantic_map.update_layers_image( - sub_key, + # sub_key, image, + channels, + fusion_methods, self.uv_correspondence, self.valid_correspondence, image_height, diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/fusion/fusion_manager.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/fusion/fusion_manager.py index 7910096d..0596af35 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/fusion/fusion_manager.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/fusion/fusion_manager.py @@ -54,11 +54,11 @@ def get_plugin_idx(self, name: str, data_type: str): """ Get a registered fusion plugin """ - name = data_type + "_" + name + # name = data_type + "_" + name for idx, plugin in enumerate(self.plugins): if plugin.name == name: return idx - print("Plugin {} is not in the list: {}".format(name, self.plugins)) + print("[WARNING] Plugin {} is not in the list: {}".format(name, self.plugins)) return None def execute_plugin( @@ -72,8 +72,8 @@ def execute_plugin( self.plugins[idx]( points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, elements_to_shift ) - else: - raise ValueError("Plugin {} is not registered".format(name)) + # else: + # raise ValueError("Plugin {} is not registered".format(name)) def execute_image_plugin( self, @@ -104,5 +104,5 @@ def execute_image_plugin( semantic_map, new_map, ) - else: - raise ValueError("Plugin {} is not registered".format(name)) + # else: + # raise ValueError("Plugin {} is not registered".format(name)) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py index 2d23a6d2..40ac3dd9 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py @@ -7,6 +7,7 @@ import numpy as np from simple_parsing.helpers import Serializable from dataclasses import field +from typing import Tuple @dataclass @@ -33,8 +34,19 @@ class Parameter(Serializable): } } ) - additional_layers: list = field(default_factory=lambda: ["feat_0"]) - fusion_algorithms: list = field(default_factory=lambda: ["average"]) + # additional_layers: list = field(default_factory=lambda: ["feat_0"]) + # fusion_algorithms: list = field(default_factory=lambda: ["average"]) + additional_layers: list = field(default_factory=lambda: ["color"]) + fusion_algorithms: list = field(default_factory=lambda: [ + "image_color", + "image_exponential", + "pointcloud_average", + "pointcloud_bayesian_inference", + "pointcloud_class_average", + "pointcloud_class_bayesian", + "pointcloud_class_max", + "pointcloud_color"]) + pointcloud_channel_fusion: dict = field(default_factory=lambda: {"rgb": "pointcloud_color", "default": "pointcloud_class_average"}) data_type: str = np.float32 average_weight: float = 0.5 @@ -126,18 +138,18 @@ def update(self): self.cell_n = int(round(self.map_length / self.resolution)) + 2 self.true_cell_n = round(self.map_length / self.resolution) self.true_map_length = self.true_cell_n * self.resolution - semantic_layers = [] - fusion_algorithms = [] - for subscriber, sub_val in self.subscriber_cfg.items(): - channels = sub_val["channels"] - fusion = sub_val["fusion"] - for i in range(len(channels)): - name = channels[i] - if name not in semantic_layers: - semantic_layers.append(name) - fusion_algorithms.append(fusion[i]) - self.additional_layers = semantic_layers - self.fusion_algorithms = fusion_algorithms + # semantic_layers = [] + # fusion_algorithms = [] + # for subscriber, sub_val in self.subscriber_cfg.items(): + # channels = sub_val["channels"] + # fusion = sub_val["fusion"] + # for i in range(len(channels)): + # name = channels[i] + # if name not in semantic_layers: + # semantic_layers.append(name) + # fusion_algorithms.append(fusion[i]) + # self.additional_layers = semantic_layers + # self.fusion_algorithms = fusion_algorithms if __name__ == "__main__": 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 d090416c..131ab0b9 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/semantic_map.py @@ -24,16 +24,31 @@ def __init__(self, param: Parameter): self.unique_data = [] self.elements_to_shift = {} - for k, config in self.param.subscriber_cfg.items(): - for f, c in zip(config["fusion"], config["channels"]): - if c not in self.layer_names: - self.layer_names.append(c) - self.layer_specs[c] = f - else: - assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!" - if f not in self.unique_fusion: - dt = config["data_type"] - self.unique_fusion.append(dt + "_" + f) + # for k, config in self.param.subscriber_cfg.items(): + # for f, c in zip(config["fusion"], config["channels"]): + # if c not in self.layer_names: + # self.layer_names.append(c) + # self.layer_specs[c] = f + # else: + # assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!" + # if f not in self.unique_fusion: + # dt = config["data_type"] + # self.unique_fusion.append(dt + "_" + f) + for channel, fusion in self.param.pointcloud_channel_fusion.items(): + if channel not in self.layer_names: + if channel != "default": + self.layer_names.append(channel) + self.layer_specs[channel] = fusion + else: + assert self.layer_specs[channel] == fusion, "Error: Single layer has multiple fusion algorithms!" + # for f, c in zip(config["fusion"], config["channels"]): + # else: + # assert self.layer_specs[c] == f, "Error: Single layer has multiple fusion algorithms!" + # if f not in self.unique_fusion: + # dt = config["data_type"] + # self.unique_fusion.append(dt + "_" + f) + self.layer_names += self.param.additional_layers + self.unique_fusion = self.param.fusion_algorithms self.amount_layer_names = len(self.layer_names) @@ -71,6 +86,43 @@ def initialize_fusion(self): self.elements_to_shift["id_max"] = id_max self.fusion_manager.register_plugin(fusion) + def update_fusion_setting(self): + for fusion in self.unique_fusion: + if "pointcloud_class_bayesian" == fusion: + pcl_ids = self.get_layer_indices("class_bayesian") + self.delete_new_layers[pcl_ids] = 0 + if "pointcloud_class_max" == fusion: + pcl_ids = self.get_layer_indices("class_max") + self.delete_new_layers[pcl_ids] = 0 + layer_cnt = self.param.fusion_algorithms.count("class_max") + id_max = cp.zeros( + (layer_cnt, self.param.cell_n, self.param.cell_n), + dtype=cp.uint32, + ) + self.elements_to_shift["id_max"] = id_max + + def add_layer(self, name): + """ + Add a new layer to the semantic map. + + Args: + name (str): The name of the new layer. + """ + if name not in self.layer_names: + self.layer_names.append(name) + self.semantic_map = cp.append( + self.semantic_map, + cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type), + axis=0, + ) + self.new_map = cp.append( + self.new_map, + cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type), + axis=0, + ) + self.delete_new_layers = cp.append(self.delete_new_layers, cp.array([1], dtype=cp.bool8)) + + def pad_value(self, x, shift_value, idx=None, value=0.0): """Create a padding of the map along x,y-axis according to amount that has shifted. @@ -120,11 +172,22 @@ def get_fusion_of_pcl(self, channels: List[str]) -> List[str]: channels (List[str]): """ fusion_list = [] + process_channels = [] for channel in channels: + if channel not in self.layer_specs: + if "default" in self.param.pointcloud_channel_fusion: + default_fusion = self.param.pointcloud_channel_fusion["default"] + print(f"[WARNING] Layer {channel} not found in layer_specs. Using {default_fusion} algorithm as default.") + self.layer_specs[channel] = default_fusion + self.update_fusion_setting() + else: + print(f"[WARNING] Layer {channel} not found in layer_specs. Skipping.") + continue x = self.layer_specs[channel] if x not in fusion_list: fusion_list.append(x) - return fusion_list + process_channels.append(channel) + return process_channels, fusion_list def get_layer_indices(self, fusion_alg): """Get the indices of the layers that are used for a specific fusion algorithm. @@ -177,11 +240,15 @@ def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map): t: translation vector elevation_map: elevation map object """ - additional_fusion = self.get_fusion_of_pcl(channels) + process_channels, additional_fusion = self.get_fusion_of_pcl(channels) + for channel in process_channels: + if channel not in self.layer_names: + print(f"Layer {channel} not found, adding it to the semantic map") + self.add_layer(channel) self.new_map[self.delete_new_layers] = 0.0 for fusion in list(set(additional_fusion)): # which layers need to be updated with this fusion algorithm - pcl_ids, layer_ids = self.get_indices_fusion(channels, fusion) + pcl_ids, layer_ids = self.get_indices_fusion(process_channels, fusion) # update the layers with the fusion algorithm self.fusion_manager.execute_plugin( fusion, @@ -198,8 +265,10 @@ def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map): def update_layers_image( self, - sub_key: str, + # sub_key: str, image: cp._core.core.ndarray, + channels: List[str], + fusion_methods: List[str], uv_correspondence: cp._core.core.ndarray, valid_correspondence: cp._core.core.ndarray, image_height: cp._core.core.ndarray, @@ -216,13 +285,26 @@ def update_layers_image( image_width: """ + # print("sub_key", sub_key) + # print("delete_new_layers", self.delete_new_layers) + # additional_fusion = self.get_fusion_of_pcl(channels) self.new_map[self.delete_new_layers] = 0.0 - config = self.param.subscriber_cfg[sub_key] + # config = self.param.subscriber_cfg[sub_key] - for j, (fusion, channel) in enumerate(zip(config["fusion"], config["channels"])): + # print("config", config) + + # for j, (fusion, channel) in enumerate(zip(config["fusion"], config["channels"])): + for j, (fusion, channel) in enumerate(zip(fusion_methods, channels)): + if channel not in self.layer_names: + print(f"Layer {channel} not found, adding it to the semantic map") + self.add_layer(channel) sem_map_idx = self.get_index(channel) + if sem_map_idx == -1: + 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 @@ -266,7 +348,9 @@ def get_map_with_name(self, name): Returns: cp.array: map """ - if self.layer_specs[name] == "color": + print("[get_map_with_name] name", name, self.layer_specs) + if name in self.layer_specs and (self.layer_specs[name] == "image_color" or self.layer_specs[name] == "pointcloud_color"): + print("this is a color map") m = self.get_rgb(name) return m else: @@ -321,4 +405,7 @@ def get_index(self, name): Returns: int: index """ - return self.layer_names.index(name) + if name not in self.layer_names: + return -1 + else: + return self.layer_names.index(name) diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 1b6c6dfa..5f926373 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -118,7 +118,7 @@ ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) cameraInfoSubs_.push_back(cam_info_sub); CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key)); + sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2)); cameraSyncs_.push_back(sync); } else { @@ -348,10 +348,10 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cl pointCloudProcessCounter_++; } -void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key) { - auto start = ros::Time::now(); - +void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, + const sensor_msgs::CameraInfoConstPtr& camera_info_msg, + const std::vector& channels, + const std::vector& fusion_methods) { // Get image cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; @@ -390,9 +390,19 @@ void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image } // Pass image to pipeline - map_.input_image(key, multichannel_image, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, + map_.input_image(multichannel_image, channels, fusion_methods, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, image.rows, image.cols); +} +void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, + const sensor_msgs::CameraInfoConstPtr& camera_info_msg) { + auto start = ros::Time::now(); + // Default channels and fusion methods for image is rgb and image_color + std::vector channels; + std::vector fusion_methods; + channels.push_back("rgb"); + fusion_methods.push_back("image_color"); + inputImage(image_msg, camera_info_msg, channels, fusion_methods); ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); } diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 51cf073e..2fd31770 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -121,6 +121,24 @@ void ElevationMappingWrapper::setParameters(ros::NodeHandle& nh) { } param_.attr("subscriber_cfg") = sub_dict; + if (!nh.hasParam("pointcloud_channel_fusion")) { + ROS_WARN("No pointcloud_channel_fusion parameter found. Using default values."); + } + else { + XmlRpc::XmlRpcValue pointcloud_channel_fusion; + nh.getParam("pointcloud_channel_fusion", pointcloud_channel_fusion); + + py::dict pointcloud_channel_fusion_dict; + for (auto& channel_fusion : pointcloud_channel_fusion) { + const char* const name = channel_fusion.first.c_str(); + std::string fusion = static_cast(channel_fusion.second); + if (!pointcloud_channel_fusion_dict.contains(name)) { + pointcloud_channel_fusion_dict[name] = fusion; + } + } + param_.attr("pointcloud_channel_fusion") = pointcloud_channel_fusion_dict; + } + param_.attr("update")(); resolution_ = py::cast(param_.attr("get_value")("resolution")); map_length_ = py::cast(param_.attr("get_value")("true_map_length")); @@ -137,10 +155,10 @@ void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector Eigen::Ref(t), positionNoise, orientationNoise); } -void ElevationMappingWrapper::input_image(const std::string& key, const std::vector& multichannel_image, const RowMatrixXd& R, +void ElevationMappingWrapper::input_image(const std::vector& multichannel_image, const std::vector& channels, const std::vector& fusion_methods, const RowMatrixXd& R, const Eigen::VectorXd& t, const RowMatrixXd& cameraMatrix, int height, int width) { py::gil_scoped_acquire acquire; - map_.attr("input_image")(key, multichannel_image, Eigen::Ref(R), Eigen::Ref(t), + map_.attr("input_image")(multichannel_image, channels, fusion_methods, Eigen::Ref(R), Eigen::Ref(t), Eigen::Ref(cameraMatrix), height, width); } @@ -189,9 +207,12 @@ void ElevationMappingWrapper::get_grid_map(grid_map::GridMap& gridMap, const std std::vector maps; for (const auto& layerName : layerNames) { - RowMatrixXf map(map_n_, map_n_); - map_.attr("get_map_with_name_ref")(layerName, Eigen::Ref(map)); - gridMap.add(layerName, map); + bool exists = map_.attr("exists_layer")(layerName).cast(); + if (exists) { + RowMatrixXf map(map_n_, map_n_); + map_.attr("get_map_with_name_ref")(layerName, Eigen::Ref(map)); + gridMap.add(layerName, map); + } } if (enable_normal_color_) { RowMatrixXf normal_x(map_n_, map_n_);