diff --git a/include/kf_based_terrain_analysis.h b/include/kf_based_terrain_analysis.h
index c223edb5a0780e9fe1e2a9e8da8f6445cf636f68..a7ab334501fdb975441423e0a6a770b492487b8a 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 ff5768d146edc97a0967cc21c8ba134a7bca2e49..a0f44d1cd9c313bfbb309e10de8c42f18664fd22 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 b1e35732d29b53f07e4eb7c3152f6f0630ee7ab2..502ceab8603d713164f56f5f06078bcff62846eb 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 178a1775d7cb5127fbe247c0a82d68cf034efffe..fca0392e92b737c7a1963fd16fde3b79fef91dbf 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;