diff --git a/include/slam_toolbox/laser_utils.hpp b/include/slam_toolbox/laser_utils.hpp
index 56c2e1041..449225b05 100644
--- a/include/slam_toolbox/laser_utils.hpp
+++ b/include/slam_toolbox/laser_utils.hpp
@@ -32,27 +32,39 @@ namespace laser_utils
 {
 
 // Convert a laser scan to a vector of readings
-inline std::vector<double> scanToReadings(
+inline void scanToReadings(
   const sensor_msgs::msg::LaserScan & scan,
+  std::vector<double> & ranges,
+  std::vector<double> & intensities,
   const bool & inverted)
 {
-  std::vector<double> readings;
+
+  size_t n = scan.ranges.size();
+  ranges.resize(n);
+  intensities.resize(n);
+
+  bool has_intensities = scan.intensities.size() == n;
 
   if (inverted) {
-    for (std::vector<float>::const_reverse_iterator it = scan.ranges.rbegin();
-      it != scan.ranges.rend(); ++it)
-    {
-      readings.push_back(*it);
+    for (size_t i = 0; i < n; ++i) {
+      ranges[i] = static_cast<double>(scan.ranges[n - 1 - i]);
+      if (has_intensities){
+        intensities[i] = static_cast<double>(scan.intensities[n - 1 - i]);
+      }else{
+        intensities[i] = 0.0;
+      }
     }
   } else {
-    for (std::vector<float>::const_iterator it = scan.ranges.begin(); it != scan.ranges.end();
-      ++it)
-    {
-      readings.push_back(*it);
+    for (size_t i = 0; i < n; ++i) {
+      ranges[i] = static_cast<double>(scan.ranges[i]);
+      if (has_intensities){
+        intensities[i] = static_cast<double>(scan.intensities[i]);
+      }else{
+        intensities[i] = 0.0;
+      }
     }
   }
-
-  return readings;
+  
 }
 
 // Store laser scanner information
diff --git a/include/slam_toolbox/map_saver.hpp b/include/slam_toolbox/map_saver.hpp
index bd992aa76..cc1e352cf 100644
--- a/include/slam_toolbox/map_saver.hpp
+++ b/include/slam_toolbox/map_saver.hpp
@@ -24,6 +24,10 @@
 #include <chrono>
 #include "rclcpp/rclcpp.hpp"
 #include "slam_toolbox/toolbox_msgs.hpp"
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+
 
 namespace map_saver
 {
@@ -32,20 +36,27 @@ namespace map_saver
 class MapSaver
 {
 public:
-  MapSaver(rclcpp::Node::SharedPtr node, const std::string & service_name);
+  MapSaver(rclcpp::Node::SharedPtr node, const std::string & topic_map_name, const std::string & topic_intensity_map_name);
 
 protected:
   bool saveMapCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<slam_toolbox::srv::SaveMap::Request> request,
     std::shared_ptr<slam_toolbox::srv::SaveMap::Response> response);
+  void bothMapsCallback(
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & map,
+    const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & intensity_map);
 
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Service<slam_toolbox::srv::SaveMap>::SharedPtr server_;
-  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_;
-  std::string service_name_, map_name_;
-  bool received_map_;
+  std::shared_ptr<message_filters::Subscriber<nav_msgs::msg::OccupancyGrid>> map_sub_, intensity_map_sub_;
+
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<nav_msgs::msg::OccupancyGrid, nav_msgs::msg::OccupancyGrid>;
+  std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
+  nav_msgs::msg::OccupancyGrid last_map_, last_intensity_map_;
+  std::string topic_map_name_, topic_intensity_map_name_;
+  bool received_map_, received_intensity_map_;
 };
 
 }  // namespace map_saver
diff --git a/include/slam_toolbox/merge_maps_kinematic.hpp b/include/slam_toolbox/merge_maps_kinematic.hpp
index 931dfd085..2f14ef976 100644
--- a/include/slam_toolbox/merge_maps_kinematic.hpp
+++ b/include/slam_toolbox/merge_maps_kinematic.hpp
@@ -72,7 +72,7 @@ class MergeMapsKinematic : public rclcpp::Node
     visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr feedback);
   void kartoToROSOccupancyGrid(
     const karto::LocalizedRangeScanVector & scans,
-    nav_msgs::srv::GetMap::Response & map);
+    nav_msgs::srv::GetMap::Response & occ_map,nav_msgs::srv::GetMap::Response & int_map);
   void transformScan(LocalizedRangeScansIt iter, tf2::Transform & submap_correction);
 
   // apply transformation to correct pose
@@ -84,6 +84,8 @@ class MergeMapsKinematic : public rclcpp::Node
   // ROS-y-ness
   std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>>> sstmS_;
   std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>>> sstS_;
+  std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>>> sstmIS_;
+  std::vector<std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>>> sstIS_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::MergeMaps>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::AddSubmap>> ssSubmap_;
 
@@ -104,6 +106,8 @@ class MergeMapsKinematic : public rclcpp::Node
   double resolution_;
   int min_pass_through_;
   double occupancy_threshold_;
+  int min_intensity_counter_;
+  std::string intensity_strategy_;
   int num_submaps_;
 };
 
diff --git a/include/slam_toolbox/slam_mapper.hpp b/include/slam_toolbox/slam_mapper.hpp
index a902cab5d..dcb3650fd 100644
--- a/include/slam_toolbox/slam_mapper.hpp
+++ b/include/slam_toolbox/slam_mapper.hpp
@@ -62,6 +62,7 @@ class SMapper
 
 protected:
   std::unique_ptr<karto::Mapper> mapper_;
+
 };
 
 }  // namespace mapper_utils
diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp
index 4e3a5b85f..d634d2464 100644
--- a/include/slam_toolbox/slam_toolbox_common.hpp
+++ b/include/slam_toolbox/slam_toolbox_common.hpp
@@ -134,6 +134,8 @@ class SlamToolbox : public rclcpp::Node
   std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> scan_filter_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> sst_;
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> sstm_;
+  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>> intensity_map_pub_;
+  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::MapMetaData>> intensity_metamap_pub_;
   std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>> pose_pub_;
   std::shared_ptr<rclcpp::Service<nav_msgs::srv::GetMap>> ssMap_;
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::Pause>> ssPauseMeasurements_;
@@ -141,7 +143,7 @@ class SlamToolbox : public rclcpp::Node
   std::shared_ptr<rclcpp::Service<slam_toolbox::srv::DeserializePoseGraph>> ssDesserialize_;
 
   // Storage for ROS parameters
-  std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
+  std::string odom_frame_, map_frame_, base_frame_, map_topic_name_, scan_topic_, intensity_map_topic_name_;
   bool use_map_saver_;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
   std_msgs::msg::Header scan_header;
diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp
index bc053a4e2..af2246dee 100644
--- a/include/slam_toolbox/visualization_utils.hpp
+++ b/include/slam_toolbox/visualization_utils.hpp
@@ -102,7 +102,8 @@ inline visualization_msgs::msg::InteractiveMarker toInteractiveMarker(
 
 inline void toNavMap(
   const karto::OccupancyGrid * occ_grid,
-  nav_msgs::msg::OccupancyGrid & map)
+  nav_msgs::msg::OccupancyGrid & occupancy_map,
+  nav_msgs::msg::OccupancyGrid & intensity_map)
 {
   // Translate to ROS format
   kt_int32s width = occ_grid->GetWidth();
@@ -110,30 +111,51 @@ inline void toNavMap(
   karto::Vector2<kt_double> offset =
     occ_grid->GetCoordinateConverter()->GetOffset();
 
-  if (map.info.width != (unsigned int) width ||
-    map.info.height != (unsigned int) height ||
-    map.info.origin.position.x != offset.GetX() ||
-    map.info.origin.position.y != offset.GetY())
+  intensity_map.header = occupancy_map.header;
+  intensity_map.info = occupancy_map.info;
+
+  if (occupancy_map.info.width != (unsigned int) width ||
+    occupancy_map.info.height != (unsigned int) height ||
+    occupancy_map.info.origin.position.x != offset.GetX() ||
+    occupancy_map.info.origin.position.y != offset.GetY())
+  {
+    occupancy_map.info.origin.position.x = offset.GetX();
+    occupancy_map.info.origin.position.y = offset.GetY();
+    occupancy_map.info.width = width;
+    occupancy_map.info.height = height;
+    occupancy_map.data.resize(occupancy_map.info.width * occupancy_map.info.height);
+  }
+  if (intensity_map.info.width != (unsigned int) width ||
+    intensity_map.info.height != (unsigned int) height ||
+    intensity_map.info.origin.position.x != offset.GetX() ||
+    intensity_map.info.origin.position.y != offset.GetY())
   {
-    map.info.origin.position.x = offset.GetX();
-    map.info.origin.position.y = offset.GetY();
-    map.info.width = width;
-    map.info.height = height;
-    map.data.resize(map.info.width * map.info.height);
+    intensity_map.info.origin.position.x = offset.GetX();
+    intensity_map.info.origin.position.y = offset.GetY();
+    intensity_map.info.width = width;
+    intensity_map.info.height = height;
+    intensity_map.data.resize(intensity_map.info.width * intensity_map.info.height);
   }
+  intensity_map.data.assign(intensity_map.info.width * intensity_map.info.height, 0);
 
   for (kt_int32s y = 0; y < height; y++) {
     for (kt_int32s x = 0; x < width; x++) {
-      kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
-      switch (value) {
+      kt_int8u occupancy = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));      
+      kt_double intensity = occ_grid->GetCellIntensity(karto::Vector2<kt_int32s>(x, y));
+
+      switch (occupancy) {
         case karto::GridStates_Unknown:
-          map.data[MAP_IDX(map.info.width, x, y)] = -1;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = -1;
           break;
         case karto::GridStates_Occupied:
-          map.data[MAP_IDX(map.info.width, x, y)] = 100;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = 100;
+          if(intensity > 0){
+            int norm_intensity = std::clamp(static_cast<int>(std::round(100.0 * (intensity / 255))), 0, 100);
+            intensity_map.data[MAP_IDX(intensity_map.info.width, x, y)] = norm_intensity;
+          }
           break;
         case karto::GridStates_Free:
-          map.data[MAP_IDX(map.info.width, x, y)] = 0;
+          occupancy_map.data[MAP_IDX(occupancy_map.info.width, x, y)] = 0;
           break;
       }
     }
diff --git a/lib/karto_sdk/Authors b/lib/karto_sdk/Authors
index 380058050..e9cff1951 100644
--- a/lib/karto_sdk/Authors
+++ b/lib/karto_sdk/Authors
@@ -4,4 +4,5 @@ Contributors:
 -------------------------------
 Michael A. Eriksen (eriksen@ai.sri.com)
 Benson Limketkai (bensonl@ai.sri.com)
-Steven Macenski (steven.macenski@simberobotics.com)
\ No newline at end of file
+Steven Macenski (steven.macenski@simberobotics.com)
+Ángel Soriano (asoriano@robotnik.es)
\ No newline at end of file
diff --git a/lib/karto_sdk/include/karto_sdk/Karto.h b/lib/karto_sdk/include/karto_sdk/Karto.h
index cfce65d9b..e26eaeb36 100644
--- a/lib/karto_sdk/include/karto_sdk/Karto.h
+++ b/lib/karto_sdk/include/karto_sdk/Karto.h
@@ -3954,6 +3954,44 @@ class KARTO_EXPORT LaserRangeFinder : public Sensor
     }
   }
 
+  /**
+   * Gets this range finder sensor's minimum intensity
+   * @return minimum intensity
+   */
+  inline kt_double GetMinimumIntensity() const
+  {
+    return m_pMinimumIntensity->GetValue();
+  }
+
+  /**
+   * Sets this range finder sensor's minimum intensity
+   * @param minimumIntensity
+   */
+  inline void SetMinimumIntensity(kt_double minimumIntensity)
+  {
+    m_pMinimumIntensity->SetValue(minimumIntensity);
+
+  }
+
+  /**
+   * Gets this range finder sensor's maximum intensity
+   * @return maximum intensity
+   */
+  inline kt_double GetMaximumIntensity() const
+  {
+    return m_pMaximumIntensity->GetValue();
+  }
+
+  /**
+   * Sets this range finder sensor's minimum intensity
+   * @param maximumIntensity
+   */
+  inline void SetMaximumIntensity(kt_double maximumIntensity)
+  {
+    m_pMaximumIntensity->SetValue(maximumIntensity);
+
+  }
+
   /**
    * Gets this range finder sensor's minimum angle
    * @return minimum angle
@@ -4272,6 +4310,9 @@ class KARTO_EXPORT LaserRangeFinder : public Sensor
     m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
     m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
 
+    m_pMinimumIntensity = new Parameter<kt_double>("MinimumIntesity", 0.0, GetParameterManager());
+    m_pMaximumIntensity = new Parameter<kt_double>("MaximumIntensity", 255.0, GetParameterManager());
+
     m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
     m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
 
@@ -4322,9 +4363,11 @@ class KARTO_EXPORT LaserRangeFinder : public Sensor
 
   Parameter<kt_double> * m_pMinimumRange;
   Parameter<kt_double> * m_pMaximumRange;
-
   Parameter<kt_double> * m_pRangeThreshold;
 
+  Parameter<kt_double> * m_pMinimumIntensity;
+  Parameter<kt_double> * m_pMaximumIntensity;
+
   Parameter<kt_bool> * m_pIs360Laser;
 
   ParameterEnum * m_pType;
@@ -4339,6 +4382,8 @@ class KARTO_EXPORT LaserRangeFinder : public Sensor
     if (Archive::is_loading::value) {
       m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
       m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
+      m_pMinimumIntensity = new Parameter<kt_double>("MinimumIntensity", 0.0, GetParameterManager());
+      m_pMaximumIntensity = new Parameter<kt_double>("MaximumIntensity", 255.0, GetParameterManager());
 
       m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
       m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
@@ -4360,6 +4405,8 @@ class KARTO_EXPORT LaserRangeFinder : public Sensor
     ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution);
     ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange);
     ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange);
+    ar & BOOST_SERIALIZATION_NVP(m_pMaximumIntensity);
+    ar & BOOST_SERIALIZATION_NVP(m_pMinimumIntensity);
     ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold);
     ar & BOOST_SERIALIZATION_NVP(m_pIs360Laser);
     ar & BOOST_SERIALIZATION_NVP(m_pType);
@@ -5191,6 +5238,10 @@ class KARTO_EXPORT SensorData : public Object
  * Type declaration of range readings vector
  */
 typedef std::vector<kt_double> RangeReadingsVector;
+/**
+ * Type declaration of reflected readings vector
+ */
+typedef std::vector<kt_double> IntensityReadingsVector;
 
 /**
  * LaserRangeScan representing the range readings from a laser range finder sensor.
@@ -5210,7 +5261,8 @@ class LaserRangeScan : public SensorData
   LaserRangeScan(const Name & rSensorName)  // NOLINT
   : SensorData(rSensorName),
     m_pRangeReadings(NULL),
-    m_NumberOfRangeReadings(0)
+    m_NumberOfRangeReadings(0),
+    m_pIntensityReadings(NULL)
   {
   }
 
@@ -5226,13 +5278,33 @@ class LaserRangeScan : public SensorData
   LaserRangeScan(const Name & rSensorName, const RangeReadingsVector & rRangeReadings)
   : SensorData(rSensorName),
     m_pRangeReadings(NULL),
-    m_NumberOfRangeReadings(0)
+    m_NumberOfRangeReadings(0),
+    m_pIntensityReadings(NULL)
   {
     assert(rSensorName.ToString() != "");
 
     SetRangeReadings(rRangeReadings);
   }
 
+  /**
+   * Constructs a scan from the given sensor with the given readings
+   * @param rSensorName
+   * @param rRangeReadings
+   * @param IntensityReadingsVector
+   */
+  LaserRangeScan(const Name & rSensorName,
+    const RangeReadingsVector & rRangeReadings,
+    const IntensityReadingsVector & rIntensityReadings)
+  : SensorData(rSensorName),
+    m_pRangeReadings(NULL),
+    m_NumberOfRangeReadings(0),
+    m_pIntensityReadings(NULL)
+  {
+    assert(rSensorName.ToString() != "");
+
+    SetRangeReadings(rRangeReadings);
+    SetIntensityReadings(rIntensityReadings);
+  }
   /**
    * Destructor
    */
@@ -5240,6 +5312,8 @@ class LaserRangeScan : public SensorData
   {
     delete[] m_pRangeReadings;
     m_pRangeReadings = nullptr;
+    delete[] m_pIntensityReadings;
+    m_pIntensityReadings = nullptr;
   }
 
 public:
@@ -5297,6 +5371,56 @@ class LaserRangeScan : public SensorData
     }
   }
 
+  /**
+   * Sets the intensity readings for this scan
+   * @param rIntensityReadings
+   */
+  inline void SetIntensityReadings(const IntensityReadingsVector & rIntensityReadings)
+  {
+
+    std::lock_guard<std::mutex> lock(intensity_mutex_);
+
+    if (!rIntensityReadings.empty())
+    {
+      if (rIntensityReadings.size() != m_NumberOfRangeReadings)
+      {
+        std::stringstream error;
+        error << "Size of intensity array (" << rIntensityReadings.size()
+          << ") does not match with range array size (" << m_NumberOfRangeReadings << ").";
+        std::cerr << "[ERROR] " << error.str() << std::endl;
+        throw Exception(error.str());
+      }
+      
+      if (m_pIntensityReadings != NULL)
+      {
+        std::cerr << "[DEBUG] SetIntensityReadings: deleting previous m_pIntensityReadings." << std::endl;
+        delete[] m_pIntensityReadings;
+        m_pIntensityReadings = NULL;
+      }
+              
+      m_pIntensityReadings = new kt_double[m_NumberOfRangeReadings];
+    
+      for (kt_int32u i = 0; i < m_NumberOfRangeReadings; i++)
+      {
+        m_pIntensityReadings[i] = rIntensityReadings[i];       
+      }
+    }
+    else
+    {
+      // If empty array, delete everything
+      std::cerr << "[DEBUG] SetIntensityReadings: empty array received, deleting m_pIntensityReadings." << std::endl;
+      delete[] m_pIntensityReadings;
+      m_pIntensityReadings = NULL;
+    }
+    
+  }
+
+  inline kt_double * GetIntensityReadings() const
+  {
+  return m_pIntensityReadings;
+  }
+
+
   /**
    * Gets the laser range finder sensor that generated this scan
    * @return laser range finder sensor of this scan
@@ -5322,6 +5446,8 @@ class LaserRangeScan : public SensorData
 private:
   kt_double * m_pRangeReadings;
   kt_int32u m_NumberOfRangeReadings;
+  kt_double * m_pIntensityReadings;
+  std::mutex intensity_mutex_;
 
   friend class boost::serialization::access;
   template<class Archive>
@@ -5332,8 +5458,10 @@ class LaserRangeScan : public SensorData
 
     if (Archive::is_loading::value) {
       m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
+      m_pIntensityReadings = new kt_double[m_NumberOfRangeReadings];
     }
     ar & boost::serialization::make_array<kt_double>(m_pRangeReadings, m_NumberOfRangeReadings);
+    ar & boost::serialization::make_array<kt_double>(m_pIntensityReadings, m_NumberOfRangeReadings);
   }
 };    // LaserRangeScan
 
@@ -5424,6 +5552,14 @@ class LocalizedRangeScan : public LaserRangeScan
     m_IsDirty(true)
   {
   }
+  /**
+   * Constructs a range scan from the given range finder with the given readings and intensities
+   */
+  LocalizedRangeScan(const Name & rSensorName, const RangeReadingsVector & rReadings, const IntensityReadingsVector & iReadings)
+  : LaserRangeScan(rSensorName, rReadings, iReadings),
+    m_IsDirty(true)
+  {
+  }
 
   LocalizedRangeScan()
   {}
@@ -5902,14 +6038,19 @@ class OccupancyGrid : public Grid<kt_int8u>
    * @param height
    * @param rOffset
    * @param resolution
+   * @param intensity_strategy
    */
   OccupancyGrid(
     kt_int32s width, kt_int32s height, const Vector2<kt_double> & rOffset,
-    kt_double resolution)
+    kt_double resolution, const std::string & intensity_strategy = "mean")
   : Grid<kt_int8u>(width, height),
     m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)),
     m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)),
-    m_pCellUpdater(NULL)
+    m_pCellUpdater(NULL),
+    m_pIntensityCells(Grid<kt_double>::CreateGrid(0, 0, resolution)),
+    m_pCurrentIntensityValue(Grid<kt_double>::CreateGrid(0, 0, resolution)),
+    m_pIntensityReadingCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution)),
+    m_pIntensityStorageStrategy(new Parameter<std::string>("IntensityStorageStrategy", intensity_strategy))
   {
     m_pCellUpdater = new CellUpdater(this);
 
@@ -5919,6 +6060,8 @@ class OccupancyGrid : public Grid<kt_int8u>
 
     m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
     m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
+    m_pMinIntensityCnt = new Parameter<kt_int32u>("MinIntensityCnt", 1);
+    m_pIntensityStorageStrategy = new Parameter<std::string>("IntensityStorageStrategy", "mean");
 
     GetCoordinateConverter()->SetScale(1.0 / resolution);
     GetCoordinateConverter()->SetOffset(rOffset);
@@ -5936,6 +6079,14 @@ class OccupancyGrid : public Grid<kt_int8u>
 
     delete m_pMinPassThrough;
     delete m_pOccupancyThreshold;
+
+    delete m_pCurrentIntensityValue;
+    delete m_pIntensityCells;
+    delete m_pIntensityReadingCnt;
+
+    delete m_pMinIntensityCnt;
+    delete m_pIntensityStorageStrategy;
+
   }
 
 public:
@@ -5943,10 +6094,14 @@ class OccupancyGrid : public Grid<kt_int8u>
    * Create an occupancy grid from the given scans using the given resolution
    * @param rScans
    * @param resolution
+   * @param min_pass_through
+   * @param occupancy_threshold
+   * @param min_intensity_cnt
+   * @param intensity_strategy
    */
   static OccupancyGrid * CreateFromScans(
     const LocalizedRangeScanVector & rScans,
-    kt_double resolution, kt_int32u min_pass_through, kt_double occupancy_threshold)
+    kt_double resolution, kt_int32u min_pass_through, kt_double occupancy_threshold, kt_int32u min_intensity_cnt, std::string intensity_strategy)
   {
     if (rScans.empty()) {
       return NULL;
@@ -5958,6 +6113,8 @@ class OccupancyGrid : public Grid<kt_int8u>
     OccupancyGrid * pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
     pOccupancyGrid->SetMinPassThrough(min_pass_through); 
     pOccupancyGrid->SetOccupancyThreshold(occupancy_threshold); 
+    pOccupancyGrid->SetMinIntensityCnt(min_intensity_cnt);
+    pOccupancyGrid->SetIntensityStorageStrategy(intensity_strategy);
     pOccupancyGrid->CreateFromScans(rScans);
 
     return pOccupancyGrid;
@@ -5978,6 +6135,10 @@ class OccupancyGrid : public Grid<kt_int8u>
     pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
     pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
     pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
+    pOccupancyGrid->m_pIntensityCells = m_pIntensityCells->Clone();
+    pOccupancyGrid->m_pCurrentIntensityValue = m_pCurrentIntensityValue->Clone();
+    pOccupancyGrid->m_pIntensityReadingCnt = m_pIntensityReadingCnt->Clone();
+    pOccupancyGrid->m_pIntensityStorageStrategy = m_pIntensityStorageStrategy->Clone();
 
     return pOccupancyGrid;
   }
@@ -6059,6 +6220,22 @@ class OccupancyGrid : public Grid<kt_int8u>
     m_pOccupancyThreshold->SetValue(thresh);
   }
 
+  /**
+   * Sets the minimum count of intensity readings in a cell to store it
+   */
+  void SetMinIntensityCnt(kt_int32u count)
+  {
+    m_pMinIntensityCnt->SetValue(count);
+  }
+
+  /**
+   * Sets the strategy of store the intensity readings in a cell
+   */
+  void SetIntensityStorageStrategy(std::string strategy)
+  {
+    m_pIntensityStorageStrategy->SetValue(strategy);
+  }
+
 protected:
   /**
    * Get cell hit grid
@@ -6077,6 +6254,31 @@ class OccupancyGrid : public Grid<kt_int8u>
   {
     return m_pCellPassCnt;
   }
+  /**
+   * Get intensity grid data pointer
+   * @return kt_double *
+   */
+  kt_double* GetIntensityDataPointer() {
+    return m_pIntensityCells->GetDataPointer();
+  }
+  /**
+   * Get intensity grid data pointer
+   * @return kt_double *
+   */
+  const kt_double* GetIntensityDataPointer() const {
+    return m_pIntensityCells->GetDataPointer();
+  }
+
+public:
+  /**
+   * Get intensity value
+   * @return kt_double
+   */
+  kt_double GetCellIntensity(const Vector2<kt_int32s>& rGrid) const
+  {
+    kt_int32s index = GridIndex(rGrid);
+    return m_pIntensityCells->GetDataPointer()[index];
+  }
 
 protected:
   /**
@@ -6125,6 +6327,15 @@ class OccupancyGrid : public Grid<kt_int8u>
     m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
     m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
 
+    m_pIntensityCells->Resize(GetWidth(), GetHeight());
+    m_pIntensityCells->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
+
+    m_pCurrentIntensityValue->Resize(GetWidth(), GetHeight());
+    m_pCurrentIntensityValue->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
+
+    m_pIntensityReadingCnt->Resize(GetWidth(), GetHeight());
+    m_pIntensityReadingCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
+
     const_forEach(LocalizedRangeScanVector, &rScans)
     {
       if (*iter == nullptr) {
@@ -6151,6 +6362,8 @@ class OccupancyGrid : public Grid<kt_int8u>
     kt_double rangeThreshold = laserRangeFinder->GetRangeThreshold();
     kt_double maxRange = laserRangeFinder->GetMaximumRange();
     kt_double minRange = laserRangeFinder->GetMinimumRange();
+    kt_double maxIntensity = laserRangeFinder->GetMaximumIntensity();
+    kt_double minIntensity = laserRangeFinder->GetMinimumIntensity();
 
     Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
     // get scan point readings
@@ -6179,7 +6392,15 @@ class OccupancyGrid : public Grid<kt_int8u>
         point.SetY(scanPosition.GetY() + ratio * dy);
       }
 
-      kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
+      kt_double intensityReading = pScan->GetIntensityReadings()[pointIndex];
+
+      if (intensityReading < minIntensity || std::isnan(intensityReading)) {
+        intensityReading = 0;
+      }else if(intensityReading > maxIntensity){
+        intensityReading = maxIntensity;
+      }
+
+      kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, intensityReading, doUpdate);
       if (!isInMap) {
         isAllInMap = false;
       }
@@ -6196,6 +6417,7 @@ class OccupancyGrid : public Grid<kt_int8u>
    * @param rWorldFrom start position of beam
    * @param rWorldTo end position of beam
    * @param isEndPointValid is the reading within the range threshold?
+   * @param intensityReading intensity reading
    * @param doUpdate whether to update the cells' occupancy status immediately
    * @return returns false if an endpoint fell off the grid, otherwise true
    */
@@ -6203,6 +6425,7 @@ class OccupancyGrid : public Grid<kt_int8u>
     const Vector2<kt_double> & rWorldFrom,
     const Vector2<kt_double> & rWorldTo,
     kt_bool isEndPointValid,
+    kt_double intensityReading,
     kt_bool doUpdate = false)
   {
     assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
@@ -6226,6 +6449,30 @@ class OccupancyGrid : public Grid<kt_int8u>
         pCellPassCntPtr[index]++;
         pCellHitCntPtr[index]++;
 
+        kt_double * pCurrentIntensityValuePtr = m_pCurrentIntensityValue->GetDataPointer();
+        kt_int32u * pIntensityReadingCntPtr = m_pIntensityReadingCnt->GetDataPointer();
+        
+        std::string strategy = m_pIntensityStorageStrategy->GetValue();
+
+        if (strategy == "mean"){
+          pCurrentIntensityValuePtr[index] = (pCurrentIntensityValuePtr[index] * pIntensityReadingCntPtr[index] + 
+            intensityReading) / (pIntensityReadingCntPtr[index] + 1);
+        } else if (strategy == "max"){
+          pCurrentIntensityValuePtr[index] = std::max(pCurrentIntensityValuePtr[index], intensityReading);
+        } else if (strategy == "latest"){
+          pCurrentIntensityValuePtr[index] = intensityReading;
+        } else{
+          static bool warned = false;
+          if(!warned){
+            std::cout << "WARN: The strategy " << strategy << " is not implemented. Using mean online." << std::endl;
+            pCurrentIntensityValuePtr[index] = (pCurrentIntensityValuePtr[index] * pIntensityReadingCntPtr[index] + 
+            intensityReading) / (pIntensityReadingCntPtr[index] + 1);
+            warned = true;
+          }
+        }
+        // increment cell intensity reading count
+        pIntensityReadingCntPtr[index]++;
+
         if (doUpdate) {
           (*m_pCellUpdater)(index);
         }
@@ -6240,14 +6487,20 @@ class OccupancyGrid : public Grid<kt_int8u>
    * @param pCell
    * @param cellPassCnt
    * @param cellHitCnt
+   * @param pCurrentIntensity
+   * @param pCellIntValue
    */
-  virtual void UpdateCell(kt_int8u * pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
+  virtual void UpdateCell(kt_int8u * pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt, kt_int32u cellIntCnt,
+    kt_double * pCurrentIntensity, kt_double * pCellIntValue)
   {
     if (cellPassCnt > m_pMinPassThrough->GetValue()) {
       kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
 
       if (hitRatio > m_pOccupancyThreshold->GetValue()) {
         *pCell = GridStates_Occupied;
+        if (cellIntCnt >= m_pMinIntensityCnt->GetValue()) {
+          *pCellIntValue = *pCurrentIntensity;
+        }
       } else {
         *pCell = GridStates_Free;
       }
@@ -6268,10 +6521,17 @@ class OccupancyGrid : public Grid<kt_int8u>
     kt_int8u * pDataPtr = GetDataPointer();
     kt_int32u * pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
     kt_int32u * pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
+    kt_int32u * pCellIntensityReadingCntPtr = m_pIntensityReadingCnt->GetDataPointer();
+    kt_double * pCellIntensityCurrentValuePtr = m_pCurrentIntensityValue->GetDataPointer();
+    kt_double * pCellIntensityCellsPtr = m_pIntensityCells->GetDataPointer();
 
     kt_int32u nBytes = GetDataSize();
-    for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++) {
-      UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
+    for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++, 
+      pCellIntensityReadingCntPtr++, pCellIntensityCurrentValuePtr++, pCellIntensityCellsPtr++) {      
+      
+      UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr, *pCellIntensityReadingCntPtr, 
+        pCellIntensityCurrentValuePtr, pCellIntensityCellsPtr);
+          
     }
   }
 
@@ -6281,10 +6541,13 @@ class OccupancyGrid : public Grid<kt_int8u>
    * @param height
    */
   virtual void Resize(kt_int32s width, kt_int32s height)
-  {
+  {    
     Grid<kt_int8u>::Resize(width, height);
     m_pCellPassCnt->Resize(width, height);
     m_pCellHitsCnt->Resize(width, height);
+    m_pIntensityCells->Resize(width, height);   
+    m_pCurrentIntensityValue->Resize(width, height);
+    m_pIntensityReadingCnt->Resize(width, height);
   }
 
 protected:
@@ -6298,6 +6561,21 @@ class OccupancyGrid : public Grid<kt_int8u>
    */
   Grid<kt_int32u> * m_pCellHitsCnt;
 
+  /**
+   * Storage of all intensities of the cells
+   */
+  Grid<kt_double> * m_pCurrentIntensityValue;
+
+  /**
+   * Intensities of cells
+   */
+  Grid<kt_double> * m_pIntensityCells;
+
+  /**
+   * Intensities readings of cells counter
+   */
+  Grid<kt_int32u> * m_pIntensityReadingCnt;
+
 private:
   /**
    * Restrict the copy constructor
@@ -6322,6 +6600,15 @@ class OccupancyGrid : public Grid<kt_int8u>
 
   // Minimum ratio of beams hitting cell to beams passing through cell to be marked as occupied
   Parameter<kt_double> * m_pOccupancyThreshold;
+
+  //Minimum counter of intensity readings in a cell to store it
+  Parameter<kt_int32u> * m_pMinIntensityCnt;
+  
+  //String to define the strategy to store intensity values
+  // "mean": calculate the mean online
+  // "max": storage the max reading
+  // "latest": overwrite with the last reading
+  Parameter<std::string> * m_pIntensityStorageStrategy;
 };    // OccupancyGrid
 
 ////////////////////////////////////////////////////////////////////////////////////////
diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
index 137237787..f161d9c66 100644
--- a/lib/karto_sdk/include/karto_sdk/Mapper.h
+++ b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -2364,6 +2364,12 @@ class KARTO_EXPORT Mapper : public Module
   // Minimum ratio of beams hitting cell to beams passing through cell to be marked as occupied
   Parameter<kt_double> * m_pOccupancyThreshold;
 
+  // Minimum intensity readings count to store the intensity value to a cell
+  Parameter<kt_int32u> * m_pMinIntensityCnt;
+
+  // Strategy to store intensity readings in a cell
+  Parameter<std::string> * m_pIntensityStrategy;
+
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
@@ -2411,6 +2417,8 @@ class KARTO_EXPORT Mapper : public Module
 // NOTE: the following two lines are commented out to avoid breaking the serialization of already existing maps
 //    ar & BOOST_SERIALIZATION_NVP(m_pMinPassThrough); 
 //    ar & BOOST_SERIALIZATION_NVP(m_pOccupancyThreshold);
+//    ar & BOOST_SERIALIZATION_NVP(m_pMinIntensityCnt);
+//    ar & BOOST_SERIALIZATION_NVP(m_pIntensityStorageStrategy);
     std::cout << "**Finished serializing Mapper**\n";
   }
 
@@ -2456,6 +2464,8 @@ class KARTO_EXPORT Mapper : public Module
   bool getParamUseResponseExpansion();
   int getParamMinPassThrough();
   double getParamOccupancyThreshold();
+  int getParamMinIntensityCnt();
+  std::string getParamIntensityStrategy();
 
   /* Setters */
   // General Parameters
@@ -2496,6 +2506,8 @@ class KARTO_EXPORT Mapper : public Module
   void setParamUseResponseExpansion(bool b);
   void setParamMinPassThrough(int i);
   void setParamOccupancyThreshold(double d);
+  void setParamMinIntensityCnt(int i);
+  void setParamIntensityStrategy(std::string s);
 };
 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper)
 }  // namespace karto
diff --git a/lib/karto_sdk/src/Karto.cpp b/lib/karto_sdk/src/Karto.cpp
index 46562fc7f..6b72db9f5 100644
--- a/lib/karto_sdk/src/Karto.cpp
+++ b/lib/karto_sdk/src/Karto.cpp
@@ -142,8 +142,11 @@ void CellUpdater::operator()(kt_int32u index)
   kt_int8u * pDataPtr = m_pOccupancyGrid->GetDataPointer();
   kt_int32u * pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
   kt_int32u * pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
+  kt_int32u * pCellIntCnt = m_pOccupancyGrid->m_pIntensityReadingCnt->GetDataPointer();
+  kt_double * pCellCurrentIntValue = m_pOccupancyGrid->m_pCurrentIntensityValue->GetDataPointer();
+  kt_double * pIntensityPtr = m_pOccupancyGrid->m_pIntensityCells->GetDataPointer();
 
-  m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
+  m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index], pCellIntCnt[index], &pCellCurrentIntValue[index], &pIntensityPtr[index]);
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////
diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
index e96940421..abb73ff4d 100644
--- a/lib/karto_sdk/src/Mapper.cpp
+++ b/lib/karto_sdk/src/Mapper.cpp
@@ -2303,6 +2303,16 @@ void Mapper::InitializeParameters()
     "OccupancyThreshold",
     "Minimum ratio of beams hitting cell to beams passing through cell to be marked as occupied",
     0.1, GetParameterManager());
+
+  m_pMinIntensityCnt = new Parameter<kt_int32u>(
+    "MinIntensityCnt",
+    "Minimum intensity readings to store the intensity in a cell",
+    1, GetParameterManager());
+
+  m_pIntensityStrategy = new Parameter<std::string>(
+    "IntensityStrategy",
+    "Strategy to store intensity readings {mean, max, latest}",
+    "mean", GetParameterManager());  
 }
 /* Adding in getters and setters here for easy parameter access */
 
@@ -2469,6 +2479,16 @@ double Mapper::getParamOccupancyThreshold()
   return static_cast<double>(m_pOccupancyThreshold->GetValue());
 }
 
+int Mapper::getParamMinIntensityCnt()
+{
+  return static_cast<int>(m_pMinIntensityCnt->GetValue());
+}
+
+std::string Mapper::getParamIntensityStrategy()
+{
+  return static_cast<std::string>(m_pIntensityStrategy->GetValue());
+}
+
 /* Setters for parameters */
 // General Parameters
 void Mapper::setParamUseScanMatching(bool b)
@@ -2631,6 +2651,15 @@ void Mapper::setParamOccupancyThreshold(double d)
   m_pOccupancyThreshold->SetValue((kt_double)d);
 }
 
+void Mapper::setParamMinIntensityCnt(int i)
+{
+  m_pMinIntensityCnt->SetValue((kt_int32u)i);
+}
+
+void Mapper::setParamIntensityStrategy(std::string s)
+{
+  m_pIntensityStrategy->SetValue((std::string)s);
+}
 
 void Mapper::Initialize(kt_double rangeThreshold)
 {
diff --git a/src/experimental/slam_toolbox_map_and_localization.cpp b/src/experimental/slam_toolbox_map_and_localization.cpp
index 06ccd6975..dc03022f5 100644
--- a/src/experimental/slam_toolbox_map_and_localization.cpp
+++ b/src/experimental/slam_toolbox_map_and_localization.cpp
@@ -82,7 +82,7 @@ void MapAndLocalizationSlamToolbox::toggleMode(bool enable_localization) {
     processor_type_ = PROCESS;
     localization_pose_sub_.reset();
     clear_localization_.reset();
-    map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(), map_name_);
+    map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(), map_topic_name_, intensity_map_topic_name_);
 
     boost::mutex::scoped_lock lock(smapper_mutex_);
     if (smapper_ && !smapper_->getMapper()->GetLocalizationVertices().empty()) {
diff --git a/src/laser_utils.cpp b/src/laser_utils.cpp
index 1e8b2680e..6f1c0c6c9 100644
--- a/src/laser_utils.cpp
+++ b/src/laser_utils.cpp
@@ -160,6 +160,32 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw)
     max_laser_range = scan_.range_max;
   }
   laser->SetRangeThreshold(max_laser_range);
+
+  //Intensities
+  double min_laser_intensity = 0.0;
+   if (!node_->has_parameter("min_laser_intensity")) {
+    node_->declare_parameter("min_laser_intensity", min_laser_intensity);
+  }
+  node_->get_parameter("min_laser_intensity", min_laser_intensity);
+
+  double max_laser_intensity = 255.0;
+   if (!node_->has_parameter("max_laser_intensity")) {
+    node_->declare_parameter("max_laser_intensity", max_laser_intensity);
+  }
+  node_->get_parameter("max_laser_intensity", max_laser_intensity);
+
+  if(min_laser_intensity >= max_laser_intensity || min_laser_intensity < 0){
+    RCLCPP_WARN(node_->get_logger(),
+      "minimum and/or maximum laser intensity setting (min:%.1f max:%.1f) not coherent "
+      "of the used Lidar. Setting Min to 0, Max to 255.", min_laser_intensity, max_laser_intensity);
+    min_laser_intensity = 0.0;
+    max_laser_intensity = 255.0;
+  }
+
+  laser->SetMinimumIntensity(min_laser_intensity);
+  laser->SetMaximumIntensity(max_laser_intensity);
+
+
   return laser;
 }
 
diff --git a/src/map_saver.cpp b/src/map_saver.cpp
index 0633feda4..d4de57176 100644
--- a/src/map_saver.cpp
+++ b/src/map_saver.cpp
@@ -19,27 +19,45 @@
 #include <memory>
 #include <string>
 #include "slam_toolbox/map_saver.hpp"
+#include <rmw/types.h>
 
 namespace map_saver
 {
 
 /*****************************************************************************/
-MapSaver::MapSaver(rclcpp::Node::SharedPtr node, const std::string & map_name)
-: node_(node), map_name_(map_name), received_map_(false)
+MapSaver::MapSaver(rclcpp::Node::SharedPtr node, const std::string & topic_map_name, const std::string & topic_intensity_map_name)
+: node_(node), topic_map_name_(topic_map_name), topic_intensity_map_name_(topic_intensity_map_name), received_map_(false), received_intensity_map_(false)
 /*****************************************************************************/
 {
   server_ = node_->create_service<slam_toolbox::srv::SaveMap>("slam_toolbox/save_map",
       std::bind(&MapSaver::saveMapCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
 
-  auto mapCallback =
-    [this](const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void
-    {
-      received_map_ = true;
-    };
+  map_sub_ = std::make_shared<message_filters::Subscriber<nav_msgs::msg::OccupancyGrid>>(
+    node_.get(), topic_map_name_, rmw_qos_profile_sensor_data);
 
-  sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
-    map_name_, rclcpp::QoS(1), mapCallback);
+  intensity_map_sub_ = std::make_shared<message_filters::Subscriber<nav_msgs::msg::OccupancyGrid>>(
+    node_.get(), topic_intensity_map_name_, rmw_qos_profile_sensor_data);
+
+  sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), *map_sub_, *intensity_map_sub_);
+  sync_->registerCallback(
+      std::bind(&MapSaver::bothMapsCallback, this, std::placeholders::_1, std::placeholders::_2)
+  );
+  if (!node_->has_parameter("map_types_to_save")) {
+    node_->declare_parameter<std::string>("map_types_to_save", "both");
+  }
+}
+
+/*****************************************************************************/
+void MapSaver::bothMapsCallback(
+  const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & map,
+  const nav_msgs::msg::OccupancyGrid::ConstSharedPtr & intensity_map)
+/*****************************************************************************/
+{
+  last_map_ = *map;
+  last_intensity_map_ = *intensity_map;
+  received_map_ = true;
+  received_intensity_map_ = true;
 }
 
 /*****************************************************************************/
@@ -49,43 +67,58 @@ bool MapSaver::saveMapCallback(
   std::shared_ptr<slam_toolbox::srv::SaveMap::Response> response)
 /*****************************************************************************/
 {
-  if (!received_map_) {
-    RCLCPP_WARN(node_->get_logger(),
-      "Map Saver: Cannot save map, no map yet received on topic %s.",
-      map_name_.c_str());
-    response->result = response->RESULT_NO_MAP_RECEIEVD;
-    return false;
+  std::string map_type = "both";
+  node_->get_parameter("map_types_to_save", map_type);
+
+  if (map_type == "occupancy" || map_type == "both") {
+    if (!received_map_) {
+      RCLCPP_WARN(node_->get_logger(),
+        "Map Saver: Cannot save map, no map yet received on topic %s.",
+        topic_map_name_.c_str());
+      response->result = response->RESULT_NO_MAP_RECEIEVD;
+      return false;
+    }
+  }
+  if (map_type == "intensity" || map_type == "both") {
+      if (!received_intensity_map_) {
+        RCLCPP_WARN(node_->get_logger(),
+          "Map Saver: Cannot save map, no map yet received on topic %s.",
+          topic_intensity_map_name_.c_str());
+        response->result = response->RESULT_NO_MAP_RECEIEVD;
+        return false;
+      } 
   }
 
-  const std::string name = req->name.data;
-  std::string set_namespace;
+  const std::string file_map_name = req->name.data.empty() ? "map" : req->name.data;
   const std::string namespace_str = std::string(node_->get_namespace());
-  if (!namespace_str.empty()) {
-    set_namespace = " -r __ns:=" + namespace_str;
+  auto make_cmd = [&](const std::string &topic, const std::string &file) {
+    std::string cmd = "ros2 run nav2_map_server map_saver_cli -f " + file +
+                      " -t " + (namespace_str == "/" ? "" : namespace_str) + topic +
+                      " --ros-args -p map_subscribe_transient_local:=true";
+    return cmd;
+  };
+
+  bool success_occ = false, success_int = false;
+
+  if (map_type == "occupancy" || map_type == "both") { 
+    std::string cmd = make_cmd(topic_map_name_, file_map_name);         
+    success_occ = (system(cmd.c_str()) == 0);
+    rclcpp::sleep_for(std::chrono::seconds(1));
   }
 
-  if (name != "") {
-    RCLCPP_INFO(node_->get_logger(),
-      "SlamToolbox: Saving map as %s.", name.c_str());
-    int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name  + " --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str());
-    if (rc == 0) {
-      response->result = response->RESULT_SUCCESS;
-    } else {
-      response->result = response->RESULT_UNDEFINED_FAILURE;
-    }
-  } else {
-    RCLCPP_INFO(node_->get_logger(),
-      "SlamToolbox: Saving map in current directory.");
-    int rc = system(("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str());
-    if (rc == 0) {
-      response->result = response->RESULT_SUCCESS;
-    } else {
-      response->result = response->RESULT_UNDEFINED_FAILURE;
-    }
+  if (map_type == "intensity" || map_type == "both") {
+    std::string cmd = make_cmd(topic_intensity_map_name_, (file_map_name +"_intensity"));      
+    success_int = (system(cmd.c_str()) == 0);
+    rclcpp::sleep_for(std::chrono::seconds(1));
   }
 
-  rclcpp::sleep_for(std::chrono::seconds(1));
-  return true;
+  bool result = ((map_type == "both" && success_occ && success_int) ||
+                 (map_type == "occupancy" && success_occ) ||
+                 (map_type == "intensity" && success_int));
+
+  response->result = result ? response->RESULT_SUCCESS : response->RESULT_UNDEFINED_FAILURE;
+
+  return false;
 }
 
 }  // namespace map_saver
diff --git a/src/merge_maps_kinematic.cpp b/src/merge_maps_kinematic.cpp
index 2cb52c38e..e6e9fe086 100644
--- a/src/merge_maps_kinematic.cpp
+++ b/src/merge_maps_kinematic.cpp
@@ -37,14 +37,22 @@ void MergeMapsKinematic::configure()
   resolution_ = 0.05;
   min_pass_through_ = 2;
   occupancy_threshold_ = 0.1;
+  min_intensity_counter_ = 1;
+  intensity_strategy_ = "mean";
   resolution_ = this->declare_parameter("resolution", resolution_);
   min_pass_through_ = this->declare_parameter("min_pass_through", min_pass_through_);
   occupancy_threshold_ = this->declare_parameter("occupancy_threshold", occupancy_threshold_);
+  min_intensity_counter_ = this->declare_parameter("min_intensity_counter", min_intensity_counter_);
+  intensity_strategy_ = this->declare_parameter("intensity_strategy", intensity_strategy_);
 
   sstS_.push_back(this->create_publisher<nav_msgs::msg::OccupancyGrid>(
       "/map", rclcpp::QoS(1)));
   sstmS_.push_back(this->create_publisher<nav_msgs::msg::MapMetaData>(
       "/map_metadata", rclcpp::QoS(1)));
+  sstIS_.push_back(this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+      "/map_intensity", rclcpp::QoS(1)));
+  sstmIS_.push_back(this->create_publisher<nav_msgs::msg::MapMetaData>(
+      "/map_intensity_metadata", rclcpp::QoS(1)));
 
   ssMap_ = this->create_service<slam_toolbox::srv::MergeMaps>("slam_toolbox/merge_submaps",
       std::bind(&MergeMapsKinematic::mergeMapCallback, this, std::placeholders::_1,
@@ -104,12 +112,17 @@ bool MergeMapsKinematic::addSubmapCallback(
       "/map_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
   sstmS_.push_back(this->create_publisher<nav_msgs::msg::MapMetaData>(
       "/map_metadata_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
+  sstIS_.push_back(this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+      "/map_intensity_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
+  sstmIS_.push_back(this->create_publisher<nav_msgs::msg::MapMetaData>(
+      "/map_intensity_metadata_" + std::to_string(num_submaps_), rclcpp::QoS(1)));
   sleep(1.0);
 
-  nav_msgs::srv::GetMap::Response map;
-  nav_msgs::msg::OccupancyGrid & og = map.map;
+  nav_msgs::srv::GetMap::Response occ_map, int_map;
+  nav_msgs::msg::OccupancyGrid & og_range = occ_map.map;
+  nav_msgs::msg::OccupancyGrid & og_intensity = int_map.map;
   try {
-    kartoToROSOccupancyGrid(scans, map);
+    kartoToROSOccupancyGrid(scans, occ_map, int_map);
   } catch (const Exception & e) {
     RCLCPP_WARN(get_logger(), "Failed to build grid to add submap, Exception: %s",
       e.GetErrorMessage().c_str());
@@ -118,16 +131,21 @@ bool MergeMapsKinematic::addSubmapCallback(
 
   tf2::Transform transform;
   transform.setIdentity();
-  transform.setOrigin(tf2::Vector3(og.info.origin.position.x +
-    og.info.width * og.info.resolution / 2.0,
-    og.info.origin.position.y + og.info.height * og.info.resolution / 2.0,
+  transform.setOrigin(tf2::Vector3(og_range.info.origin.position.x +
+    og_range.info.width * og_range.info.resolution / 2.0,
+    og_range.info.origin.position.y + og_range.info.height * og_range.info.resolution / 2.0,
     0.));
-  og.info.origin.position.x = -(og.info.width * og.info.resolution / 2.0);
-  og.info.origin.position.y = -(og.info.height * og.info.resolution / 2.0);
-  og.header.stamp = this->now();
-  og.header.frame_id = "map_" + std::to_string(num_submaps_);
-  sstS_[num_submaps_]->publish(og);
-  sstmS_[num_submaps_]->publish(og.info);
+  og_range.info.origin.position.x = -(og_range.info.width * og_range.info.resolution / 2.0);
+  og_range.info.origin.position.y = -(og_range.info.height * og_range.info.resolution / 2.0);
+  og_range.header.stamp = this->now();  
+  og_range.header.frame_id = "map_" + std::to_string(num_submaps_);
+  og_intensity.info = og_range.info;
+  og_intensity.header = og_range.header;
+
+  sstS_[num_submaps_]->publish(og_range);
+  sstmS_[num_submaps_]->publish(og_range.info);
+  sstIS_[num_submaps_]->publish(og_intensity);
+  sstmIS_[num_submaps_]->publish(og_intensity.info);
 
   geometry_msgs::msg::TransformStamped msg;
   msg.transform = tf2::toMsg(transform);
@@ -277,9 +295,9 @@ bool MergeMapsKinematic::mergeMapCallback(
   }
 
   // create the map
-  nav_msgs::srv::GetMap::Response map;
+  nav_msgs::srv::GetMap::Response occ_map, int_map;
   try {
-    kartoToROSOccupancyGrid(transformed_scans, map);
+    kartoToROSOccupancyGrid(transformed_scans, occ_map, int_map);
   } catch (const Exception & e) {
     RCLCPP_WARN(get_logger(),
       "Failed to build grid to merge maps together, Exception: %s",
@@ -287,27 +305,35 @@ bool MergeMapsKinematic::mergeMapCallback(
   }
 
   // publish
-  map.map.header.stamp = this->now();
-  map.map.header.frame_id = "map";
-  sstS_[0]->publish(map.map);
-  sstmS_[0]->publish(map.map.info);
+  occ_map.map.header.stamp = this->now();
+  occ_map.map.header.frame_id = "map";
+  sstS_[0]->publish(occ_map.map);
+  sstmS_[0]->publish(occ_map.map.info);
+
+  int_map.map.header.stamp = this->now();
+  int_map.map.header.frame_id = "map_intensity";
+  sstIS_[0]->publish(int_map.map);
+  sstmIS_[0]->publish(int_map.map.info);
+
   return true;
 }
 
 /*****************************************************************************/
 void MergeMapsKinematic::kartoToROSOccupancyGrid(
   const LocalizedRangeScanVector & scans,
-  nav_msgs::srv::GetMap::Response & map)
+  nav_msgs::srv::GetMap::Response & occupancy_map, nav_msgs::srv::GetMap::Response & intensity_map)
 /*****************************************************************************/
 {
   OccupancyGrid * occ_grid = NULL;
-  occ_grid = OccupancyGrid::CreateFromScans(scans, resolution_, min_pass_through_, occupancy_threshold_);
+  occ_grid = OccupancyGrid::CreateFromScans(scans, resolution_,
+    min_pass_through_, occupancy_threshold_, min_intensity_counter_, intensity_strategy_);
   if (!occ_grid) {
     RCLCPP_INFO(get_logger(),
       "MergeMapsKinematic: Could not make occupancy grid.");
   } else {
-    map.map.info.resolution = resolution_;
-    vis_utils::toNavMap(occ_grid, map.map);
+    occupancy_map.map.info.resolution = resolution_;
+    intensity_map.map.info.resolution = resolution_;
+    vis_utils::toNavMap(occ_grid, occupancy_map.map, intensity_map.map);
   }
 
   delete occ_grid;
diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp
index 5e4dcec7a..9b75e217b 100644
--- a/src/slam_mapper.cpp
+++ b/src/slam_mapper.cpp
@@ -66,7 +66,10 @@ karto::OccupancyGrid * SMapper::getOccupancyGrid(const double & resolution)
   karto::OccupancyGrid * occ_grid = nullptr;
   return karto::OccupancyGrid::CreateFromScans(
     mapper_->GetAllProcessedScans(),
-    resolution, (kt_int32u)mapper_->getParamMinPassThrough(), (kt_double)mapper_->getParamOccupancyThreshold());
+    resolution, (kt_int32u)mapper_->getParamMinPassThrough(),
+    (kt_double)mapper_->getParamOccupancyThreshold(),
+    (kt_int32u)mapper_->getParamMinIntensityCnt(),
+    (std::string)mapper_->getParamIntensityStrategy());
 }
 
 /*****************************************************************************/
@@ -355,7 +358,6 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node)
   node->get_parameter("use_response_expansion", use_response_expansion);
   mapper_->setParamUseResponseExpansion(use_response_expansion);
 
-
   int min_pass_through = 2;
   if (!node->has_parameter("min_pass_through")) {
     node->declare_parameter("min_pass_through", min_pass_through);
@@ -369,6 +371,21 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node)
   }
   node->get_parameter("occupancy_threshold", occupancy_threshold);
   mapper_->setParamOccupancyThreshold(occupancy_threshold);
+
+  int min_intensity_counter = 1;
+  if (!node->has_parameter("min_intensity_counter")) {
+    node->declare_parameter("min_intensity_counter", min_intensity_counter);
+  }
+  node->get_parameter("min_intensity_counter", min_intensity_counter);
+  mapper_->setParamMinIntensityCnt(min_intensity_counter);
+
+  std::string intensity_strategy = "mean";
+  if (!node->has_parameter("intensity_strategy")) {
+    node->declare_parameter("intensity_strategy", intensity_strategy);
+  }
+  node->get_parameter("intensity_strategy", intensity_strategy);
+  mapper_->setParamIntensityStrategy(intensity_strategy);
+
 }
 
 /*****************************************************************************/
diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp
index 77ac6b113..602b6c936 100644
--- a/src/slam_toolbox_common.cpp
+++ b/src/slam_toolbox_common.cpp
@@ -63,7 +63,7 @@ void SlamToolbox::configure()
   scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
   if (use_map_saver_) {
     map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(),
-        map_name_);
+        map_topic_name_, intensity_map_topic_name_);
   }
   closure_assistant_ =
     std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
@@ -143,8 +143,11 @@ void SlamToolbox::setParams()
       "this isn't allowed so it will be set to default value 0.05.");
     resolution_ = 0.05;
   }
-  map_name_ = std::string("/map");
-  map_name_ = this->declare_parameter("map_name", map_name_);
+  map_topic_name_ = std::string("/map");
+  map_topic_name_ = this->declare_parameter("map_name", map_topic_name_);
+
+  intensity_map_topic_name_ = std::string("/intensity_map");
+  intensity_map_topic_name_ = this->declare_parameter("intensity_map_name", intensity_map_topic_name_);
 
   use_map_saver_ = true;
   use_map_saver_ = this->declare_parameter("use_map_saver", use_map_saver_);
@@ -209,10 +212,16 @@ void SlamToolbox::setROSInterfaces()
   pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "pose", 10);
   sst_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
-    map_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+    map_topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
   sstm_ = this->create_publisher<nav_msgs::msg::MapMetaData>(
-    map_name_ + "_metadata",
+    map_topic_name_ + "_metadata",
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+  intensity_map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>(
+    intensity_map_topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+  intensity_metamap_pub_ = this->create_publisher<nav_msgs::msg::MapMetaData>(
+    intensity_map_topic_name_ + "_metadata",
     rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
   ssMap_ = this->create_service<nav_msgs::srv::GetMap>("slam_toolbox/dynamic_map",
       std::bind(&SlamToolbox::mapCallback, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3));
@@ -392,26 +401,37 @@ LaserRangeFinder * SlamToolbox::getLaser(
 bool SlamToolbox::updateMap()
 /*****************************************************************************/
 {
-  if (sst_->get_subscription_count() == 0) {
-    return true;
-  }
-  boost::mutex::scoped_lock lock(smapper_mutex_);
-  OccupancyGrid * occ_grid = smapper_->getOccupancyGrid(resolution_);
-  if (!occ_grid) {
-    return false;
-  }
+  if (sst_->get_subscription_count() > 0 || intensity_map_pub_->get_subscription_count() > 0) {
+    boost::mutex::scoped_lock lock(smapper_mutex_);
+    OccupancyGrid * occ_grid = smapper_->getOccupancyGrid(resolution_);
+    if (!occ_grid) {
+      return false;
+    }
 
-  vis_utils::toNavMap(occ_grid, map_.map);
+    nav_msgs::msg::OccupancyGrid intensity_map;
+    intensity_map.header = map_.map.header;
 
-  // publish map as current
-  map_.map.header.stamp = scan_header.stamp;
-  sst_->publish(
-    std::move(std::make_unique<nav_msgs::msg::OccupancyGrid>(map_.map)));
-  sstm_->publish(
-    std::move(std::make_unique<nav_msgs::msg::MapMetaData>(map_.map.info)));
+    vis_utils::toNavMap(occ_grid, map_.map, intensity_map);
+
+    if (sst_->get_subscription_count() > 0) {
+      // publish map as current
+      map_.map.header.stamp = scan_header.stamp;
+      sst_->publish(
+        std::move(std::make_unique<nav_msgs::msg::OccupancyGrid>(map_.map)));
+      sstm_->publish(
+        std::move(std::make_unique<nav_msgs::msg::MapMetaData>(map_.map.info)));
+    }
+    if (intensity_map_pub_->get_subscription_count() > 0) {
+      intensity_map.header.stamp = scan_header.stamp;
+      intensity_map_pub_->publish(std::make_unique<nav_msgs::msg::OccupancyGrid>(intensity_map));
+      intensity_metamap_pub_->publish(std::make_unique<nav_msgs::msg::MapMetaData>(intensity_map.info));
+    }
+    
+    delete occ_grid;
+    occ_grid = nullptr;
+
+  }
 
-  delete occ_grid;
-  occ_grid = nullptr;
   return true;
 }
 
@@ -481,17 +501,23 @@ LocalizedRangeScan * SlamToolbox::getLocalizedRangeScan(
 /*****************************************************************************/
 {
   // Create a vector of doubles for lib
-  std::vector<kt_double> readings = laser_utils::scanToReadings(
-    *scan, lasers_[scan->header.frame_id].isInverted());
+  std::vector<kt_double> ranges, intensities;
+  laser_utils::scanToReadings(
+    *scan, ranges, intensities, lasers_[scan->header.frame_id].isInverted());
 
   // transform by the reprocessing transform
   tf2::Transform pose_original = smapper_->toTfPose(odom_pose);
   tf2::Transform tf_pose_transformed = reprocessing_transform_ * pose_original;
   Pose2 transformed_pose = smapper_->toKartoPose(tf_pose_transformed);
 
-  // create localized range scan
-  LocalizedRangeScan * range_scan = new LocalizedRangeScan(
-    laser->GetName(), readings);
+  LocalizedRangeScan * range_scan = new LocalizedRangeScan(laser->GetName(), ranges, intensities);
+
+  // Check if the sizes are equal
+  if (range_scan->GetNumberOfRangeReadings() != intensities.size()) {
+    RCLCPP_INFO(get_logger(), "Discrepancy: GetNumberOfRangeReadings() = %u vs intensities.size() = %zu",
+                 range_scan->GetNumberOfRangeReadings(), intensities.size());
+  }
+
   range_scan->SetOdometricPose(transformed_pose);
   range_scan->SetCorrectedPose(transformed_pose);
   range_scan->SetTime(rclcpp::Time(scan->header.stamp).nanoseconds()/1.e9);