From f3dde0e2053392ad9f725aee551f9fbf9abff725 Mon Sep 17 00:00:00 2001 From: idelpino <telecosg@gmail.com> Date: Mon, 16 Jan 2023 19:12:47 +0100 Subject: [PATCH] added loading of neural net from file --- include/kf_based_terrain_analysis.h | 11 + include/structs_definitions.h | 2 + src/CMakeLists.txt | 6 +- src/kf_based_terrain_analysis.cpp | 357 ++++++++++++++++++++++++---- 4 files changed, 325 insertions(+), 51 deletions(-) diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h index c223edb..a7ab334 100644 --- a/include/kf_based_terrain_analysis.h +++ b/include/kf_based_terrain_analysis.h @@ -25,6 +25,9 @@ #include "time_supervisor.h" #include "observation_downsampler.h" +#include "rapidcsv.h" +#include "shallow_neural_net.h" + class CKf_Based_Terrain_Analysis { private: @@ -35,8 +38,10 @@ private: std::ofstream dataset_file_; bool extract_data_to_external_training_of_the_network_; + bool use_neural_network_; bool first_iteration_; + ShallowNeuralNet nn_; float predictZ(const pcl::PointXYZRGBNormal reference, const float delta_x, const float delta_y); @@ -110,6 +115,12 @@ private: const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr, std::vector<std::vector<int>> edges); + void createFileToDumpDataset(const std::string filename); + + void loadNeuralNetwork(const std::string filename); + + void prepareFileInterface(const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration); + public: CKf_Based_Terrain_Analysis(); ~CKf_Based_Terrain_Analysis(); diff --git a/include/structs_definitions.h b/include/structs_definitions.h index ff5768d..a0f44d1 100644 --- a/include/structs_definitions.h +++ b/include/structs_definitions.h @@ -168,6 +168,8 @@ struct FilteringConfiguration bool use_neural_network; bool extract_data_to_external_training_of_the_network; + std::string dataset_filename_with_global_path; + std::string neural_net_filename_with_global_path; float max_pred_std_dev_for_labelling; float score_threshold; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b1e3573..502ceab 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,13 +1,15 @@ # driver source files SET(sources kf_based_terrain_analysis.cpp time_supervisor.cpp - observation_downsampler.cpp) + observation_downsampler.cpp + shallow_neural_net.cpp) # application header files SET(headers ../include/kf_based_terrain_analysis.h ../include/structs_definitions.h ../include/time_supervisor.h - ../include/observation_downsampler.h) + ../include/observation_downsampler.h + ../include/shallow_neural_net.h) # locate the necessary dependencies FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp) diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp index 178a177..fca0392 100644 --- a/src/kf_based_terrain_analysis.cpp +++ b/src/kf_based_terrain_analysis.cpp @@ -1007,15 +1007,15 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( // 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; + 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 @@ -1054,7 +1054,15 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( (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! + double azimuth = atan2(point_to_evaluate.y, point_to_evaluate.x); + double rho = sqrt(point_to_evaluate.x * point_to_evaluate.x + point_to_evaluate.y * point_to_evaluate.y); + double slope_x = reference_in_sensor_frame.data_c[GRND_REF_DATA_C_2_PITCH]; + double slope_y = reference_in_sensor_frame.data_c[GRND_REF_DATA_C_1_ROLL]; + + double incidence_angle = atan2(-1.0 * point_to_evaluate.z, rho) + + atan(cos(azimuth) * slope_x + sin(azimuth) * slope_y); + + incidence_angles.push_back(incidence_angle); // 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); @@ -1154,6 +1162,54 @@ 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; + if (filtering_configuration.use_neural_network) + { + for (int n = 0; n < num_of_ground_points; ++n) + { + Eigen::VectorXd features(13, 1); + + 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); + pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] = predicted_class; + + switch(predicted_class) + { + case ROAD: + + break; + + case SIDEWALK: + + break; + + case TERRAIN: + + break; + + case VEGETATION: + + break; + + case CLASS_OBSTACLE: + + break; + } + } + } + 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; @@ -1183,40 +1239,55 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel( && 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!"); - 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; + 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; + + dataset_file_ << s.str(); + + //std::cout << "gt_label = " << gt_label << std::endl; + } } } } @@ -1317,6 +1388,200 @@ void CKf_Based_Terrain_Analysis::ensureThatConnectionsAreReflectedInBothEnds( } } +void CKf_Based_Terrain_Analysis::loadNeuralNetwork(const std::string filename) +{ + rapidcsv::Document doc(filename.c_str(), rapidcsv::LabelParams(-1, -1)); + + nn_.number_of_features_ = 13; + nn_.number_of_neurons_in_hidden_layer_ = 10; + nn_.number_of_neurons_in_output_layer_ = 5; + + int b1_n_rows = nn_.number_of_neurons_in_hidden_layer_; + int b1_n_cols = 1; + nn_.b1_.resize(b1_n_rows, b1_n_cols); + std::cout << "nn_.b1_.rows() = " << nn_.b1_.rows() << " nn_.b1_.cols() = " << nn_.b1_.cols() << std::endl; + + int b1_row_ini = 0; + int b1_row_fin = b1_row_ini + b1_n_rows - 1; + + int current_row_in_eigen = 0; + for (int i = b1_row_ini; i <= b1_row_fin; ++i) + { + std::cout << "row index in file = " << i << std::endl; + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + { + std::cout << "Writing value = " << row[j] << " in Eigen matrix at index ( " << current_row_in_eigen << ", " << j + << " )" << std::endl; + nn_.b1_(current_row_in_eigen, j) = row[j]; + } + current_row_in_eigen++; + } + + int b2_n_rows = nn_.number_of_neurons_in_output_layer_; + int b2_n_cols = 1; + nn_.b2_.resize(b2_n_rows, b2_n_cols); + + int b2_row_ini = b1_row_fin + 1; + int b2_row_fin = b2_row_ini + b2_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = b2_row_ini; i <= b2_row_fin; ++i) + { + std::cout << "row index in file = " << i << std::endl; + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + { + std::cout << "Writing value = " << row[j] << " in Eigen matrix at index ( " << current_row_in_eigen << ", " << j + << " )" << std::endl; + nn_.b2_(current_row_in_eigen, j) = row[j]; + } + current_row_in_eigen++; + } + + int gain_n_rows = nn_.number_of_features_; + int gain_n_cols = 1; + nn_.gain_.resize(gain_n_rows, gain_n_cols); + + int gain_row_ini = b2_row_fin + 1; + int gain_row_fin = gain_row_ini + gain_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = gain_row_ini; i <= gain_row_fin; ++i) + { + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + nn_.gain_(current_row_in_eigen, j) = row[j]; + + current_row_in_eigen++; + } + + int w1_n_rows = nn_.number_of_neurons_in_hidden_layer_; + int w1_n_cols = nn_.number_of_features_; + nn_.w1_.resize(w1_n_rows, w1_n_cols); + + int w1_row_ini = gain_row_fin + 1; + int w1_row_fin = w1_row_ini + w1_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = w1_row_ini; i <= w1_row_fin; ++i) + { + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + nn_.w1_(current_row_in_eigen, j) = row[j]; + + current_row_in_eigen++; + } + + int w2_n_rows = nn_.number_of_neurons_in_output_layer_; + int w2_n_cols = nn_.number_of_neurons_in_hidden_layer_; + nn_.w2_.resize(w2_n_rows, w2_n_cols); + + int w2_row_ini = w1_row_fin + 1; + int w2_row_fin = w2_row_ini + w2_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = w2_row_ini; i <= w2_row_fin; ++i) + { + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + nn_.w2_(current_row_in_eigen, j) = row[j]; + + current_row_in_eigen++; + } + + int x_offset_n_rows = nn_.number_of_features_; + int x_offset_n_cols = 1; + nn_.x_offset_.resize(x_offset_n_rows, x_offset_n_cols); + + int x_offset_row_ini = w2_row_fin + 1; + int x_offset_row_fin = x_offset_row_ini + x_offset_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = x_offset_row_ini; i <= x_offset_row_fin; ++i) + { + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + nn_.x_offset_(current_row_in_eigen, j) = row[j]; + + current_row_in_eigen++; + } + + int y_min_n_rows = 1; + int y_min_n_cols = 1; + nn_.y_min_.resize(y_min_n_rows, y_min_n_cols); + + int y_min_row_ini = x_offset_row_fin + 1; + int y_min_row_fin = y_min_row_ini + y_min_n_rows - 1; + + current_row_in_eigen = 0; + for (int i = y_min_row_ini; i <= y_min_row_fin; ++i) + { + std::vector<double> row = doc.GetRow<double>(i); + for (int j = 0; j < row.size(); ++j) + nn_.y_min_(current_row_in_eigen, j) = row[j]; + + current_row_in_eigen++; + } + + int total_rows_in_neural_network_description_file = b1_n_rows + b2_n_rows + gain_n_rows + w1_n_rows + w2_n_rows + + x_offset_n_rows + y_min_n_rows; + + std::cout << "total_rows_in_neural_network_description_file = " << total_rows_in_neural_network_description_file + << std::endl; + + std::cout << "b1 = " << std::endl << nn_.b1_ << std::endl; + std::cout << "b2 = " << std::endl << nn_.b2_ << std::endl; + std::cout << "gain = " << std::endl << nn_.gain_ << std::endl; + std::cout << "w1 = " << std::endl << nn_.w1_ << std::endl; + std::cout << "w2 = " << std::endl << nn_.w2_ << std::endl; + std::cout << "x_offset = " << std::endl << nn_.x_offset_ << std::endl; + std::cout << "y_min = " << std::endl << nn_.y_min_ << std::endl; + + std::cout << "Waiting for the user to press any key..." << std::endl; + getchar(); + return; +} + +void CKf_Based_Terrain_Analysis::createFileToDumpDataset(const std::string filename) +{ + std::cout << "Creating file to dump the dataset, already existing files with the same name will be erased! " + << filename.c_str() << std::endl; + + std::cout << "Are you sure you want to continue? (y/n) " << std::endl; + + char response = 'n'; + response = getchar(); + + if (response == 'y' || response == 'Y') + { + std::cout << "Creating file..." << std::endl; + dataset_file_.open(filename.c_str(), std::ios_base::trunc); + std::cout << "File created!" << std::endl; + } + else + { + std::cout << "File creation aborted!" << std::endl; + assert(false && "Killing node!"); + } + return; +} + +void CKf_Based_Terrain_Analysis::prepareFileInterface( + const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration) +{ + 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_) + createFileToDumpDataset(filtering_configuration.dataset_filename_with_global_path); + + use_neural_network_ = filtering_configuration.use_neural_network; + + if (use_neural_network_) + loadNeuralNetwork(filtering_configuration.neural_net_filename_with_global_path); +} + void CKf_Based_Terrain_Analysis::groundSegmentation( const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration, const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration, @@ -1328,18 +1593,12 @@ 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_) + if (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); + prepareFileInterface(filtering_configuration); + first_iteration_ = false; } - 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