added field for mean intensity

include/structs_definitions.h

@@ -14,8 +14,8 @@ const float OUT_OF_RANGE = 1000.0;
 const float MAX_RANGE = 100.0;
 
 const int DATA_N_0_INTENSITY = 0;
-const int DATA_N_1_Z_GROUND = 1;
 
+const int DATA_N_1_Z_GROUND = 1;
 const int DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE = 1;
 
 const int DATA_N_2_Z_MEAN = 2;
@@ -23,7 +23,10 @@ const int DATA_N_3_Z_VARIANCE = 3;
 
 const int DATA_C_0_RGB_CAST_INTO_FLOAT = 0;
 const int DATA_C_1_ID_CLASS = 1;
+
 const int DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL = 2;
+const int DATA_C_2_MEAN_INTENSITY = 2;
+
 const int DATA_C_3_ORIGINAL_INDEX = 3;
 
 const int DATA_C_2_CAR_PROB = 2; // TODO: remove this

src/kf_based_terrain_analysis.cpp

@@ -206,7 +206,7 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm
 
   float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast;
 
-  if (std::isnan (mahalanobis_distance_fast))
+  if (std::isnan(mahalanobis_distance_fast))
   {
     std::cout << "ref (x,y,z)   = " << reference_in_sensor_frame.x << ", " << reference_in_sensor_frame.y << ", "
         << reference_in_sensor_frame.z << std::endl;
@@ -1066,6 +1066,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
   //CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
   //std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
   //    << std::endl;
+
   for (pcl::PointCloud<pcl::PointXYZRGBNormal>::iterator i = elevation_cloud_ptr->begin();
       i != elevation_cloud_ptr->end(); ++i)
   {
@@ -1076,6 +1077,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
 
     if (reference_index != INDEX_UNKNOWN)
     {
+      bool flag_reduce_mahalanobis_thres_if_an_obstacle_is_found = false;  // TODO: Extract parameters!
+      bool flag_obstacle_found = false;
+
       pcl::PointXYZRGBNormal reference_in_sensor_frame = ground_reference_cloud_ptr->points[reference_index];
 
       // 1) We point to the vector of indices related to the elevation_cloud point under evaluation
@@ -1096,8 +1100,14 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
 //
 //          std::getchar();
 //        }
+
+        //TODO: extract parameters!
+        float maha_thres = filtering_configuration.mahalanobis_threshold;
+        if(flag_reduce_mahalanobis_thres_if_an_obstacle_is_found && flag_obstacle_found)
+          maha_thres = maha_thres * 0.85;
+
         // TODO: check this!
-        pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY] =
+        pcl_cloud_ptr->points[*point_iterator].data_n[DATA_C_2_MEAN_INTENSITY] =
             point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
             point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];
@@ -1107,20 +1117,18 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
 //            << point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]
 //            << std::endl;
 //
-//        std::cout << "Intensity in pcl cloud: mean = " << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY]
-//            << "    var = "
-//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE]
-//            << std::endl;
+//        std::cout << "Intensity in pcl cloud: point = "
+//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY] << "    mean = "
+//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_C_2_MEAN_INTENSITY] << "    var = "
+//            << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] << std::endl;
 
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
         pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
             point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
 
         //sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) < 0.06) TODO: Could we use this variance to penalize uneven terrain?
-        float score = 0.995 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold); // we use 0.995 instead of 1.0 to avoid overflowing the class field
+        float score = 0.995 - (mahalanobis_distance / maha_thres); // we use 0.995 instead of 1.0 to avoid overflowing the class field
 
-        //if (sqrt(reference_in_sensor_frame.data_c[GRND_REF_DATA_C_3_Z_VARIANCE]) > 0.05)
-        //  score = 0.0;
 
         if (score < 0.0)
           score = 0.0;
@@ -1151,6 +1159,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
             pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
 
             pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + score;
+
+            if (flag_reduce_mahalanobis_thres_if_an_obstacle_is_found)
+              flag_obstacle_found = true;
           }
           else
           {
@@ -1176,7 +1187,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
             point_iterator != index_iterator->end(); ++point_iterator)
         {
           //pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_Z_GROUND] = point_in_sensor_frame.z;
-          pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_0_INTENSITY] =
+          pcl_cloud_ptr->points[*point_iterator].data_n[DATA_C_2_MEAN_INTENSITY] =
               point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
           pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE] =
               point_in_sensor_frame.data_n[DATA_N_1_ELEVATION_CLOUD_INTENSITY_VARIANCE];