From c1078a4600f1d79851c4dfa24e0916c8c9f9eb04 Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Mon, 9 Jan 2023 19:11:54 +0100
Subject: [PATCH] dumping dataset to file for nn training in matlab
---
include/kf_based_terrain_analysis.h | 13 +++
include/structs_definitions.h | 3 +
src/kf_based_terrain_analysis.cpp | 155 ++++++++++++++++++++--------
3 files changed, 130 insertions(+), 41 deletions(-)
diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index d388976..c223edb 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -12,6 +12,15 @@
#include <pcl/search/kdtree.h>
#include <pcl/filters/voxel_grid.h>
+#include <iostream>
+//#include <sstream>
+//#include <iomanip>
+//#include <memory>
+#include <fstream>
+#include <string>
+//#include <sys/stat.h>
+//#include <cmath>
+
#include "structs_definitions.h"
#include "time_supervisor.h"
#include "observation_downsampler.h"
@@ -24,6 +33,10 @@ private:
bool use_full_covariance_matrices_;
std::vector<Eigen::Matrix3d> references_covariance_matrix_vector_;
+ std::ofstream dataset_file_;
+ bool extract_data_to_external_training_of_the_network_;
+ bool first_iteration_;
+
float predictZ(const pcl::PointXYZRGBNormal reference, const float delta_x, const float delta_y);
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index 01aa17d..ff5768d 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -166,6 +166,9 @@ struct FilteringConfiguration
float mahalanobis_threshold;
+ bool use_neural_network;
+ bool extract_data_to_external_training_of_the_network;
+
float max_pred_std_dev_for_labelling;
float score_threshold;
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 654d29f..178a177 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -4,11 +4,17 @@ CKf_Based_Terrain_Analysis::CKf_Based_Terrain_Analysis()
{
performance_supervisor_ptr_ = boost::make_shared<CTimeSupervisor>();
use_full_covariance_matrices_ = false; // TODO: Extract parameter!!
+ first_iteration_ = true;
+ std::cout << "Ground terrain analysis processor created!" << std::endl;
}
CKf_Based_Terrain_Analysis::~CKf_Based_Terrain_Analysis()
{
-
+ if (extract_data_to_external_training_of_the_network_)
+ {
+ //std::cout << "Closing file!" << std::endl;
+ dataset_file_.close();
+ }
}
void CKf_Based_Terrain_Analysis::showPerformanceStatistics(void)
@@ -998,15 +1004,19 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// 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;
- std::vector<double> gt_label;
+// if (filtering_configuration.use_neural_network
+// || filtering_configuration.extract_data_to_external_training_of_the_network)
+// {
+ 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;
+ std::vector<double> gt_label;
+// }
// 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()
@@ -1034,25 +1044,29 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
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
- //std::cout << "Index saved = " << *point_iterator << std::endl;
+ if (filtering_configuration.use_neural_network
+ || filtering_configuration.extract_data_to_external_training_of_the_network)
+ {
+ 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;
- 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!
+ 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!
- incidence_angles.push_back(0.0); // TODO: Incidence angle w.r.t the plane defined by the ground reference // Feature!
- intensities.push_back(point_to_evaluate.data_n[DATA_N_0_INTENSITY]); // Point intensity // Feature!
+ incidence_angles.push_back(0.0); // TODO: Incidence angle w.r.t the plane defined by the ground reference // Feature!
+ intensities.push_back(point_to_evaluate.data_n[DATA_N_0_INTENSITY]); // Point intensity // Feature!
- 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);
+ 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); // Feature!
+ point_to_reference_squared_distances.push_back(delta_x * delta_x + delta_y * delta_y + delta_z * delta_z); // Feature!
- prediction_errors.push_back((double)euclideanDistance(reference_in_sensor_frame, point_to_evaluate)); // Prediction Error // Feature!
+ prediction_errors.push_back((double)euclideanDistance(reference_in_sensor_frame, point_to_evaluate)); // Prediction Error // Feature!
- scores.push_back((double)score); // Feature!
+ scores.push_back((double)score); // Feature!
+ }
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_GROUND + score;
}
@@ -1091,10 +1105,13 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
}
}
}
+
int num_of_ground_points = squared_distances.size();
//std::cout << "num_of_ground_points = " << num_of_ground_points << std::endl;
- if (num_of_ground_points > 0)
+ if (num_of_ground_points > 0
+ && (filtering_configuration.use_neural_network
+ || filtering_configuration.extract_data_to_external_training_of_the_network))
{
int total_num_of_points = correspondence_indexes[point_in_sensor_frame.data_c[DATA_C_3_ORIGINAL_INDEX]].size();
//std::cout << "total_num_of_points = " << total_num_of_points << std::endl;
@@ -1140,23 +1157,67 @@ 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;
- 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;
+ 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;
+
+ assert(
+ gt_label != -1
+ && "ERROR in CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel, unexpected class label!");
+
+ 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;
+
+ 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
@@ -1267,6 +1328,18 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("groundSegmentation", performance_supervisor_ptr_);
+ extract_data_to_external_training_of_the_network_ = filtering_configuration.extract_data_to_external_training_of_the_network;
+ if (extract_data_to_external_training_of_the_network_ && first_iteration_)
+ {
+// std::string filename = "/mnt/DATA/Datasets/semantic_kitti/point_level_results_for_learning/dataset.txt";
+ std::string filename = "/home/idelpino/Documentos/dataset.txt";
+ std::cout << "Creating file " << filename.c_str() << std::endl;
+
+ dataset_file_.open(filename.c_str(), std::ios_base::trunc);
+ }
+
+ first_iteration_ = false;
+
//std::cout << "Starting ground segmentation!!" << std::endl;
//std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
--
GitLab