dumping dataset to file for nn training in matlab

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);
 

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;
 

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;