Added low obstacle detection algorithm

.gitmodules

@@ -5,3 +5,4 @@
 [submodule "gata_neural_net_training"]
 	path = gata_neural_net_training
 	url = https://gitlab.iri.upc.edu/idelpino/gata_neural_net_training
+        branch = main

cfg/GroundSegmentation.cfg

@@ -78,6 +78,11 @@ gen.add("score_threshold", double_t, 0, "for assigning ground class label: one m
 gen.add("classify_not_labeled_points_as_obstacles", bool_t, 0, "when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle", True);
 gen.add("ground_threshold_in_not_analyzed_areas", double_t, 0, "when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and all the points above it and below this threshold will be classified as ground", 0.3, 0.01, 1.0);
 
+# Low obstacle detection parameters
+gen.add("low_obstacle_min_height", double_t, 0, "obstacle below this height are not considered", 0.05, 0.001, 0.3);
+gen.add("sensor_vertical_resolution_deg", double_t, 0, "to calculate distances at which low obstacles can be detected", 0.333, 0.01, 10.0);
+gen.add("max_navigation_slope", double_t, 0, "slopes greater than this threshold will be considered obstacles", 0.2, 0.01, 1.0);
+
 # visualization and debug parameters
 gen.add("measure_performance", bool_t, 0, "to measure times of inner functions", False);
 gen.add("show_dense_reconstruction", bool_t, 0, "creates dense pointcloud with ground predictions", False);

gata_neural_net_training @ c031cdc8

-Subproject commit 403c8e7df3e2ad59a05f9d824b711d2032cc423c
+Subproject commit c031cdc89cf4d8c9943b858b876bbd3dbc5596fe

include/ground_segmentation_alg_node.h

@@ -148,6 +148,8 @@ private:
 
   void convertOutputsToSuitableFormats(void);
 
+  void detectLowObstacles(float h, float res, float alpha);
+
   // [service attributes]
 
   // [client attributes]

kf_based_terrain_analysis @ 7c5334ad

-Subproject commit 61c1886b8227e98ec5315dfb2f8f05d011f982ff
+Subproject commit 7c5334ad89b0b8f0677640b66844605d07e1fd00

params/iri_ground_segmentation.yaml

@@ -36,7 +36,7 @@ iri_ground_segmentation: {
                                     ## and a big value will increase the number of false ground points)
 
   # Neural Network related parameters
-  use_neural_network: true,
+  use_neural_network: false,
   extract_data_to_external_training_of_the_network: false,
   dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset.csv', # This is the filename with path where the data for neural net training is dumped
   neural_net_filename_with_global_path: '/home/idelpino/iri-lab/ground_segmentation_ws/src/iri_ground_segmentation/shallow_neural_nets/veg_terrain_and_obs_13_features_39_neurons.csv',
@@ -50,6 +50,11 @@ iri_ground_segmentation: {
   classify_not_labeled_points_as_obstacles: true,        ## when a point has no reference satisfying the max_pred_std_dev_for_labelling threshold we can leave as unknown or label it as obstacle
   ground_threshold_in_not_analyzed_areas: 0.1,           ## when it is not possible to make a local analysis of the data, we will use the lowest point (the one in elevation_cloud) as ground height, and
                                                          ## all the points above it and below this threshold will be classified as ground 
+                           
+  # low obstacle detection parameters
+  low_obstacle_min_height: 0.05,
+  sensor_vertical_resolution_deg: 0.333,
+  max_navigation_slope: 0.2,                           
                                                          
   # visualization and debug parameters
   measure_performance: false,            ## (feature still not debugged) Flag to measure number of execution and execution times of the different functions of the algorithm 

src/ground_segmentation_alg_node.cpp

@@ -229,6 +229,27 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
   else
     this->alg_.filtering_configuration_.show_dense_reconstruction = config_.show_dense_reconstruction;
 
+  if (!this->private_node_handle_.getParam("low_obstacle_min_height", this->config_.low_obstacle_min_height))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'low_obstacle_min_height' not found");
+  }
+  else
+    this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
+
+  if (!this->private_node_handle_.getParam("sensor_vertical_resolution_deg", this->config_.sensor_vertical_resolution_deg))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'sensor_vertical_resolution_deg' not found");
+  }
+  else
+    this->alg_.filtering_configuration_.sensor_vertical_resolution_deg = config_.sensor_vertical_resolution_deg;
+
+  if (!this->private_node_handle_.getParam("max_navigation_slope", this->config_.max_navigation_slope))
+  {
+    ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'max_navigation_slope' not found");
+  }
+  else
+    this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
+
   // [init subscribers]
   this->lidar_subscriber_ = this->private_node_handle_.subscribe("lidar_points", 1,
                                                                  &GroundSegmentationAlgNode::cb_lidar, this);
@@ -620,6 +641,100 @@ void GroundSegmentationAlgNode::showPerformanceStatistics(void)
 //  std::cout << "-------------------------------------------------------------------------------" << std::endl;
 }
 
+struct Cell
+{
+  float z_min, z_max;
+  int label = -1; // -1 sin inicializar, 0 transitable, 1 obstáculo
+};
+
+void GroundSegmentationAlgNode::detectLowObstacles(float h, float res_deg, float alpha)
+{
+  std::cout << "Starting detectLowObstacles function!" << std::endl;
+  float res = res_deg * M_PI / 180.0;
+  float r_max = h / alpha;
+  float K = 1 / sin(res);
+  float d_max = K * r_max;
+  float cell_side = r_max / sqrt(8);
+  int num_of_cells = (d_max * 2 / cell_side);
+
+  std::cout << "r_max = "       << r_max << std::endl;
+  std::cout << "K = "            << K << std::endl;
+  std::cout << "d_max = "        << d_max << std::endl;
+  std::cout << "cell_side = "    << cell_side << std::endl;
+  std::cout << "num_of_cells = " << num_of_cells << std::endl;
+
+  std::vector < std::vector < Cell >> grid(num_of_cells, std::vector < Cell > (num_of_cells));
+
+  for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud_.begin(); i != lidar_pcl_cloud_.end(); ++i)
+  {
+    if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
+    {
+      //std::cout << "Analyzing ground point!" << std::endl;
+      int x = i->x / cell_side;
+      int y = i->y / cell_side;
+      //std::cout << "x cell number = " << x << "  y cell number = " << y << std::endl;
+      if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
+      {
+        if (grid[x][y].label == -1)
+        {
+          grid[x][y].z_min = grid[x][y].z_max = i->z;
+          grid[x][y].label = 0;
+        }
+        else
+        {
+          grid[x][y].z_min = std::min(grid[x][y].z_min, i->z);
+          grid[x][y].z_max = std::max(grid[x][y].z_max, i->z);
+        }
+      }
+    }
+  }
+
+  for (int i = 0; i < num_of_cells; ++i)
+  {
+    for (int j = 0; j < num_of_cells; ++j)
+    {
+      if (grid[i][j].label != -1)
+      {
+        for (int dx = -1; dx <= 1; ++dx)
+        {
+          for (int dy = -1; dy <= 1; ++dy)
+          {
+            int nx = i + dx, ny = j + dy;
+            if (nx >= 0 && nx < num_of_cells && ny >= 0 && ny < num_of_cells && grid[nx][ny].label != -1)
+            {
+              if (abs(grid[i][j].z_min - grid[nx][ny].z_max) > h || abs(grid[i][j].z_max - grid[nx][ny].z_min) > h)
+              {
+                grid[i][j].label = 1;
+                dx = dy = 2; // abortar los bucles
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = lidar_pcl_cloud_.begin(); i != lidar_pcl_cloud_.end(); ++i)
+  {
+    if ((int)std::floor(i->data_c[DATA_C_1_ID_CLASS]) == CLASS_GROUND)
+    {
+      int x = i->x / cell_side;
+      int y = i->y / cell_side;
+      if (x >= 0 && x < num_of_cells && y >= 0 && y < num_of_cells)
+      {
+        if (grid[x][y].label == 1)
+        {
+          i->data_c[DATA_C_1_ID_CLASS] == CLASS_LOW_OBSTACLE;
+          i->r = R_CLASS_LOW_OBSTACLE;
+          i->g = G_CLASS_LOW_OBSTACLE;
+          i->b = B_CLASS_LOW_OBSTACLE;
+        }
+        //std::cout << "Low obstacle found!" << std::endl;
+      }
+    }
+  }
+}
+
 void GroundSegmentationAlgNode::mainNodeThread(void)
 {
   if (this->isAllRequiredDataAlreadyReceived())
@@ -635,6 +750,11 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
     //std::cout << "Starting ground segmentation!" << std::endl;
     this->alg_.segmentGround(common_header_, lidar_pcl_cloud_, roadmap_vertices_pcl_cloud_, roadmap_edges_array_,
                              ground_dense_reconstruction_pcl_cloud_, roadmap_MarkerArray_msg_);
+
+    this->detectLowObstacles(this->alg_.filtering_configuration_.low_obstacle_min_height,
+                             this->alg_.filtering_configuration_.sensor_vertical_resolution_deg,
+                             this->alg_.filtering_configuration_.max_navigation_slope);
+
     //std::cout << "Ground segmentation finished!" << std::endl;
     this->alg_.unlock();
     this->convertOutputsToSuitableFormats();
@@ -730,6 +850,11 @@ void GroundSegmentationAlgNode::node_config_update(Config &config, uint32_t leve
   this->alg_.filtering_configuration_.classify_not_labeled_points_as_obstacles =
       config_.classify_not_labeled_points_as_obstacles;
 
+  // Low obstacles detection parameters
+  this->alg_.filtering_configuration_.low_obstacle_min_height = config_.low_obstacle_min_height;
+  this->alg_.filtering_configuration_.sensor_vertical_resolution_deg = config_.sensor_vertical_resolution_deg;
+  this->alg_.filtering_configuration_.max_navigation_slope = config_.max_navigation_slope;
+
   // visualization and debug parameters
   this->alg_.filtering_configuration_.measure_performance = config.measure_performance;
   this->alg_.filtering_configuration_.show_dense_reconstruction = config.show_dense_reconstruction;