Feature: Creation of an Intensity Map #783

asoriano1 · 2025-06-21T06:09:04Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	gazebo simulation of Robotnik RB-VOGUI

Description of contribution in a few bullet points

Adds intensity data processing alongside occupancy in the SLAM pipeline.
Extends internal data structures to store per-cell intensity values alongside occupancy data.
Ensures full backward compatibility when intensity data is not available.
ROS2 service 'save_map' now saves the occupancy and the intensity map with same name but with "_intensity" sufix.

Description of documentation updates required from your changes

New parameter: min_intensity_counter (default: 2).
Parameter to select intensity range: intensity_min, intensity_max (default: 0, 255).
New param intensity_strategy for different strategies to store the intensity data on a cell ("mean", "max", "latest"). (default: "mean")
Optionally update launch/param examples and the README to reflect the new functionality.

Future work that may be required in bullet points

Add automated tests (unit/integration) to verify the correct behavior.
Validate behavior on different robot platforms and real-world datasets with variable intensity profiles because now the asumed range is from 0 to 255.
Define default values for well-known laserscans. As it is defined for minRange and maxRange.

SteveMacenski · 2025-06-23T23:40:57Z

@asoriano1 can you give us some metrics or examples of this in use for how much it improves localization and/or better addresses some situations that it previously didn't do well at?

SteveMacenski

Some preliminary notes

SteveMacenski · 2025-06-26T21:37:20Z

src/intensity_map_saver.cpp

+#include <cstdlib>
+#include "rclcpp/rclcpp.hpp"
+
+namespace intensity_map_saver {


Is there any difference here between this and the map saver? If not, this can be removed and we can just have 2x instances of the map_saver. One for the occ grid and another for the intensity map.

I think actually we should have them both run in the map_saver in 1 instance that way the maps are synchronized

There is no difference between intensity_map_saver and map_saver but map_saver have not parametrize the name of the map, only the name of the topic. This means that if we use two instances of map_saver for different topics, we need to ensure that the output file names are different to avoid overwriting. When the service is called without specifying a map name, this is not an issue: two files are created (e.g., map.pgm and map_2.pgm).
However, if a file name is provided via the service, both savers would try to use the same name, which could lead to one overwriting the other. We could solve this by automatically adding a suffix based on the topic name (the only param we have), but then the final file name would no longer exactly match what the user specified in the service call.
Other option is to add a new parameter (e.g. bool) to MapSaver to specify if is_intensity or not. I'm not sure which approach is the least problematic.

I think adding an argument for which to save could make sense, but I think the best is to save both in 1 instance of the map saver node. That way both are saved close to simultaneously. Then, we can just append _intensity (or whatever) to the service call for the intensity one.

Done with 1 instance of the map saver node at 44a9829.
The service still receives just the "map_name" and it will save occupancy map as "map_name" and intensity map as "map_name_intensity".

include/slam_toolbox/intensity_map_utils.hpp

…ith 4 params

…arameter. MEAN strategy is applied by default

…ED_MEAN fusion strategy

SteveMacenski · 2025-07-01T19:00:31Z

I jsut also noticed this is open against Humble. That's OK, but we will need one on ros2 too so that this can be enabled on all future distributions and backported to Jazzy as well.

SteveMacenski · 2025-07-01T19:02:10Z

include/slam_toolbox/intensity_grid.hpp

+   * @param gridIndex index
+   * @param intensity intensity value (p.e. 0 a 255)
+   */
+  inline void SetIntensity(kt_int32s gridIndex, kt_int8u intensity)


I ... don't actually sett this or the GetIntensity function used anywhere. Should it be?

Removed. The method and the file. Now all is managed by karto lib

SteveMacenski · 2025-07-01T19:05:21Z

src/slam_mapper.cpp

+/*****************************************************************************/
+{
+  // Create the occupancy grid from scans
+  karto::OccupancyGrid * occ_grid = karto::OccupancyGrid::CreateFromScans(


This is very, very inefficient to call twice (once in just getOccupancyGrid above). Remove this function and get the intensity grid in the same getOccupancyGrid function.

solved at 1c8bb3a.
Now everything is processed at Karto lib.

SteveMacenski · 2025-07-01T19:07:11Z

src/slam_mapper.cpp

+  // For each scan update the intensity grid
+  const auto & scans = mapper_->GetAllProcessedScans();
+  for (auto scan : scans) {
+    slam_toolbox::updateIntensityGridFromScan(scan, occ_grid, *intensity_grid, min_intensity_threshold_, 


Can the occ grid creation process not be combined with the intensity grid creation process? Doing this separately after iterating through all the data is very inefficient

solved at 1c8bb3a.
Now everything is processed at Karto lib.

…date the map

…ccupancyGrid class

…aps working

…_intensity_counter parameter for intensity values storage

… and publishes them

…:msg::OccupancyGrid

…ginal map_saver updated to save occupancy and intensity maps

…ensure the range of the intensity values

…ring the intensity in a cell. Options are: mean, max, latest

SteveMacenski · 2025-07-14T21:43:36Z

@asoriano1 let me know if / when you're ready for another review!

…ant prints removed

SteveMacenski reviewed Jun 26, 2025

View reviewed changes

asoriano1 added 3 commits June 28, 2025 23:44

now a base intentisy map is created when mapping

ef66f59

minimum intensity value parametrized as parameter. It was hardcoded

1e539a4

typo corrected in print

8d22542

asoriano1 force-pushed the feature-intensities branch from b68f08f to 8d22542 Compare June 28, 2025 21:48

asoriano1 added 4 commits June 29, 2025 00:16

getIntensityGrid updated to call the new CreateFromScans definition w…

688af96

…ith 4 params

Limit intensity values to 255 to prevent overflow in intensity map

9e9cabc

added different strategies for fusion the intensity data. Through a p…

cf5172d

…arameter. MEAN strategy is applied by default

added intensity_weighted_mean_alpha (def:0.8) as parameter for WEIGHT…

ed6bed6

…ED_MEAN fusion strategy

SteveMacenski reviewed Jul 1, 2025

View reviewed changes

asoriano1 added 14 commits July 3, 2025 13:01

removed unused GetIntensity function

654ff22

removed unused SetIntensity function

2921ccc

merging get occupancy and intensity grid into just one function to up…

58e27d7

…date the map

karto_sdk updated to store and manage intensity laser values inside O…

1c8bb3a

…ccupancyGrid class

updated methods and signs from karto to keep the merge of occupancy m…

70d2560

…aps working

removed unused old intensity methods(now are at lib/karto), added min…

d02bab8

…_intensity_counter parameter for intensity values storage

updateMap now calls toNavMap() method for occupancy and intensity map…

80b1e1e

… and publishes them

toNavMap method updated to convert also intensity values to nav_msgs:…

7880f85

…:msg::OccupancyGrid

removed empty spaces to clean the code

0a6292f

removed files and code for independent map_saver for intensities. Ori…

44a9829

…ginal map_saver updated to save occupancy and intensity maps

added minimumIntensity and maximumIntensity parameters to define and …

41acdd5

…ensure the range of the intensity values

delete redundant intensity initialization

8d6eaae

added different strategies parametrized as intensity_strategy for sto…

6d9bb10

…ring the intensity in a cell. Options are: mean, max, latest

merge_maps_kinematics now publishes the intensity map merged

6cc5116

karto.h typo checking the saturated intensity value. >= changed to >

51ed3e0

asoriano1 marked this pull request as draft July 15, 2025 12:46

OccupancyGrid:: IntensityReadingVector unused function and some const…

39f540f

…ant prints removed

asoriano1 added 2 commits July 22, 2025 13:55

scanToReadings() modified to get ranges and intensities at the same time

06266ea

min and max laser intensity params read and set from ROS parameter

74c9a8c

Feature: Creation of an Intensity Map #783

Are you sure you want to change the base?

Feature: Creation of an Intensity Map #783

Uh oh!

Conversation

asoriano1 commented Jun 21, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

Uh oh!

SteveMacenski commented Jun 23, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Jul 1, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Jul 14, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

asoriano1 commented Jun 21, 2025 •

edited

Loading