Add color to costmap/occupancy grid

[MotorTrick]

Hi, I was reading #526 and I read that it would be possible that "the resulting points in obstacle and ground can have colors". Where should I edit the code to have something like this? I want to differentiate between different obstacles, hence for each obstacle cell I may want a different color in the map and this should be rectified every time is necessary. The faster way would be just to add this color encoding to every obstacle cell clearly (without having to create another costmap) and to add this information to the database. This would mean to create a new list for each signature (along empty, obstacle, ground) as long as obstacle where for each element I have the corresponding color value (so that the system is able to recreate it)

[matlabbe]

For example with this demo, if you modify the the launch file as follows:

• Set Grid/FromDepth to true here
• Add:

  <param name="Grid/3D"   type="string" value="true"/>
  <param name="Grid/RayTracing"   type="string" value="false"/>
  <param name="Grid/MaxObstacleHeight" type="string" value="2"/>

  Launch the demo and add /rtabmap/cloud_obstacles to rviz:

  roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
  rosbag play --clock demo_mapping.bag

  The obstacles have the color of the RGB image. When visualizing individual local grids in DatabaseViewer:

The colors are added implicitly here when creating the cloud to segment from RGB-D or Stereo data contained in the node: https://github.com/introlab/rtabmap/blob/e8e3649f249382a790968ecbfa1dd9f32606d7d4/corelib/src/OccupancyGrid.cpp#L368-L369

As pointed out in the referred post, the obstacles here would contain a RGB channel if Grid/FromDepth is true and Grid/3D is true. Before making any modifications, how are obstacles semantic computed in your approach? Is it image-based segmentation or pointcloud-based segmentation? For image-based segmentation, one could feed the segmented image instead of the RGB image to rtabmap, but loop closure detection won't work anymore (can be ok if you are using a lidar for localization though). With more deep modifications, using the multi-cameras approach already in rtabmap, would be to feed the segmented image along the RGB images (like if it was a two cameras setup), but we should add parameters to rtabmap to tell it to do loop closure detection on the first camera, but grid segmentation with the second camera.

EDIT: just thought that only one image could be used for loop closure and grid segmentation if the input RGB image is a grayscale image with colored tint from the segmentation. For loop closure, the RGB images are converted to grayscale anyway, and the tint could be recovered in the grid.

cheers, Mathieu

[MotorTrick]

Actually, I'd like to color/create a 2D map. Each cell will be colored based either on the kind of obstacle that it represent/of there's a feature above that.

For example if I have a feature in world coordinates in (3,3,3) the 2D map in that point will be "colored" or marked. Same happens if I decide to use segmentation (that I don't have rn) but still I'm interested in the creation of a 2D map. Most probably, I'll use features. My idea was to have on the end a map, like the costmap that has probability values, with a value (between 0,100) that represent the number of features in that column. We already have the features in the base frame if I'm correct, so the work would be just to create another layer of costmaps correct?

[MotorTrick]

So I thought about that during the last few days: the best way for this to work for me would be creating a matrix where for each cell I can have an "array" of values. Much like a 2D map but for each cell instead of having a value like this I can have a list of values (int would be good). Then the structure would be the same (centering, resolution, etc).

In this way for example if I query (0.1, 0.1) I will have [1, 2, 5, 7] as result not just a single value.

I think that this would be possible if I can:

• add my custom structure in createLocalMap
• understand how to edit the code in the update such that the same cell viewed by two viewpoints is correctly merged
• write my caller/publisher following my custom structure/L647 and subsequents [easiest part]

EDIT: I think that this would be the same of adding another map layer where instead of having a list I can have a single value. It's just the type of the data that is different. BTW the information I need to embed and project is, for now, sparse but I don't want to cicle through all of that every time I need it but instead I prefer to gather it already prepped through your system [so it's almost instant]😄

[matlabbe]

This is quite specific to your application. One way would be to subscribe to data you want and reconstruct a map layer as you want. For example subscribing to /rtabmap/mapData to get features, transform them in map frame and project them in the occupancy grid map. The problem is when a loop closure happens, the whole occupancy grip has to be regenerated, you would need to update your structure too.

[MotorTrick]

Yup, I know but to avoid unneccessary override and information flow I'd like to integrate that within your system. Each time a new node is added you use this method, right? If that returns true the node is correctly added. So I could directly edit my structure there while adding information where nodes are being added to the graph. Then when a loop closure is detected I can directly edit such information. That means that here I will have my new poses in the new locations and I can then edit (by moving around and eventually delete) my information placing myself here by working in a custom else (so it won't be updated if the LC is rejected). Any comment/suggestion/thought about this? Is this correct?