Optimizing radiusSearch speed

Signed-off-by: anhnv3991 <anh.nguyen.2@tier4.jp>

Author: anhnv3991

localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp

@@ -198,6 +198,13 @@ class NDTScanMatcher : public rclcpp::Node
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
   HyperParameters param_;
+
+  // For debug
+  std::ofstream exe_log_file_;
 };
 
+#ifndef timeDiff
+#define timeDiff(start, end) ((end.tv_sec - start.tv_sec) * 1000000 + end.tv_usec - start.tv_usec)
+#endif
+
 #endif  // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -151,6 +151,9 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
     return false;  // No update
   }
 
+  // For debug
+  std::ofstream test_file("/home/anh/Work/autoware/exe_time.csv", std::ios::app);
+
   const auto exe_start_time = std::chrono::system_clock::now();
   // Perform heavy processing outside of the lock scope
 
@@ -174,6 +177,8 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<double>(duration_micro_sec) / 1000.0;
   RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time);
+
+  test_file << exe_time << std::endl;
   return true;  // Updated
 }
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -185,6 +185,9 @@ NDTScanMatcher::NDTScanMatcher()
     std::make_unique<MapUpdateModule>(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading);
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  // For debug
+  exe_log_file_.open("/home/anh/Work/autoware/align_cost.csv");
 }
 
 void NDTScanMatcher::publish_diagnostic()
@@ -330,11 +333,13 @@ void NDTScanMatcher::callback_sensor_points(
     // return;
   }
 
+  // Put the time counter here to measure the actual processing time
+  // = time of waiting for unlock + time of align
+  const auto exe_start_time = std::chrono::system_clock::now();
+
   // mutex ndt_ptr_
   std::lock_guard<std::mutex> lock(ndt_ptr_mtx_);
 
-  const auto exe_start_time = std::chrono::system_clock::now();
-
   // preprocess input pointcloud
   pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_in_sensor_frame(
     new pcl::PointCloud<PointSource>);
@@ -425,6 +430,10 @@ void NDTScanMatcher::callback_sensor_points(
     std::chrono::duration_cast<std::chrono::microseconds>(exe_end_time - exe_start_time).count();
   const auto exe_time = static_cast<float>(duration_micro_sec) / 1000.0f;
 
+  // For debug
+  exe_log_file_ << exe_time << "," << ndt_result.iteration_num << std::endl;
+  // End
+
   // publish
   initial_pose_with_covariance_pub_->publish(interpolation_result.interpolated_pose);
   exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time));