feat: adds min_pass_through and occupancy_threshold ros parameters

lib/karto_sdk/include/karto_sdk/Karto.h

@@ -5946,7 +5946,7 @@ class OccupancyGrid : public Grid<kt_int8u>
    */
   static OccupancyGrid * CreateFromScans(
     const LocalizedRangeScanVector & rScans,
-    kt_double resolution)
+    kt_double resolution, kt_int32u min_pass_through, kt_double occupancy_threshold)
   {
     if (rScans.empty()) {
       return NULL;
@@ -5956,6 +5956,8 @@ class OccupancyGrid : public Grid<kt_int8u>
     Vector2<kt_double> offset;
     ComputeDimensions(rScans, resolution, width, height, offset);
     OccupancyGrid * pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
+    pOccupancyGrid->SetMinPassThrough(min_pass_through); 
+    pOccupancyGrid->SetOccupancyThreshold(occupancy_threshold); 
     pOccupancyGrid->CreateFromScans(rScans);
 
     return pOccupancyGrid;

lib/karto_sdk/include/karto_sdk/Mapper.h

@@ -2357,6 +2357,13 @@ class KARTO_EXPORT Mapper : public Module
   // whether to increase the search space if no good matches are initially found
   Parameter<kt_bool> * m_pUseResponseExpansion;
 
+  // Number of beams that must pass through a cell before it will be considered to be occupied 
+  // or unoccupied.  This prevents stray beams from messing up the map. 
+  Parameter<kt_int32u> * m_pMinPassThrough;
+
+  // Minimum ratio of beams hitting cell to beams passing through cell to be marked as occupied
+  Parameter<kt_double> * m_pOccupancyThreshold;
+
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
@@ -2401,6 +2408,8 @@ class KARTO_EXPORT Mapper : public Module
     ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty);
     ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
     ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
+    ar & BOOST_SERIALIZATION_NVP(m_pMinPassThrough);
+    ar & BOOST_SERIALIZATION_NVP(m_pOccupancyThreshold);
     std::cout << "**Finished serializing Mapper**\n";
   }
 
@@ -2444,6 +2453,8 @@ class KARTO_EXPORT Mapper : public Module
   double getParamMinimumAnglePenalty();
   double getParamMinimumDistancePenalty();
   bool getParamUseResponseExpansion();
+  int getParamMinPassThrough();
+  double getParamOccupancyThreshold();
 
   /* Setters */
   // General Parameters
@@ -2482,6 +2493,8 @@ class KARTO_EXPORT Mapper : public Module
   void setParamMinimumAnglePenalty(double d);
   void setParamMinimumDistancePenalty(double d);
   void setParamUseResponseExpansion(bool b);
+  void setParamMinPassThrough(int i);
+  void setParamOccupancyThreshold(double d);
 };
 BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper)
 }  // namespace karto

lib/karto_sdk/src/Mapper.cpp

@@ -2285,12 +2285,24 @@ void Mapper::InitializeParameters()
     "Minimum value of the distance penalty multiplier so scores do not "
     "become too small.",
     0.5, GetParameterManager());
-
+  
   m_pUseResponseExpansion = new Parameter<kt_bool>(
     "UseResponseExpansion",
     "Whether to increase the search space if no good matches are initially "
     "found.",
     false, GetParameterManager());
+
+  m_pMinPassThrough = new Parameter<kt_int32u>(
+    "MinPassThrough",
+    "Number of beams that must pass through a cell before it will be considered to be occupied "
+    "or unoccupied.  This prevents stray beams from messing up the map. "
+    "found.",
+    2, GetParameterManager());
+
+  m_pOccupancyThreshold = new Parameter<kt_double>(
+    "OccupancyThreshold",
+    "Minimum ratio of beams hitting cell to beams passing through cell to be marked as occupied",
+    0.1, GetParameterManager());
 }
 /* Adding in getters and setters here for easy parameter access */
 
@@ -2447,6 +2459,16 @@ bool Mapper::getParamUseResponseExpansion()
   return static_cast<bool>(m_pUseResponseExpansion->GetValue());
 }
 
+int Mapper::getParamMinPassThrough()
+{
+  return static_cast<int>(m_pMinPassThrough->GetValue());
+}
+
+double Mapper::getParamOccupancyThreshold()
+{
+  return static_cast<double>(m_pOccupancyThreshold->GetValue());
+}
+
 /* Setters for parameters */
 // General Parameters
 void Mapper::setParamUseScanMatching(bool b)
@@ -2599,6 +2621,16 @@ void Mapper::setParamUseResponseExpansion(bool b)
   m_pUseResponseExpansion->SetValue((kt_bool)b);
 }
 
+void Mapper::setParamMinPassThrough(int i)
+{
+  m_pMinPassThrough->SetValue((kt_int32u)i);
+}
+
+void Mapper::setParamOccupancyThreshold(double d)
+{
+  m_pOccupancyThreshold->SetValue((kt_double)d);
+}
+
 
 void Mapper::Initialize(kt_double rangeThreshold)
 {

src/merge_maps_kinematic.cpp

@@ -297,7 +297,7 @@ void MergeMapsKinematic::kartoToROSOccupancyGrid(
 /*****************************************************************************/
 {
   OccupancyGrid * occ_grid = NULL;
-  occ_grid = OccupancyGrid::CreateFromScans(scans, resolution_);
+  occ_grid = OccupancyGrid::CreateFromScans(scans, resolution_, 2, 0.1);
   if (!occ_grid) {
     RCLCPP_INFO(get_logger(),
       "MergeMapsKinematic: Could not make occupancy grid.");

src/slam_mapper.cpp

@@ -66,7 +66,7 @@ karto::OccupancyGrid * SMapper::getOccupancyGrid(const double & resolution)
   karto::OccupancyGrid * occ_grid = nullptr;
   return karto::OccupancyGrid::CreateFromScans(
     mapper_->GetAllProcessedScans(),
-    resolution);
+    resolution, (kt_int32u)mapper_->getParamMinPassThrough(), (kt_double)mapper_->getParamOccupancyThreshold());
 }
 
 /*****************************************************************************/
@@ -354,6 +354,21 @@ 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);
+  }
+  node->get_parameter("min_pass_through", min_pass_through);
+  mapper_->setParamMinPassThrough(min_pass_through);
+
+  double occupancy_threshold = 0.1;
+  if (!node->has_parameter("occupancy_threshold")) {
+    node->declare_parameter("occupancy_threshold", occupancy_threshold);
+  }
+  node->get_parameter("occupancy_threshold", occupancy_threshold);
+  mapper_->setParamOccupancyThreshold(occupancy_threshold);
 }
 
 /*****************************************************************************/