Which files should I modify to implement 2D color map generation from RGB-D images?

[ScottishOwl]

I have been launching RTAB-Map mapping using the Grid/FromSensor true mode. Now, I wish to modify the code to additionally generate a 2D color map from RGB-D images, similar to how RTAB-Map generates a global 2D occupancy grid map from depth images.

However, I’m not entirely clear about which specific functions in which code files are called during the map generation process. Could you please tell me which code files are involved in the process of generating a global 2D occupancy grid map from depth images in RTAB-Map? For implementing my requirement, which functions would I need to modify?

I understand this question might sound a bit trivial or troublesome, so I would be truly grateful if anyone would be kind enough to offer some guidance.

As additional information, I’m providing the mapping command I’ve been using:
ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_args:="--delete_db_on_start --Vis/MinInliers 10 --Mem/IncrementalMemory true --Grid/FromSensor true --Grid/MaxGroundHeight 0.08 --Grid/MaxObstacleHeight 1.5 --Grid/NoiseFilteringRadius 0.05 --Grid/NoiseFilteringMinNeighbors 5 --Grid/RayTracing true --Grid/FootprintLength 0.211 --Grid/FootprintWidth 0.17 --Optimizer/GravitySigma 0.3 --Vis/FeatureType 2" rtabmap_viz:=false rviz:=false rgb_topic:=/camera/camera/color/image_raw depth_topic:=/camera/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/camera/color/camera_info imu_topic:=/imu/data_filtered frame_id:=base_link approx_sync:=true queue_size:=20 wait_imu_to_init:=true qos:=2 imu_sensor_qos:=2 visual_odometry:=true wait_for_transform:=0.2

[matlabbe]

You mean Grid/FromDepth=true (which is now Grid/Sensor=1 for depth image)... but what do you mean by "2D color map from RGB-D images"? Would it be like the 2D occupancy grid but with color?

If you use Grid/3D=true, the colors would be kept in local occupancy grid data of each node in the map. If you want to assemble the local occupancy grids into a 2D grid with colors, you may inherit GlobalMap class and add a new one similar to OccupancyGrid class, but generating your 2D map with color instead. See some implementations of GlobalMap here: https://github.com/introlab/rtabmap/tree/master/corelib/src/global_map (basically you have to implement the abstract function assemble() and generate the map in a local member, then add a public getter to get it). Once you have your new global map, you could add it to MapsManager class along the other ones here, while adding a new publisher for your new map type.

[ScottishOwl]

You mean Grid/FromDepth=true (which is now Grid/Sensor=1 for depth image)... but what do you mean by "2D color map from RGB-D images"? Would it be like the 2D occupancy grid but with color?

If you use Grid/3D=true, the colors would be kept in local occupancy grid data of each node in the map. If you want to assemble the local occupancy grids into a 2D grid with colors, you may inherit GlobalMap class and add a new one similar to OccupancyGrid class, but generating your 2D map with color instead. See some implementations of GlobalMap here: https://github.com/introlab/rtabmap/tree/master/corelib/src/global_map (basically you have to implement the abstract function assemble() and generate the map in a local member, then add a public getter to get it). Once you have your new global map, you could add it to MapsManager class along the other ones here, while adding a new publisher for your new map type.

Thank you very much for your detailed and helpful response! I truly appreciate you taking the time to explain the implementation approach—this gives me a clear direction to proceed.

Following your suggestion, I am trying to inherit the GlobalMap class and create a new map similar to OccupancyGrid, and planning to modify the MapsManager accordingly.

Regarding overriding the assemble() function, I have reviewed the relevant source files but still have some uncertainty about the update mechanism of the cellCount_ variable. Could you kindly explain a bit more about how cellCount_ should be updated during the assembly process? For example, what is the relationship between it and the poses?

Any further guidance on this would be greatly appreciated. Thank you again for your support!

[matlabbe]

cellCount_ is a specific member of OccupancyGrid, and it is also not used anymore. I think we forgot to remove it after we removed GlobalGrid/FullUpdate parameter. It was used for an incremental way to re-update the occupancy grid, which had some flaws.

[ScottishOwl]

cellCount_ is a specific member of OccupancyGrid, and it is also not used anymore. I think we forgot to remove it after we removed GlobalGrid/FullUpdate parameter. It was used for an incremental way to re-update the occupancy grid, which had some flaws.

Thank you very much for the clarification and for pointing out that cellCount_ is essentially deprecated. I sincerely appreciate your patience and guidance.

I apologize for not mentioning this earlier—I am actually working with the 0.22.1-humble version of the code, and I’m modifying based on that release. In this version, the code related to cellCount_ still appears present and may differ from the recent removal you mentioned.

Given that, could you please advise how I should handle cellCount_ in this older version? Should I still keep and update it in my derived class, or can I safely ignore it (or treat it as a legacy member that no longer affects functionality)?

Thank you once again for your continuous support—it is truly invaluable to me.

[ScottishOwl]

You mean Grid/FromDepth=true (which is now Grid/Sensor=1 for depth image)... but what do you mean by "2D color map from RGB-D images"? Would it be like the 2D occupancy grid but with color?

If you use Grid/3D=true, the colors would be kept in local occupancy grid data of each node in the map. If you want to assemble the local occupancy grids into a 2D grid with colors, you may inherit GlobalMap class and add a new one similar to OccupancyGrid class, but generating your 2D map with color instead. See some implementations of GlobalMap here: https://github.com/introlab/rtabmap/tree/master/corelib/src/global_map (basically you have to implement the abstract function assemble() and generate the map in a local member, then add a public getter to get it). Once you have your new global map, you could add it to MapsManager class along the other ones here, while adding a new publisher for your new map type.

Thank you once again for this valuable guidance. I am currently implementing the color 2D map functionality based on your earlier suggestions.

In my approach, I have inherited from the LocalGrid class and overloaded its constructor, adding a public member variable colorCells (intended to be a cv::Mat of type CV_32FC(6)) to be used when assembling the color 2D map in my own assemble() function. However, I have found that this new constructor never gets called, and I am unable to locate where the original constructor of LocalGrid is invoked within the mapping workflow.

Could you kindly point out where in the mapping process the LocalGrid class is initialized? Any insight into this would be greatly appreciated.

Thank you sincerely for your time and support!!!

[matlabbe]

You can safely remove everything about cellCount_ in 0.22.1-humble, to get a slight performance boost at the same time.

A LocalGrid is mainly a format used by LocalGridCache, which is simply a std::map<int, LocalGrid> that would be accessed by the GlobalMap's inherited classes to get corresponding data (ground, obstacle, empty cells) to a node.

In that context, inheriting LocalGrid to a class like LocalGridColor would not be possible because LocalGridCache uses object LocalGrid directly, not its pointer.

You could pass the colors in ground, obstacle or empty point clouds given to LocalGrid directly. For example, if you look at CloudMap class, it does generate a colored point cloud map from the ground and obstacle point clouds from LocalGrid. For example:

rtabmap/corelib/src/global_map/CloudMap.cpp

Line 86 in e23c38f

*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.groundCells), iter->second, 0, 255, 0);

Here the ground cells would have already a color channel, then converted to a RGB point cloud.

[ScottishOwl]

You can safely remove everything about cellCount_ in 0.22.1-humble, to get a slight performance boost at the same time.

A LocalGrid is mainly a format used by LocalGridCache, which is simply a std::map<int, LocalGrid> that would be accessed by the GlobalMap's inherited classes to get corresponding data (ground, obstacle, empty cells) to a node.

In that context, inheriting LocalGrid to a class like LocalGridColor would not be possible because LocalGridCache uses object LocalGrid directly, not its pointer.

You could pass the colors in ground, obstacle or empty point clouds given to LocalGrid directly. For example, if you look at CloudMap class, it doesn't generate a colored point cloud map from the ground and obstacle point clouds from LocalGrid. For example:

rtabmap/corelib/src/global_map/CloudMap.cpp

Line 86 in e23c38f

*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.groundCells), iter->second, 0, 255, 0);
Here the ground cells would have already a color channel, then converted to a RGB point cloud.

Thank you again for your patient reply!
I've managed to generate colored grid maps following these valuable instructions you gave.

You mean Grid/FromDepth=true (which is now Grid/Sensor=1 for depth image)... but what do you mean by "2D color map from RGB-D images"? Would it be like the 2D occupancy grid but with color?

If you use Grid/3D=true, the colors would be kept in local occupancy grid data of each node in the map. If you want to assemble the local occupancy grids into a 2D grid with colors, you may inherit GlobalMap class and add a new one similar to OccupancyGrid class, but generating your 2D map with color instead. See some implementations of GlobalMap here: https://github.com/introlab/rtabmap/tree/master/corelib/src/global_map (basically you have to implement the abstract function assemble() and generate the map in a local member, then add a public getter to get it). Once you have your new global map, you could add it to MapsManager class along the other ones here, while adding a new publisher for your new map type.

And now I am wondering if I can publish this colored map as a ROS topic and visualize it in Rviz2. However, it seems like that Rviz2 cannot accept maps whose type is not int[8].

[matlabbe]

The default Map plugin in rviz only interprets grayscale values. The nav_msgs/OccupancyGrid topic is by default single byte per cell (which is a percentage of occupation). Note that a hack could be to interpret the values 0->100 as a hue-scaled value from blue to red (or vice-versa) and assuming fixed saturation and value level, then in rviz's Map plugin, add a new colormap based on that (similarly to "costmap" colormap). You can find code to convert RGB to HSV (H: 0-360), then you may convert hue to 0-100 scale instead.

Otherwise, to get a colored occupancy grid (3-bytes channel RGB per cell), you may have to create your own topic type and your own rviz plugin to show it.

[ScottishOwl]

The default Map plugin in rviz only interprets grayscale values. The nav_msgs/OccupancyGrid topic is by default single byte per cell (which is a percentage of occupation). Note that a hack could be to interpret the values 0->100 as a hue-scaled value from blue to red (or vice-versa) and assuming fixed saturation and value level, then in rviz's Map plugin, add a new colormap based on that (similarly to "costmap" colormap). You can find code to convert RGB to HSV (H: 0-360), then you may convert hue to 0-100 scale instead.

Otherwise, to get a colored occupancy grid (3-bytes channel RGB per cell), you may have to create your own topic type and your own rviz plugin to show it.

I would like to ask another question regarding my specific RTAB-Map launch command. Based on the command I'm using for mapping, how are the visual sensor and IMU respectively utilized for localization during the mapping process? Specifically, does this command cause RTAB-Map to use visual odometry or visual-inertial odometry?

Here is the launch command I've been using (with certain details omitted for brevity, but I can provide the full command if needed):
ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_args:="--delete_db_on_start --Vis/MinInliers 10 --Mem/IncrementalMemory true --Grid/FromSensor true --Grid/MaxGroundHeight 0.08 --Grid/MaxObstacleHeight 1.5 --Grid/NoiseFilteringRadius 0.05 --Grid/NoiseFilteringMinNeighbors 5 --Grid/RayTracing true --Grid/FootprintLength 0.211 --Grid/FootprintWidth 0.17 --Optimizer/GravitySigma 0.3 --Vis/FeatureType 2" rtabmap_viz:=false rviz:=false rgb_topic:=/camera/camera/color/image_raw depth_topic:=/camera/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/camera/color/camera_info imu_topic:=/imu/data_filtered frame_id:=base_link approx_sync:=true queue_size:=20 wait_imu_to_init:=true qos:=2 imu_sensor_qos:=2 visual_odometry:=true wait_for_transform:=0.2

I understand that RTAB-Map's odometry strategy can be configured through parameters such as Odom/Strategy and imu_fusion. However, I'm not entirely certain how these settings are interpreted based on my specific command line arguments.

Could you please help clarify:

1. In my current configuration, is the IMU data being fused directly into the visual odometry pipeline (i.e., true VIO), or is it used only for gravity alignment or other auxiliary purposes?
2. If I wanted to switch between pure visual odometry and visual-inertial odometry, which parameters would I need to modify in my launch command?

Thank you so much for your time and expertise—your answers have been incredibly helpful for my implementation.