From 0fb8f2b62920cf1d1afce17057d43796d9ff6be2 Mon Sep 17 00:00:00 2001
From: idelpino <telecosg@gmail.com>
Date: Thu, 16 Feb 2023 20:57:45 +0100
Subject: [PATCH] added neural network of different possible sizes
---
include/kf_based_terrain_analysis.h | 2 +-
include/shallow_neural_net.h | 31 ++++++
include/structs_definitions.h | 3 +
src/kf_based_terrain_analysis.cpp | 154 +++++++++++++++++++---------
src/shallow_neural_net.cpp | 101 ++++++++++++++++++
5 files changed, 240 insertions(+), 51 deletions(-)
create mode 100644 include/shallow_neural_net.h
create mode 100644 src/shallow_neural_net.cpp
diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index a7ab334..129cf8c 100644
--- a/include/kf_based_terrain_analysis.h
+++ b/include/kf_based_terrain_analysis.h
@@ -117,7 +117,7 @@ private:
void createFileToDumpDataset(const std::string filename);
- void loadNeuralNetwork(const std::string filename);
+ void loadNeuralNetwork(const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration);
void prepareFileInterface(const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration);
diff --git a/include/shallow_neural_net.h b/include/shallow_neural_net.h
new file mode 100644
index 0000000..bcc00ae
--- /dev/null
+++ b/include/shallow_neural_net.h
@@ -0,0 +1,31 @@
+#ifndef _shallow_neural_net_h_
+#define _shallow_neural_net_h_
+
+#include <iostream>
+#include <Eigen/Dense>
+#include "structs_definitions.h"
+
+class ShallowNeuralNet
+{
+protected:
+
+public:
+ Eigen::MatrixXd b1_;
+ Eigen::MatrixXd b2_;
+ Eigen::MatrixXd gain_;
+ Eigen::MatrixXd w1_;
+ Eigen::MatrixXd w2_;
+ Eigen::MatrixXd x_offset_;
+ Eigen::MatrixXd y_min_;
+
+ int number_of_features_;
+ int number_of_neurons_in_hidden_layer_;
+ int number_of_neurons_in_output_layer_;
+
+ ShallowNeuralNet(void);
+ ~ShallowNeuralNet(void);
+
+ double predict(Eigen::VectorXd features);
+};
+
+#endif
diff --git a/include/structs_definitions.h b/include/structs_definitions.h
index a0f44d1..05773ca 100644
--- a/include/structs_definitions.h
+++ b/include/structs_definitions.h
@@ -170,6 +170,9 @@ struct FilteringConfiguration
bool extract_data_to_external_training_of_the_network;
std::string dataset_filename_with_global_path;
std::string neural_net_filename_with_global_path;
+ int neural_net_number_of_features;
+ int neural_net_number_of_neurons_in_hidden_layer;
+ int neural_net_number_of_neurons_in_output_layer;
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 5f5d265..ea46cf7 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -1004,11 +1004,12 @@ 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
-// 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> rgb_values;
+ std::vector<double> hsv_values;
+ std::vector<double> local_binary_patterns;
+
std::vector<double> squared_distances;
std::vector<double> incidence_angles;
std::vector<double> intensities;
@@ -1016,7 +1017,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
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()
@@ -1050,6 +1050,10 @@ 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!
+ hsv_values.push_back(point_to_evaluate.data_n[DATA_N_2_Z_MEAN]); // Feature!
+ local_binary_patterns.push_back(point_to_evaluate.data_n[DATA_N_3_Z_VARIANCE]); // 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!
@@ -1097,9 +1101,6 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
// probability of being ground is zero, the class label is used to distinguish between general obstacles
// and overhanging obstacles
pcl_cloud_ptr->points[*point_iterator].data_c[DATA_C_1_ID_CLASS] = (float)CLASS_OBSTACLE + 0.0;
-
-// if (flag_reduce_mahalanobis_thres_if_an_obstacle_is_found)
-// flag_obstacle_found = true;
}
else
{
@@ -1172,55 +1173,100 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
{
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;
+ 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);
+ }
int predicted_class = nn_.predict(features);
- pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] = (float)predicted_class + (float)scores[n];
- switch(predicted_class)
+ if (predicted_class == CLASS_OBSTACLE)
+ {
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] =
+ (float)predicted_class + 0.0;
+ }
+ else
+ {
+ pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] =
+ (float)predicted_class + (float)scores[n];
+ }
+ //std::cout << "Class + score = " << pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].data_c[DATA_C_1_ID_CLASS] << std::endl;
+
+ switch (predicted_class)
{
case ROAD:
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_ROAD;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_ROAD;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_ROAD;
+ //std::cout << "NN says point is road!" << std::endl;
break;
case SIDEWALK:
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_SIDEWALK;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_SIDEWALK;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_SIDEWALK;
+ std::cout << "NN says point is sidewalk!" << std::endl;
break;
case TERRAIN:
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_TERRAIN;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_TERRAIN;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_TERRAIN;
+ std::cout << "NN says point is terrain!" << std::endl;
break;
case VEGETATION:
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_VEGETATION;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_VEGETATION;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_VEGETATION;
+ std::cout << "NN says point is vegetation!" << std::endl;
break;
case CLASS_OBSTACLE:
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].r = R_CLASS_KITTI_OBSTACLE;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].g = G_CLASS_KITTI_OBSTACLE;
pcl_cloud_ptr->points[features_indexes_in_pcl_cloud_ptr[n]].b = B_CLASS_KITTI_OBSTACLE;
+ std::cout << "NN says point is Obstacle!" << std::endl;
break;
}
}
@@ -1273,21 +1319,25 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
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;
+ 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;
//std::cout << "features = " << std::endl << features << std::endl;
@@ -1298,9 +1348,11 @@ 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) << ", " << gt_label << std::endl;
+ << ", " << features(12, 0) << ", " << features(13, 0) << ", " << features(14, 0) << ", "
+ << features(15, 0) << ", " << features(16, 0) << ", " << gt_label << std::endl;
- dataset_file_ << s.str();
+ if (features(0, 0) >= 0.0)
+ dataset_file_ << s.str();
//std::cout << "gt_label = " << gt_label << std::endl;
}
@@ -1413,13 +1465,15 @@ void CKf_Based_Terrain_Analysis::ensureThatConnectionsAreReflectedInBothEnds(
}
}
-void CKf_Based_Terrain_Analysis::loadNeuralNetwork(const std::string filename)
+void CKf_Based_Terrain_Analysis::loadNeuralNetwork(
+ const kf_based_terrain_analysis_lib::FilteringConfiguration filtering_configuration)
{
- rapidcsv::Document doc(filename.c_str(), rapidcsv::LabelParams(-1, -1));
+ rapidcsv::Document doc(filtering_configuration.neural_net_filename_with_global_path.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;
+ nn_.number_of_features_ = filtering_configuration.neural_net_number_of_features;
+ nn_.number_of_neurons_in_hidden_layer_ = filtering_configuration.neural_net_number_of_neurons_in_hidden_layer;
+ nn_.number_of_neurons_in_output_layer_ = filtering_configuration.neural_net_number_of_neurons_in_output_layer;
int b1_n_rows = nn_.number_of_neurons_in_hidden_layer_;
int b1_n_cols = 1;
@@ -1563,8 +1617,8 @@ void CKf_Based_Terrain_Analysis::loadNeuralNetwork(const std::string filename)
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();
+ //std::cout << "Waiting for the user to press any key..." << std::endl;
+ //getchar();
return;
}
@@ -1604,7 +1658,7 @@ void CKf_Based_Terrain_Analysis::prepareFileInterface(
use_neural_network_ = filtering_configuration.use_neural_network;
if (use_neural_network_)
- loadNeuralNetwork(filtering_configuration.neural_net_filename_with_global_path);
+ loadNeuralNetwork(filtering_configuration);
}
void CKf_Based_Terrain_Analysis::groundSegmentation(
@@ -1645,7 +1699,7 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
// (ground_reference_cloud_ptr contains the vertices and the vector of int vectors named edges contains the edgesof the graph)
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-// This is an optimized version of a previous function (thats why the "fast" is appended at the begginning of the name),
+// This is an optimized version of a previous function (thats why the "fast" is appended at the beginning of the name),
// but it became the standard so the previous one was deleted for sake of code clarity. This function generates the
// representation using the grid to keep only the lowest points in each cell and a vector with indexes to the rest of points
fastGenerateElevationCloud(filtering_configuration, pcl_cloud_ptr, elevation_cloud_ptr, correspondence_indexes);
diff --git a/src/shallow_neural_net.cpp b/src/shallow_neural_net.cpp
new file mode 100644
index 0000000..ed991b5
--- /dev/null
+++ b/src/shallow_neural_net.cpp
@@ -0,0 +1,101 @@
+#include "shallow_neural_net.h"
+
+ShallowNeuralNet::ShallowNeuralNet(void)
+{
+
+}
+
+ShallowNeuralNet::~ShallowNeuralNet(void)
+{
+
+}
+
+double ShallowNeuralNet::predict(Eigen::VectorXd features)
+{
+ int predicted_class = -1;
+
+ assert(features.rows() == this->number_of_features_ && "Error using ShallowNeuralNet::predict, number of features not expected!");
+
+ //std::cout << "features = " << std::endl << features << std::endl;
+ Eigen::VectorXd x_offset_applied(number_of_features_, 1);
+ x_offset_applied = features - this->x_offset_;
+
+ //std::cout << "x_offset_applied = " << std::endl << x_offset_applied << std::endl;
+
+ Eigen::VectorXd x_gain_plus_y_min(number_of_features_, 1);
+ for (int i = 0; i < number_of_features_; ++i)
+ x_gain_plus_y_min(i,0) = x_offset_applied(i, 0) * this->gain_(i, 0) + this->y_min_(0, 0);
+
+ //std::cout << "x_gain_plus_y_min = " << std::endl << x_gain_plus_y_min << std::endl;
+
+ Eigen::VectorXd z1(this->number_of_neurons_in_hidden_layer_, 1);
+ z1 = this->b1_ + (this->w1_ * x_gain_plus_y_min);
+
+ //std::cout << "z1 = " << std::endl << z1 << std::endl;
+
+ Eigen::VectorXd a1(this->number_of_neurons_in_hidden_layer_, 1);
+ for (int i = 0; i < this->number_of_neurons_in_hidden_layer_; ++i)
+ a1(i,0) = (2.0 / (1.0 + exp(-2.0 * z1(i,0)))) - 1.0;
+
+ //std::cout << "a1 = " << std::endl << a1 << std::endl;
+
+ Eigen::VectorXd z2(this->number_of_neurons_in_output_layer_, 1);
+ z2 = this->b2_ + (this->w2_ * a1);
+ //std::cout << "z2 = " << std::endl << z2 << std::endl;
+
+ double z2_max = z2.maxCoeff();
+ //std::cout << "z2_max = " << std::endl << z2_max << std::endl;
+ z2.array() -= z2_max;
+ //std::cout << "z2 = " << std::endl << z2 << std::endl;
+
+ Eigen::VectorXd numerator(this->number_of_neurons_in_output_layer_, 1);
+ for(int i = 0; i < this->number_of_neurons_in_output_layer_; ++i)
+ numerator(i,0) = exp(z2(i,0));
+
+ //std::cout << "numerator = " << std::endl << numerator << std::endl;
+
+ double denominator = numerator.sum();
+ //std::cout << "denominator = " << std::endl << denominator << std::endl;
+
+ Eigen::VectorXd prediction(this->number_of_neurons_in_output_layer_, 1);
+ prediction = numerator / denominator;
+
+ //std::cout << "prediction = " << std::endl << prediction << std::endl;
+
+ int index_of_max_prob = -1;
+ double max_prob = -1.0;
+ for (int i = 0; i < this->number_of_neurons_in_output_layer_; ++i)
+ {
+ if(prediction(i,0) > max_prob)
+ {
+ max_prob = prediction(i,0);
+ index_of_max_prob = i;
+ }
+ }
+ predicted_class = index_of_max_prob;
+
+ switch(predicted_class)
+ {
+ case 0:
+ predicted_class = CLASS_OBSTACLE;
+ break;
+ case 1:
+ predicted_class = ROAD;
+ break;
+ case 2:
+ predicted_class = SIDEWALK;
+ break;
+ case 3:
+ predicted_class = TERRAIN;
+ break;
+ case 4:
+ predicted_class = VEGETATION;
+ break;
+ default:
+ assert(false && "ERROR in ShallowNeuralNet::predict: unexpected predicted class!!");
+ }
+
+ //std::cout << "predicted_class = " << std::endl << predicted_class << std::endl;
+
+ return(predicted_class);
+}
--
GitLab