diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 0ce44b3cdf44c20ff60b84375b77315b56062326..2ba5b779b1d3c5b88389f28a790e80594f61a663 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -104,7 +104,7 @@ float CKf_Based_Terrain_Analysis::computeLikelihood(const pcl::PointXYZRGBNormal
* (exp(-1.0 * ((point_in_sensor_frame.z - z_pred) * (point_in_sensor_frame.z - z_pred)) / (2 * var_z_pred)));
//DEBUG!float likelihood = ((point_in_sensor_frame.z - z_pred) * (point_in_sensor_frame.z - z_pred)) / (2 * var_z_pred);
- if (false) //point_in_sensor_frame.z < -1.75)
+ if (false) //point_in_sensor_frame.z < -1.75)
{
std::cout << "delta_x = " << delta_x << std::endl;
std::cout << "delta_y = " << delta_y << std::endl;
@@ -996,7 +996,18 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// check the Mahalanobis distance and make the actual labeling of the original point cloud
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
+ // 1) We create a vectors to store the features to be used with the Shallow Neural Network for
+ // ground classification
+ std::vector<int> features_indexes_in_pcl_cloud_ptr;
+
+ std::vector<double> squared_distances;
+ std::vector<double> incidence_angles;
+ std::vector<double> intensities;
+ std::vector<double> point_to_reference_squared_distances;
+ std::vector<double> prediction_errors;
+ std::vector<double> scores;
+
+ // 2) We point to the vector of indices related to the elevation_cloud point under evaluation
std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX];
@@ -1004,60 +1015,14 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
point_iterator != index_iterator->end(); ++point_iterator)
{
- // We now copy the data that is common to every point in the vector (because are statistics extracted
- // from the points in the vector)
-
- // in data_n[0] we have the point intensity, so nothing to change
- // in data_n[1] we will store the intensity variance in the elevation cloud cell
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] =
- point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
-
- // in data_n[2] we store the mean z value in the elevation cell
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
-
- // in data_n[3] we store the z variance in the elevation cell
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
- point_in_sensor_frame.data_n[DATA_N_3_Z_VARIANCE];
-
- // TODO: we copy the mean intensity value in the channel reserved for the color, we do this because
- // we have detected a problem with pcl conversions: when passing the point cloud through a ROS topic,
- // channels c2 and c3 get corrupted, so we avoid to use them, we will investigate this problem in the
- // future (hopefully)
- // pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
- // point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] =
- point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
-
-
-// std::cout << "Intensity in elevation cloud: mean = " << point_in_sensor_frame.data_n[DATA_N_0_INTENSITY]
-// << " var = "
-// << point_in_sensor_frame.data_n[DATA_N_1_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_c[DATA_C_0_RGB_CAST_INTO_FLOAT]
-// << " var = " << pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_1_INTENSITY_VARIANCE] << std::endl;
-
- // once we have all the common values stored, only rest to compute the class and score, that will be
- // packaged into the data_c[1] field
float mahalanobis_distance = mahalanobisDistance(reference_in_sensor_frame,
pcl_cloud_ptr->points[*point_iterator]);
-// if (std::isnan(mahalanobis_distance))
-// {
-// std::cout << "posterior pred sigma = " << prediction_std_dev << " uncertainty = "
-// << prediction_std_dev * filtering_configuration.mahalanobis_threshold << " mahalanobis distance = "
-// << mahalanobis_distance << std::endl;
-//
-// std::getchar();
-// }
float score = 0.995 - (mahalanobis_distance / filtering_configuration.mahalanobis_threshold); // we use 0.995 instead of 1.0 to avoid overflowing the class field
if (score < 0.0)
score = 0.0;
- //float score = computeLikelihood(reference_in_sensor_frame, pcl_cloud_ptr->points[*point_iterator]) / computeLikelihood(reference_in_sensor_frame, reference_in_sensor_frame);
-
- if (score > filtering_configuration.score_threshold)
+ if (score > filtering_configuration.score_threshold) // We only use the Neural Network to segment ground points into several classes
{
// TODO: when passing the point cloud through a ROS topic,
// channels c2 and c3 get corrupted, so we avoid to use them, we are temporarily the c0 channel
@@ -1066,6 +1031,27 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
+ pcl::PointXYZRGBNormal point_to_evaluate = pcl_cloud_ptr->points[*point_iterator];
+
+ features_indexes_in_pcl_cloud_ptr.push_back(*point_iterator); // Index to modify the classification in a later step
+
+ squared_distances.push_back(
+ (double)(point_to_evaluate.x * point_to_evaluate.x + point_to_evaluate.y * point_to_evaluate.y
+ + point_to_evaluate.z * point_to_evaluate.z));
+
+ incidence_angles.push_back(0.0); // TODO: Incidence angle w.r.t the plane defined by the ground reference
+ intensities.push_back(point_to_evaluate.data_n[DATA_N_0_INTENSITY]); // Point intensity
+
+ double delta_x = (point_to_evaluate.x - reference_in_sensor_frame.x);
+ double delta_y = (point_to_evaluate.y - reference_in_sensor_frame.y);
+ double delta_z = (point_to_evaluate.z - reference_in_sensor_frame.z);
+
+ point_to_reference_squared_distances.push_back(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z);
+
+ prediction_errors.push_back((double)euclideanDistance(reference_in_sensor_frame, point_to_evaluate)); // Prediction Error
+
+ scores.push_back((double)score);
+
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
}
else
@@ -1099,11 +1085,42 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OVERHANGING_OBSTACLE;
// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OVERHANGING_OBSTACLE;
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE
- + 0.0;
+ pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.0;
}
}
}
+
+ double ratio_ground_points = features_container.size()
+ / correspondence_indexes[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].size();
+
+ double mean_intensity_ground_points_in_cell = 0.0;
+ double std_intensity_ground_points_in_cell = 0.0;
+ double mean_prediction_error = 0.0;
+ double std_prediction_error = 0.0;
+ double mean_squared_error_of_predictions = 0.0;
+ double mean_of_scores = 0.0;
+ double std_of_scores = 0.0;
+
+ float sum = std::accumulate(elevation_cell_vector[i].z_coordinates.begin(),
+ elevation_cell_vector[i].z_coordinates.end(), 0.0);
+ float z_mean = sum / (float)elevation_cell_vector[i].z_coordinates.size();
+ float var = 0;
+ for (int n = 0; n < elevation_cell_vector[i].z_coordinates.size(); ++n)
+ var += ((elevation_cell_vector[i].z_coordinates[n] - z_mean)
+ * (elevation_cell_vector[i].z_coordinates[n] - z_mean));
+
+ var /= (float)elevation_cell_vector[i].z_coordinates.size();
+
+ Eigen::VectorXd features(14, 1);
+
+ features(06, 0) = ratio_ground_points;
+ features(07, 0) = mean_intensity_ground_points_in_cell;
+ features(08, 0) = std_intensity_ground_points_in_cell;
+ features(09, 0) = mean_prediction_error;
+ features(10, 0) = std_prediction_error;
+ features(11, 0) = mean_squared_error_of_predictions;
+ features(12, 0) = mean_of_scores;
+ features(13, 0) = std_of_scores;
}
else // if we don't have enough information to try to predict the ground level at this point
{
@@ -1124,7 +1141,8 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
point_in_sensor_frame.data_n[DATA_N_1_INTENSITY_VARIANCE];
// in data_n[2] we store the mean z value in the elevation cell
- 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_2_Z_MEAN] =
+ point_in_sensor_frame.data_n[DATA_N_2_Z_MEAN];
// in data_n[3] we store the z variance in the elevation cell
pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_3_Z_VARIANCE] =
@@ -1136,8 +1154,8 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// future (hopefully)
// pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_2_MEAN_INTENSITY] =
// point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] =
- point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
+// pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_0_RGB_CAST_INTO_FLOAT] =
+// point_in_sensor_frame.data_n[DATA_N_0_INTENSITY];
if ((pcl_cloud_ptr->points[*point_iterator].z - point_in_sensor_frame.z)
< filtering_configuration.ground_threshold_in_not_analyzed_areas)
@@ -1239,7 +1257,7 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
//std::cout << "Elevation cloud generated! Npoints = " << elevation_cloud_ptr->points.size() << std::endl;
- // This function juit create the first vertex in the graph, using the prior information in the lidar and filtering configuration
+ // This function just creates the first vertex in the graph, using the prior information in the lidar and filtering configuration
// structures.
generateRootVertex(lidar_configuration, filtering_configuration, ground_reference_cloud_ptr);
//std::cout << "Root vertex generated! Num of points = " << ground_reference_cloud_ptr->points.size() << std::endl;