diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 8a540f60c24717f44307f061fd02d0150b74f837..f853cfffde498fedf78bebaf7372cbd2364565f9 100644
--- a/include/structs_definitions.h
+++ b/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
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 8772ab3c673291b89db9743ec346333601308ceb..78c9e9dc96344d61a06860ffbc4b59c6d186f381 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/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];