From 61c1886b8227e98ec5315dfb2f8f05d011f982ff Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Thu, 18 Apr 2024 12:01:11 +0200
Subject: [PATCH] Corrected error-> it is not possible to save the dataset AND
use the algorithm simultaneously
---
src/kf_based_terrain_analysis.cpp | 154 ++++++++++++++++--------------
1 file changed, 82 insertions(+), 72 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 2a6a78c..8ad9cb0 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1020,10 +1020,12 @@ 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
{
- 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;
-
+ if (!filtering_configuration.extract_data_to_external_training_of_the_network) // We dont want to change the ground truth when saving the dataset!
+ {
+ 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;
+ }
pcl::PointXYZRGBNormal point_to_evaluate = pcl_cloud_ptr->points[*point_iterator];
if (filtering_configuration.use_neural_network
@@ -1065,10 +1067,12 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
if (euclideanDistance(reference_in_sensor_frame, pcl_cloud_ptr->points[*point_iterator])
< filtering_configuration.robot_height)
{
- 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;
-
+ if (!filtering_configuration.extract_data_to_external_training_of_the_network)
+ {
+ 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;
+ }
// 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
// and overhanging obstacles
@@ -1077,11 +1081,12 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
else
{
//std::cout << "Overhanging obstacle detected!" << std::endl;
-
- 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;
-
+ if (!filtering_configuration.extract_data_to_external_training_of_the_network)
+ {
+ 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].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE + 0.0;
}
}
@@ -1186,16 +1191,16 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
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(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;
@@ -1219,7 +1224,8 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
// This chunk of code is for using the shallow neural net for traversability prediction
- if (filtering_configuration.use_neural_network)
+ if (filtering_configuration.use_neural_network
+ && !filtering_configuration.extract_data_to_external_training_of_the_network)
{
for (int n = 0; n < num_of_ground_points; ++n)
{
@@ -1323,69 +1329,73 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
else // if we don't have enough information to try to predict the ground level at this point
{
- std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
- + elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX];
-
- for (std::vector<int>::const_iterator point_iterator = index_iterator->begin();
- point_iterator != index_iterator->end(); ++point_iterator)
+ if (!filtering_configuration.extract_data_to_external_training_of_the_network)
{
- // We now copy the data that is common to every point in the vector (because are statistics extracted
- // from the points in the vector)
+ std::vector<std::vector<int>>::const_iterator index_iterator = correspondence_indexes.begin()
+ + elevation_cloud_point.data_c[DATA_C_3_ORIGINAL_INDEX];
- // 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] =
- elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE];
+ 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[2] we store the mean z value in the elevation cell
- pcl_cloud_ptr->points[*point_iterator].data_n[DATA_N_2_Z_MEAN] = elevation_cloud_point.data_n[DATA_N_2_Z_MEAN];
+ // 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] =
+ elevation_cloud_point.data_n[DATA_N_1_INTENSITY_VARIANCE];
- // 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] =
- elevation_cloud_point.data_n[DATA_N_3_Z_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] =
+ elevation_cloud_point.data_n[DATA_N_2_Z_MEAN];
- if (filtering_configuration.classify_not_labeled_points_as_obstacles)
- {
- if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
- < filtering_configuration.ground_threshold_in_not_analyzed_areas)
- {
- 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;
+ // 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] =
+ elevation_cloud_point.data_n[DATA_N_3_Z_VARIANCE];
- 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;
- }
- else if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
- < filtering_configuration.robot_height)
+ if (filtering_configuration.classify_not_labeled_points_as_obstacles)
{
- 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;
+ if ((pcl_cloud_ptr->points[*point_iterator].z - elevation_cloud_point.z)
+ < filtering_configuration.ground_threshold_in_not_analyzed_areas)
+ {
+ 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;
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + score;
+ 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;
+ }
+ 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;
+ 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;
+ 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
+ + score;
+ }
}
else
{
- 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_UNKNOWN;
+ pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_UNKNOWN;
+ pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_UNKNOWN;
float score = 0.0;
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OVERHANGING_OBSTACLE
- + score;
+ pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_UNKNOWN + score;
}
}
- else
- {
- pcl_cloud_ptr->points[*point_iterator].r = R_CLASS_UNKNOWN;
- pcl_cloud_ptr->points[*point_iterator].g = G_CLASS_UNKNOWN;
- pcl_cloud_ptr->points[*point_iterator].b = B_CLASS_UNKNOWN;
-
- float score = 0.0;
- pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_UNKNOWN + score;
- }
}
}
}
--
GitLab