performance_test.cpp

Go to the documentation of this file.

#include <gtest/gtest.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
 
#include <chrono>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
 
#include "clustering/dbscan.hpp"
#include "cone_differentiation/least_squares_differentiation.hpp"
#include "ground_removal/ransac.hpp"
 
struct PerceptionExecutionData {
  long long initial_n_points;     
  long long RANSAC_time;          
  long long ground_removed_size;  
  long long DBSCAN_time;          
  long long ConeDif_time;         
  long long Total_time;           
  long long generatedClusters;    
  long long blues;                
  long long yellows;              
  long long undefineds;           
  long long conversion_duration;  
  double RANSAC_eps;              
};
 
class PerceptionPerformanceTest : public ::testing::Test {
public:
  void SetUp() override {
    pcl_cloud = std::make_unique<pcl::PointCloud<pcl::PointXYZI>>();
    ground_removal = std::make_unique<RANSAC>(RANSAC_eps, RANSAC_Iter);
    clustering = std::make_unique<DBSCAN>(DBSCAN_neighbours_threshold, DBSCAN_dist_threshold);
    cone_differentiator = std::make_unique<LeastSquaresDifferentiation>();
  }
 
  std::string get_current_date_time_as_string() {
    auto now = std::chrono::system_clock::now();
    std::time_t now_time = std::chrono::system_clock::to_time_t(now);
    std::tm now_tm;
    localtime_r(&now_time, &now_tm);
 
    std::stringstream ss;
    ss << std::put_time(&now_tm, "%Y-%m-%d-%H:%M");
    return ss.str();
  }
 
  void writeToFile(const PerceptionExecutionData& executionTime) {
    bool fileExists = std::filesystem::exists(output_statistics_path_file);
 
    std::ofstream csv_file(output_statistics_path_file, std::ios::app);
    if (!fileExists) {
      csv_file << "Initial Number of Points,Conversion Duration,"
               << "RANSAC Epsilon,RANSAC Number Iterations,"
               << "Number of Points after RANSAC,RANSAC Duration(ms),"
               << "DBSCAN Distance Threshold,DBSCAN Neighbours Threshold,"
               << "Number of Generated Clusters,"
               << "DBSCAN Duration(ms),"
               << "Number of Blue Cones,Number of Yellow Cones,"
               << "Number of Undefined Cones,ConeDifferentiation Duration(ms),"
               << "Total Duration(ms)\n";
    }
 
    csv_file << std::fixed << std::setprecision(6);
    csv_file << executionTime.initial_n_points << "," << std::fixed << std::setprecision(3)
             << executionTime.conversion_duration << "," << RANSAC_eps << "," << RANSAC_Iter << ","
             << executionTime.ground_removed_size << "," << std::fixed << std::setprecision(3)
             << executionTime.RANSAC_time << "," << DBSCAN_dist_threshold << ","
             << executionTime.DBSCAN_time << "," << executionTime.generatedClusters << ","
             << std::fixed << std::setprecision(3) << executionTime.DBSCAN_time << ","
             << executionTime.blues << "," << executionTime.yellows << ","
             << executionTime.undefineds << "," << std::fixed << std::setprecision(3)
             << executionTime.ConeDif_time << "," << std::fixed << std::setprecision(3)
             << executionTime.Total_time << std::endl;
 
    csv_file.close();
  }
  std::string output_statistics_path_file = "../../performance/exec_time/perception/perception_" +
                                            get_current_date_time_as_string() + ".csv";
  std::string inputFilesPaths = "../../src/perception/test/point_clouds/";
 
  std::unique_ptr<GroundRemoval> ground_removal;
  std::unique_ptr<Clustering> clustering;
  std::unique_ptr<ConeDifferentiation> cone_differentiator;
  pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud;
 
  float RANSAC_eps = 0.1;
  int RANSAC_Iter = 20;
 
  float DBSCAN_dist_threshold = 0.1;
  int DBSCAN_neighbours_threshold = 1;
};
 
TEST_F(PerceptionPerformanceTest, TestPerformance) {
  for (long unsigned int it = 0; it <= 7; it++) {
    std::stringstream ss;
    ss << inputFilesPaths << "PointCloud" << it << ".pcd";
    std::string file_name = ss.str();
 
    struct PerceptionExecutionData executionTime;
 
    // Point cloud loading and transform into ROS msg
    pcl::io::loadPCDFile<pcl::PointXYZI>(file_name, *pcl_cloud);
    sensor_msgs::msg::PointCloud2 msg;
    pcl::toROSMsg(*pcl_cloud, msg);
    pcl_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>);
 
    auto start_time = std::chrono::high_resolution_clock::now();
 
    pcl::fromROSMsg(msg, *pcl_cloud);
 
    auto conversion_time = std::chrono::high_resolution_clock::now();
 
    executionTime.conversion_duration =
        std::chrono::duration_cast<std::chrono::milliseconds>(start_time - conversion_time).count();
 
    const pcl::PointCloud<pcl::PointXYZI>::Ptr ground_removed_cloud(
        new pcl::PointCloud<pcl::PointXYZI>);
 
    Plane plane;
    const SplitParameters split_params;
 
    ground_removal->ground_removal(pcl_cloud, ground_removed_cloud, plane, split_params);
 
    auto ransac_time = std::chrono::high_resolution_clock::now();
    executionTime.RANSAC_time =
        std::chrono::duration_cast<std::chrono::milliseconds>(ransac_time - conversion_time)
            .count();
 
    std::vector<Cluster> clusters;
    clustering->clustering(ground_removed_cloud, &clusters);
 
    auto dbscan_time = std::chrono::high_resolution_clock::now();
    executionTime.DBSCAN_time =
        std::chrono::duration_cast<std::chrono::milliseconds>(dbscan_time - ransac_time).count();
 
    executionTime.blues = 0;
    executionTime.yellows = 0;
    executionTime.undefineds = 0;
    for (auto cluster : clusters) {
      cone_differentiator->coneDifferentiation(&cluster);
      std::string color = cluster.get_color();
      if (color == "blue")
        executionTime.blues++;
      else if (color == "yellow")
        executionTime.yellows++;
      else
        executionTime.undefineds++;
    }
 
    auto end_time = std::chrono::high_resolution_clock::now();
    executionTime.ConeDif_time =
        std::chrono::duration_cast<std::chrono::milliseconds>(end_time - dbscan_time).count();
 
    executionTime.Total_time =
        std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(end_time - start_time)
            .count();
 
    executionTime.initial_n_points = pcl_cloud->points.size();
    executionTime.ground_removed_size = ground_removed_cloud->points.size();
 
    writeToFile(executionTime);
  }
}