diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index c7f4d3ad5de731dfc797b9eb737ad1c07864601d..2a6a78c56619ca60ef6d4cee202cd714377369d4 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1135,6 +1135,90 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
var_of_scores /= (double)num_of_ground_points; // Feature!
//std::cout << "var_of_scores = " << var_of_scores << std::endl;
+ // This chunk of code is for generating a dataset to train the shallow neural network
+ // We do this before predicting with the neural ne, so the ground truth is not altered
+ if (filtering_configuration.extract_data_to_external_training_of_the_network)
+ {
+ int randomIndex = std::rand() % num_of_ground_points; // To save the dataset we will use only one random point of each elevation cloud
+ //std::cout << "randomIndex = " << randomIndex << std::endl;
+
+ int gt_label = -1;
+ pcl::PointXYZRGBNormal point_to_read_gt_label =
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[randomIndex]];
+
+ if (point_to_read_gt_label.r == R_CLASS_ROAD && point_to_read_gt_label.g == G_CLASS_ROAD
+ && point_to_read_gt_label.b == B_CLASS_ROAD)
+ gt_label = ROAD;
+
+ if (point_to_read_gt_label.r == R_CLASS_TERRAIN && point_to_read_gt_label.g == G_CLASS_TERRAIN
+ && point_to_read_gt_label.b == B_CLASS_TERRAIN)
+ gt_label = TERRAIN;
+
+ if (point_to_read_gt_label.r == R_CLASS_VEGETATION && point_to_read_gt_label.g == G_CLASS_VEGETATION
+ && point_to_read_gt_label.b == B_CLASS_VEGETATION)
+ gt_label = VEGETATION;
+
+ if (point_to_read_gt_label.r == R_CLASS_SIDEWALK && point_to_read_gt_label.g == G_CLASS_SIDEWALK
+ && point_to_read_gt_label.b == B_CLASS_SIDEWALK)
+ gt_label = SIDEWALK;
+
+ if (point_to_read_gt_label.r == R_CLASS_KITTI_OBSTACLE && point_to_read_gt_label.g == G_CLASS_KITTI_OBSTACLE
+ && point_to_read_gt_label.b == B_CLASS_KITTI_OBSTACLE)
+ gt_label = CLASS_OBSTACLE;
+
+ if (point_to_read_gt_label.r == R_CLASS_OUTLIER && point_to_read_gt_label.g == G_CLASS_OUTLIER
+ && point_to_read_gt_label.b == B_CLASS_OUTLIER)
+ gt_label = OUTLIER;
+
+ if (gt_label == -1)
+ {
+ std::cout << "gt_label = " << gt_label << std::endl;
+ std::cout << "point_to_read_gt_label.r = " << (int)point_to_read_gt_label.r
+ << " point_to_read_gt_label.g = " << (int)point_to_read_gt_label.g
+ << " point_to_read_gt_label.b = " << (int)point_to_read_gt_label.b << std::endl;
+ }
+
+ assert(
+ gt_label != -1
+ && "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel, unexpected class label!");
+
+ if (gt_label != OUTLIER)
+ {
+ 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;
+
+ std::setiosflags(std::ios::fixed);
+ std::setprecision(6);
+ std::ostringstream s;
+
+ 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) << ", " << gt_label << std::endl;
+
+ if (features(0, 0) >= 0.0)
+ dataset_file_ << s.str();
+
+ //std::cout << "gt_label = " << gt_label << std::endl;
+ }
+ }
+
+ // This chunk of code is for using the shallow neural net for traversability prediction
if (filtering_configuration.use_neural_network)
{
for (int n = 0; n < num_of_ground_points; ++n)
@@ -1235,87 +1319,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
}
}
-
- int randomIndex = std::rand() % num_of_ground_points; // To save the dataset we will use only one random point of each elevation cloud
- //std::cout << "randomIndex = " << randomIndex << std::endl;
-
- if (filtering_configuration.extract_data_to_external_training_of_the_network)
- {
- int gt_label = -1;
- pcl::PointXYZRGBNormal point_to_read_gt_label =
- pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[randomIndex]];
-
- if (point_to_read_gt_label.r == R_CLASS_ROAD && point_to_read_gt_label.g == G_CLASS_ROAD
- && point_to_read_gt_label.b == B_CLASS_ROAD)
- gt_label = ROAD;
-
- if (point_to_read_gt_label.r == R_CLASS_TERRAIN && point_to_read_gt_label.g == G_CLASS_TERRAIN
- && point_to_read_gt_label.b == B_CLASS_TERRAIN)
- gt_label = TERRAIN;
-
- if (point_to_read_gt_label.r == R_CLASS_VEGETATION && point_to_read_gt_label.g == G_CLASS_VEGETATION
- && point_to_read_gt_label.b == B_CLASS_VEGETATION)
- gt_label = VEGETATION;
-
- if (point_to_read_gt_label.r == R_CLASS_SIDEWALK && point_to_read_gt_label.g == G_CLASS_SIDEWALK
- && point_to_read_gt_label.b == B_CLASS_SIDEWALK)
- gt_label = SIDEWALK;
-
- if (point_to_read_gt_label.r == R_CLASS_KITTI_OBSTACLE && point_to_read_gt_label.g == G_CLASS_KITTI_OBSTACLE
- && point_to_read_gt_label.b == B_CLASS_KITTI_OBSTACLE)
- gt_label = CLASS_OBSTACLE;
-
- if (point_to_read_gt_label.r == R_CLASS_OUTLIER && point_to_read_gt_label.g == G_CLASS_OUTLIER
- && point_to_read_gt_label.b == B_CLASS_OUTLIER)
- gt_label = OUTLIER;
-
- if (gt_label == -1)
- {
- std::cout << "gt_label = " << gt_label << std::endl;
- std::cout << "point_to_read_gt_label.r = " << (int)point_to_read_gt_label.r
- << " point_to_read_gt_label.g = " << (int)point_to_read_gt_label.g
- << " point_to_read_gt_label.b = " << (int)point_to_read_gt_label.b << std::endl;
- }
-
- assert(
- gt_label != -1
- && "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel, unexpected class label!");
-
- if (gt_label != OUTLIER)
- {
- 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;
-
- std::setiosflags(std::ios::fixed);
- std::setprecision(6);
- std::ostringstream s;
-
- 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) << ", " << gt_label << std::endl;
-
- if (features(0, 0) >= 0.0)
- dataset_file_ << s.str();
-
- //std::cout << "gt_label = " << gt_label << std::endl;
- }
- }
}
}
else // if we don't have enough information to try to predict the ground level at this point