diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index b4647309d6aa7c9f80113c7c050df9a1db8cf0bd..c7f4d3ad5de731dfc797b9eb737ad1c07864601d 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -205,9 +205,10 @@ float CKf_Based_Terrain_Analysis::mahalanobisDistance(const pcl::PointXYZRGBNorm
partial_derivatives(1) = (point_in_sensor_frame.x - reference_in_sensor_frame.x);
partial_derivatives(2) = (point_in_sensor_frame.y - reference_in_sensor_frame.y);
- prediction_std_fast = sqrt(partial_derivatives.transpose()
- * references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]]
- * partial_derivatives);
+ prediction_std_fast = sqrt(
+ partial_derivatives.transpose()
+ * references_covariance_matrix_vector_[reference_in_sensor_frame.data_n[GRND_REF_DATA_N_3_ORIGINAL_INDEX]]
+ * partial_derivatives);
}
float mahalanobis_distance_fast = fabs(point_in_sensor_frame.z - prediction_fast) / prediction_std_fast;
@@ -994,10 +995,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// ground classification
std::vector<int> features_indexes_in_pcl_cloud_ptr;
- std::vector<double> rgb_values; // Currently not in use, but we left it here for future research in sensor fusion
- std::vector<double> hsv_values; // Currently not in use, but we left it here for future research in sensor fusion
- std::vector<double> local_binary_patterns; // Currently not in use, but we left it here for future research in sensor fusion
-
std::vector<double> squared_distances;
std::vector<double> incidence_angles;
std::vector<double> intensities;
@@ -1023,9 +1020,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
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
- // which is the one storing RGB values packed as a float, so colour is disabled at the moment
pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND;
pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
@@ -1038,10 +1032,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
features_indexes_in_pcl_cloud_ptr.push_back(*point_iterator); // Index to modify the classification in a later step
//std::cout << "Index saved = " << *point_iterator << std::endl;
- rgb_values.push_back(point_to_evaluate.data_n[DATA_N_1_INTENSITY_VARIANCE]); // Feature! // Currently not in use, but we left it here for future research in sensor fusion
- hsv_values.push_back(point_to_evaluate.data_n[DATA_N_2_Z_MEAN]); // Feature! // Currently not in use, but we left it here for future research in sensor fusion
- local_binary_patterns.push_back(point_to_evaluate.data_n[DATA_N_3_Z_VARIANCE]); // Feature! // Currently not in use, but we left it here for future research in sensor fusion
-
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)); // Feature!
@@ -1072,20 +1062,12 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
else
{
- //if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
- // < filtering_configuration.robot_height)
if (euclideanDistance(reference_in_sensor_frame, pcl_cloud_ptr->points[*point_iterator])
< filtering_configuration.robot_height)
{
- // 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
- // which is the one storing RGB values packed as a float, so colour is disabled at the moment
pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE;
pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE;
pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
-// pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_KITTI_OBSTACLE;
-// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
-// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
// We don't use score with obstacle points, if they are considered obstacles, we believe that their
// probability of being ground is zero, the class label is used to distinguish between general obstacles
@@ -1096,15 +1078,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
//std::cout << "Overhanging obstacle detected!" << std::endl;
- // 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
- // which is the one storing RGB values packed as a float, so colour is disabled at the moment
pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE;
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].r = R_CLASS_KITTI_OBSTACLE;
-// pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_KITTI_OBSTACLE;
-// pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_KITTI_OBSTACLE;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.0;
}
@@ -1165,48 +1141,19 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
Eigen::VectorXd features(nn_.number_of_features_, 1);
- switch (nn_.number_of_features_)
- {
- case 6:
- features(0, 0) = squared_distances[n];
- features(1, 0) = incidence_angles[n];
- features(2, 0) = scores[n];
- features(3, 0) = ratio_ground_points;
- features(4, 0) = mean_of_scores;
- features(5, 0) = var_of_scores;
- break;
- case 9:
- features(0, 0) = squared_distances[n];
- features(1, 0) = incidence_angles[n];
- features(2, 0) = intensities[n];
- features(3, 0) = scores[n];
- features(4, 0) = ratio_ground_points;
- features(5, 0) = mean_intensity_ground_points_in_cell;
- features(6, 0) = var_intensity_ground_points_in_cell;
- features(7, 0) = mean_of_scores;
- features(8, 0) = var_of_scores;
- break;
-
- case 13:
- features(0, 0) = squared_distances[n];
- features(1, 0) = incidence_angles[n];
- features(2, 0) = intensities[n];
- features(3, 0) = point_to_reference_squared_distances[n];
- features(4, 0) = prediction_errors[n];
- features(5, 0) = scores[n];
- features(6, 0) = ratio_ground_points;
- features(7, 0) = mean_intensity_ground_points_in_cell;
- features(8, 0) = var_intensity_ground_points_in_cell;
- features(9, 0) = mean_prediction_error;
- features(10, 0) = var_prediction_error;
- features(11, 0) = mean_of_scores;
- features(12, 0) = var_of_scores;
- break;
- default:
- assert(
- "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel: Number of features not valid!"
- && false);
- }
+ features(0, 0) = squared_distances[n];
+ features(1, 0) = incidence_angles[n];
+ features(2, 0) = intensities[n];
+ features(3, 0) = point_to_reference_squared_distances[n];
+ features(4, 0) = prediction_errors[n];
+ features(5, 0) = scores[n];
+ features(6, 0) = ratio_ground_points;
+ features(7, 0) = mean_intensity_ground_points_in_cell;
+ features(8, 0) = var_intensity_ground_points_in_cell;
+ features(9, 0) = mean_prediction_error;
+ features(10, 0) = var_prediction_error;
+ features(11, 0) = mean_of_scores;
+ features(12, 0) = var_of_scores;
int predicted_class = nn_.predict(features);
@@ -1336,25 +1283,21 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
if (gt_label != OUTLIER)
{
- Eigen::VectorXd features(17, 1);
-
- features(0, 0) = rgb_values[randomIndex];
- features(1, 0) = hsv_values[randomIndex];
- features(2, 0) = local_binary_patterns[randomIndex];
- features(3, 0) = point_to_read_gt_label.z;
- features(4, 0) = squared_distances[randomIndex];
- features(5, 0) = incidence_angles[randomIndex];
- features(6, 0) = intensities[randomIndex];
- features(7, 0) = point_to_reference_squared_distances[randomIndex];
- features(8, 0) = prediction_errors[randomIndex];
- features(9, 0) = scores[randomIndex];
- features(10, 0) = ratio_ground_points;
- features(11, 0) = mean_intensity_ground_points_in_cell;
- features(12, 0) = var_intensity_ground_points_in_cell;
- features(13, 0) = mean_prediction_error;
- features(14, 0) = var_prediction_error;
- features(15, 0) = mean_of_scores;
- features(16, 0) = var_of_scores;
+ Eigen::VectorXd features(13, 1);
+
+ features(0, 0) = squared_distances[randomIndex];
+ features(1, 0) = incidence_angles[randomIndex];
+ features(2, 0) = intensities[randomIndex];
+ features(3, 0) = point_to_reference_squared_distances[randomIndex];
+ features(4, 0) = prediction_errors[randomIndex];
+ features(5, 0) = scores[randomIndex];
+ features(6, 0) = ratio_ground_points;
+ features(7, 0) = mean_intensity_ground_points_in_cell;
+ features(8, 0) = var_intensity_ground_points_in_cell;
+ features(9, 0) = mean_prediction_error;
+ features(10, 0) = var_prediction_error;
+ features(11, 0) = mean_of_scores;
+ features(12, 0) = var_of_scores;
//std::cout << "features = " << std::endl << features << std::endl;
@@ -1365,8 +1308,7 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
s << features(0, 0) << ", " << features(1, 0) << ", " << features(2, 0) << ", " << features(3, 0) << ", "
<< features(4, 0) << ", " << features(5, 0) << ", " << features(6, 0) << ", " << features(7, 0) << ", "
<< features(8, 0) << ", " << features(9, 0) << ", " << features(10, 0) << ", " << features(11, 0)
- << ", " << features(12, 0) << ", " << features(13, 0) << ", " << features(14, 0) << ", "
- << features(15, 0) << ", " << features(16, 0) << ", " << gt_label << std::endl;
+ << ", " << features(12, 0) << ", " << gt_label << std::endl;
if (features(0, 0) >= 0.0)
dataset_file_ << s.str();
@@ -1404,12 +1346,9 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
< filtering_configuration.ground_threshold_in_not_analyzed_areas)
{
- // 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
- // which is the one storing RGB values packed as a float, so colour is disabled at the moment
- pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND; // 255 // We use instead the r g b channels directly
- pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND; // 0
- pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND; // 255
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_GROUND;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_GROUND;
float score = 0.0; // we don't have too much confidence in these points, because they are not analyzed
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
@@ -1417,18 +1356,18 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
else if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
< filtering_configuration.robot_height)
{
- pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE; // 255
- pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE; // 0
- pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE; // 255
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + score;
}
else
{
- pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE; // 255
- pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OVERHANGING_OBSTACLE; // 0
- pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OVERHANGING_OBSTACLE; // 255
+ pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_OVERHANGING_OBSTACLE;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_OVERHANGING_OBSTACLE;
float score = 0.0;
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE