added includes from travel algorithm

params/iri_ground_segmentation.yaml

@@ -6,11 +6,11 @@ iri_ground_segmentation: {
   robot_height: 2.00,       ## All obstacle points above this value (w.r.t the z ground predicted by the algorithm) are treated as overhanging obstacles 
 
   # Parameters affecting the exploration of the pointcloud
-  ROI_delta_x_and_y: 3.0,    ## This value sets the size of the ROIs: ROI_size = (2*ROI_delta_x_and_y)^2
-  ROI_shadow_area: 7.0, #5.5, #6.0,     ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
+  ROI_delta_x_and_y: 3.0, #4.0,   ## This value sets the size of the ROIs: ROI_size = (2*ROI_delta_x_and_y)^2
+  ROI_shadow_area:   7.0, #6.5,   ## This value is the same that the above, but only used in the root vertex to overcome the shadow area
 
-  ground_reference_search_resolution_deg: 40.0, #6.0, #40.0, #20.0, #40 #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
-                                                        ## it will affect the number of nodes in the graph: lower values generate more nodes
+  ground_reference_search_resolution_deg: 12, #40.0, #6.0, #12.00, ## It is the angular resolution when generating new ground references (graph nodes),
+                                                              ## it will affect the number of nodes in the graph: lower values generate more nodes
 
   elevation_grid_resolution: 2.1, #1.2, #1.5, #1.0 #0.5,   ## This value is used to create the "elevation point cloud" which is a reduction of the original pointcloud, where
                                           ## only the lowest z values in each cell are stored (along with a vector of indexes pointing to the remaining points
@@ -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: false,
+  use_neural_network: true,
   extract_data_to_external_training_of_the_network: false,
   dataset_filename_with_global_path: '/home/idelpino/Documentos/dataset_rgb_hsv_olbp_10_frame_inc.csv',
   neural_net_filename_with_global_path: '/media/sf_virtual_box_shared/neural_networks/veg_terrain_and_obs_13_features_39_neurons.csv', #five_classes_13_features_39_neurons.csv'
@@ -45,14 +45,14 @@ iri_ground_segmentation: {
   neural_net_number_of_neurons_in_output_layer: 2,
   
   # labeling parameters
-  max_pred_std_dev_for_labelling: 0.5,                   ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
+  max_pred_std_dev_for_labelling: 0.333,                 ## ONLY IN USE TO GIVE COLOUR TO DENSE RECONSTRUCTION
   score_threshold: 0.0,                                  ## for assigning ground class label: one means maha. dist. equal to zero, zero means mahalanobis dist equal to maha. thres
-  classify_not_labeled_points_as_obstacles: false,        ## 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
+  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 
                                                          
   # 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 
-  show_dense_reconstruction: true,      ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)   
+  show_dense_reconstruction: false,      ## To show a dense ground surface reconstruction using the predictions of the ground mode (colored using the std_dev of z coordinate)   
                                          ## or alternatively the "elevation point cloud" (useful for parameter tunning) 
 }

src/ground_segmentation_alg.cpp

@@ -199,9 +199,9 @@ void GroundSegmentationAlgorithm::segmentGround(
       vertex.pose.orientation.z = 0.0;
       vertex.pose.orientation.w = 1.0;
 
-      vertex.scale.x = 0.15;
-      vertex.scale.y = 0.15;
-      vertex.scale.z = 0.15;
+      vertex.scale.x = 0.25;
+      vertex.scale.y = 0.25;
+      vertex.scale.z = 0.25;
 
       vertex.color.a = 1.0; // Don't forget to set the alpha!
       if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
@@ -226,11 +226,14 @@ void GroundSegmentationAlgorithm::segmentGround(
       {
         edges_temp.edges.push_back(*e);
 
+        pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
+
         visualization_msgs::Marker edge;
         edge.action = visualization_msgs::Marker::ADD;
         edge.header = common_header;
 
-        if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+        if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT &&
+            child_ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
         {
           edge.ns = "edges";
           edge.id = edge_id++;
@@ -249,21 +252,21 @@ void GroundSegmentationAlgorithm::segmentGround(
         p.z = ground_ref.z;
         edge.points.push_back(p);
 
-        pcl::PointXYZRGBNormal child_ground_ref = roadmap_vertices_pcl_cloud.points[*e];
         p.x = child_ground_ref.x;
         p.y = child_ground_ref.y;
         p.z = child_ground_ref.z;
         edge.points.push_back(p);
 
-        edge.scale.x = 0.05;
+        edge.scale.x = 0.1;
 
         edge.pose.orientation.x = 0.0;
         edge.pose.orientation.y = 0.0;
         edge.pose.orientation.z = 0.0;
         edge.pose.orientation.w = 1.0;
 
-        edge.color.a = 0.7; // Don't forget to set the alpha!
-        if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
+        edge.color.a = 1.0; // Don't forget to set the alpha!
+        if (ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT &&
+            child_ground_ref.data_n[GRND_REF_DATA_N_2_VERTEX_CLASS] == VERTEX_CONNECTED_TO_ROOT)
         {
           edge.color.r = 0.8;
           edge.color.g = 0.53;
@@ -308,14 +311,14 @@ void GroundSegmentationAlgorithm::segmentGround(
         p.z = child_ground_ref.z;
         discarded_edge.points.push_back(p);
 
-        discarded_edge.scale.x = 0.05;
+        discarded_edge.scale.x = 0.1;
 
         discarded_edge.pose.orientation.x = 0.0;
         discarded_edge.pose.orientation.y = 0.0;
         discarded_edge.pose.orientation.z = 0.0;
         discarded_edge.pose.orientation.w = 1.0;
 
-        discarded_edge.color.a = 0.7;
+        discarded_edge.color.a = 1.0;
         switch (discarded_edge_index_and_class.edge_class)
         {
           case CLASS_EDGE_DISCARDED_TOO_MUCH_MAHALANOBIS_DISTANCE: