Cleaned up and using channel_info for image topic.

elevation_map_msgs/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
 add_message_files(
   FILES
   Statistics.msg
-  FusionInfo.msg
+  ChannelInfo.msg
 )
 
 ## Generate services in the 'srv' folder

elevation_map_msgs/msg/ChannelInfo.msg

@@ -0,0 +1,2 @@
+Header header
+string[] channels       # channel names for each layer

elevation_mapping_cupy/compile_commands.json

@@ -0,0 +1 @@
+/home/takahiro/catkin_ws/build/elevation_mapping_cupy/compile_commands.json

elevation_mapping_cupy/config/setups/anymal/anymal_sensor_parameter.yaml

@@ -1,5 +1,15 @@
 plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/anymal/anymal_plugin_config.yaml'
 
+pointcloud_channel_fusions:
+  rgb: 'color'
+  default: 'average'
+
+image_channel_fusions:
+  rgb: 'color'
+  default: 'exponential'
+  feat_.*: 'exponential'
+  sem_.*: 'exponential'
+
 #### Publishers ########
 # topic_name:
 #   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
@@ -8,7 +18,7 @@ plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/anymal
 
 publishers:
   elevation_map_raw:
-    layers: ['elevation', 'traversability', 'variance']
+    layers: ['elevation', 'traversability', 'variance', 'rgb']
     basic_layers: ['elevation', 'traversability']
     fps: 5.0
 
@@ -20,55 +30,47 @@ publishers:
 #### Subscribers ########
 subscribers:
   front_upper_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_front_upper/point_cloud_self_filtered
     data_type: pointcloud
 
   front_lower_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_front_lower/point_cloud_self_filtered
     data_type: pointcloud
 
   rear_upper_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_rear_upper/point_cloud_self_filtered
     data_type: pointcloud
 
   rear_lower_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_rear_lower/point_cloud_self_filtered
     data_type: pointcloud
 
   left_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_left/point_cloud_self_filtered
     data_type: pointcloud
 
   right_depth:
-    channels: []
-    fusion: []
     topic_name: /depth_camera_right/point_cloud_self_filtered
     data_type: pointcloud
 
+  front_wide_angle:
+    topic_name: /wide_angle_camera_front/image_raw
+    camera_info_topic_name: /wide_angle_camera_front/camera_info
+    data_type: image
+
+  rear_wide_angle:
+    topic_name: /wide_angle_camera_rear/image_raw
+    camera_info_topic_name: /wide_angle_camera_rear/camera_info
+    data_type: image
+
   # velodyne:
-  #   channels: []
-  #   fusion: []
   #   topic_name: /point_cloud_filter/lidar/point_cloud_filtered
   #   data_type: pointcloud
 
   front_bpearl:
-    channels: []
-    fusion: []
     topic_name: /robot_self_filter/bpearl_front/point_cloud
     data_type: pointcloud
 
   rear_bpearl:
-    channels: []
-    fusion: []
     topic_name: /robot_self_filter/bpearl_rear/point_cloud
     data_type: pointcloud

elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_semantics.yaml

@@ -1,47 +1,40 @@
 #### Plugins ########
 plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/turtle_bot/plugin_config.yaml'
 
-pointcloud_channel_fusion:
+pointcloud_channel_fusions:
+  none: 'none'
+  # rgb: 'color'
+  # default: 'average'
+
+image_channel_fusions:
   rgb: 'color'
-  default: 'average'
-  sheep: 'average'
-  sofa: 'class_average'
-  person: 'class_average'
+  default: 'exponential'
+  feat_.*: 'exponential'
 
 #### Subscribers ########
 subscribers:
   color_cam: # for color camera
-    # channels: ['rgb']
-    # fusion: ['color']
     topic_name: '/camera/rgb/image_raw'
     camera_info_topic_name: '/camera/depth/camera_info'
     data_type: image
-  # front_cam:
-    # channels: ['rgb', 'sheep','sofa',"person" ]
-    # fusion: [ 'color','class_average','class_average','class_average' ]
-    # topic_name: '/elevation_mapping/pointcloud_semantic'
-    # semantic_segmentation: True
-    # publish_segmentation_image: True
-    # segmentation_model: 'lraspp_mobilenet_v3_large' # detectron_coco_panoptic_fpn_R_101_3x, lraspp_mobilenet_v3_large
-    # show_label_legend: False
-
-    # cam_info_topic: "/camera/rgb/camera_info"
-    # image_topic: "/camera/rgb/image_raw"
-    # cam_frame: "camera_rgb_optical_frame"
-    # data_type: image
-    # depth_topic: "/camera/depth/image_raw"
-  semantic_cam: # for color camera
-    # channels: ['rgb']
-    # fusion: ['color']
+  semantic_cam: # for semantic images
     topic_name: '/front_cam/semantic_image'
     camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
-    fusion_info_topic_name: '/front_cam/fusion_info'
+    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:
-    channels: []
-    fusion: []
     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' ]
+    data_type: image
 
 
 #### Publishers ########

elevation_mapping_cupy/config/setups/turtle_bot/turtle_bot_simple.yaml

@@ -1,11 +1,14 @@
+pointcloud_channel_fusions:
+  rgb: 'color'
+  default: 'average'
+
+image_channel_fusions:
+  rgb: 'color'
+  default: 'exponential'
+  feat_.*: 'exponential'
+
 subscribers:
-#  sensor_name:
-#    channels: ['feat_0','feat_1']
-#    fusion: ['average','average']
-#    topic_name: '/elevation_mapping/pointcloud_semantic'
   front_cam:
-    channels: ['rgb' ]
-    fusion: [ 'color']
     topic_name: '/camera/depth/points'
     data_type: pointcloud
 

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -49,7 +49,7 @@
 
 #include <elevation_map_msgs/CheckSafety.h>
 #include <elevation_map_msgs/Initialize.h>
-#include <elevation_map_msgs/FusionInfo.h>
+#include <elevation_map_msgs/ChannelInfo.h>
 
 #include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp"
 
@@ -65,26 +65,37 @@ class ElevationMappingNode {
 
   using ImageSubscriber = image_transport::SubscriberFilter;
   using ImageSubscriberPtr = std::shared_ptr<ImageSubscriber>;
+
+  // Subscriber and Synchronizer for CameraInfo messages
   using CameraInfoSubscriber = message_filters::Subscriber<sensor_msgs::CameraInfo>;
   using CameraInfoSubscriberPtr = std::shared_ptr<CameraInfoSubscriber>;
   using CameraPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo>;
   using CameraSync = message_filters::Synchronizer<CameraPolicy>;
   using CameraSyncPtr = std::shared_ptr<CameraSync>;
 
-  using FusionInfoSubscriber = message_filters::Subscriber<elevation_map_msgs::FusionInfo>;
-  using FusionInfoSubscriberPtr = std::shared_ptr<FusionInfoSubscriber>;
-  using CameraFusionPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, elevation_map_msgs::FusionInfo>;
-  using CameraFusionSync = message_filters::Synchronizer<CameraFusionPolicy>;
-  using CameraFusionSyncPtr = std::shared_ptr<CameraFusionSync>;
+  // Subscriber and Synchronizer for ChannelInfo messages
+  using ChannelInfoSubscriber = message_filters::Subscriber<elevation_map_msgs::ChannelInfo>;
+  using ChannelInfoSubscriberPtr = std::shared_ptr<ChannelInfoSubscriber>;
+  using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, elevation_map_msgs::ChannelInfo>;
+  using CameraChannelSync = message_filters::Synchronizer<CameraChannelPolicy>;
+  using CameraChannelSyncPtr = std::shared_ptr<CameraChannelSync>;
+
+  // Subscriber and Synchronizer for Pointcloud messages
+  using PointCloudSubscriber = message_filters::Subscriber<sensor_msgs::PointCloud2>;
+  using PointCloudSubscriberPtr = std::shared_ptr<PointCloudSubscriber>;
+  using PointCloudPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, elevation_map_msgs::ChannelInfo>;
+  using PointCloudSync = message_filters::Synchronizer<PointCloudPolicy>;
+  using PointCloudSyncPtr = std::shared_ptr<PointCloudSync>;
 
  private:
   void readParameters();
   void setupMapPublishers();
   void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key);
-  void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg,
-                  const std::vector<std::string>& channels, const std::vector<std::string>& fusion_methods);
-  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
-  void imageFusionCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::FusionInfoConstPtr& fusion_info_msg);
+  void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector<std::string>& channels);
+  void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector<std::string>& channels);
+  void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key);
+  void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg);
+  void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg);
   // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& 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);
@@ -110,9 +121,10 @@ class ElevationMappingNode {
   std::vector<ros::Subscriber> pointcloudSubs_;
   std::vector<ImageSubscriberPtr> imageSubs_;
   std::vector<CameraInfoSubscriberPtr> cameraInfoSubs_;
-  std::vector<FusionInfoSubscriberPtr> fusionInfoSubs_;
+  std::vector<ChannelInfoSubscriberPtr> channelInfoSubs_;
   std::vector<CameraSyncPtr> cameraSyncs_;
-  std::vector<CameraFusionSyncPtr> cameraFusionSyncs_;
+  std::vector<CameraChannelSyncPtr> cameraChannelSyncs_;
+  std::vector<PointCloudSyncPtr> pointCloudSyncs_;
   std::vector<ros::Publisher> mapPubs_;
   tf::TransformBroadcaster tfBroadcaster_;
   ros::Publisher alivePub_;

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp

@@ -48,7 +48,7 @@ class ElevationMappingWrapper {
 
   void input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R, const Eigen::VectorXd& t,
              const double positionNoise, const double orientationNoise);
-  void input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& channels, const std::vector<std::string>& fusion_methods, const RowMatrixXd& R,
+  void input_image(const std::vector<ColMatrixXf>& multichannel_image, const std::vector<std::string>& channels, 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();

elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py

@@ -469,7 +469,7 @@ def input_image(
         self,
         image: List[cp._core.core.ndarray],
         channels: List[str],
-        fusion_methods: List[str],
+        # fusion_methods: List[str],
         R: cp._core.core.ndarray,
         t: cp._core.core.ndarray,
         K: cp._core.core.ndarray,
@@ -490,9 +490,6 @@ def input_image(
         Returns:
             None:
         """
-        print("input_image")
-        print("channels", channels)
-        print("fusion_methods", fusion_methods)
         image = np.stack(image, axis=0)
         if len(image.shape) == 2:
             image = image[None]
@@ -517,9 +514,6 @@ def input_image(
         self.valid_correspondence[:, :] = False
         # self.distance_correspondence *= 0.0
 
-        print("channels", channels)
-        print("fusion_methods", fusion_methods)
-
         with self.map_lock:
             self.image_to_map_correspondence_kernel(
                 self.elevation_map,
@@ -532,15 +526,11 @@ def input_image(
                 self.center,
                 self.uv_correspondence,
                 self.valid_correspondence,
-                # self.distance_correspondence,
                 size=int(self.cell_n * self.cell_n),
             )
-            # print("distance_correspondence", self.distance_correspondence)
             self.semantic_map.update_layers_image(
-                # sub_key,
                 image,
                 channels,
-                fusion_methods,
                 self.uv_correspondence,
                 self.valid_correspondence,
                 image_height,

elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py

@@ -16,26 +16,12 @@ class Parameter(Serializable):
     subscriber_cfg: dict = field(
         default_factory=lambda: {
             "front_cam": {
-                "cam_frame": "zed2i_right_camera_optical_frame",
-                "cam_info_topic": "/zed2i/zed_node/depth/camera_info",
                 "channels": ["rgb", "person"],
-                "confidence": True,
-                "confidence_threshold": 10,
-                "confidence_topic": "/zed2i/zed_node/confidence/confidence_map",
-                "depth_topic": "/zed2i/zed_node/depth/depth_registered",
-                "feature_extractor": False,
-                "fusion": ["color", "class_average"],
-                "image_topic": "/zed2i/zed_node/left/image_rect_color",
-                "segmentation_model": "lraspp_mobilenet_v3_large",
-                "semantic_segmentation": True,
-                "show_label_legend": True,
                 "topic_name": "/elevation_mapping/pointcloud_semantic",
                 "data_type": "pointcloud",
             }
         }
     )
-    # 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",
@@ -46,7 +32,8 @@ class Parameter(Serializable):
         "pointcloud_class_bayesian",
         "pointcloud_class_max",
         "pointcloud_color"])
-    pointcloud_channel_fusion: dict = field(default_factory=lambda: {"rgb": "pointcloud_color", "default": "pointcloud_class_average"})
+    pointcloud_channel_fusions: dict = field(default_factory=lambda: {"rgb": "color", "default": "class_average"})
+    image_channel_fusions: dict = field(default_factory=lambda: {"rgb": "color", "default": "exponential"})
     data_type: str = np.float32
     average_weight: float = 0.5
 
@@ -138,18 +125,6 @@ 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
 
 
 if __name__ == "__main__":

elevation_mapping_cupy/script/elevation_mapping_cupy/plugins/plugin_manager.py

@@ -146,7 +146,7 @@ def update_with_name(
         elements_to_shift={},
     ):
         idx = self.get_layer_index_with_name(name)
-        if idx is not None:
+        if idx is not None and idx < len(self.plugins):
             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)