Speedup voxel grid build (#42)

* Coding Signed-off-by: anhnv3991 <[email protected]> * Rearrange the code to avoid duplicated processing Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Added multithread voxel grid build Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Fixed Signed-off-by: anhnv3991 <[email protected]> * Fixed wrong tmp_score usage Signed-off-by: anhnv3991 <[email protected]> * Refactor multigrid_ndt_omp (#32) * Refactoring multigrid_ndt_omp Signed-off-by: anhnv3991 <[email protected]> * Refactored multigrid_ndt_omp Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Fixed to build Signed-off-by: Shintaro Sakoda <[email protected]> * Removed line breaks Signed-off-by: Shintaro Sakoda <[email protected]> * For reference, this should be discared later Signed-off-by: anhnv3991 <[email protected]> * Testing Signed-off-by: anhnv3991 <[email protected]> * Clean up a bit Signed-off-by: anhnv3991 <[email protected]> * Clean up a bit Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * refactor: organized ndt params (#43) * Organized ndt params Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> * Speed up multigrid NDT align (#41) * Coding Signed-off-by: anhnv3991 <[email protected]> * Rearrange the code to avoid duplicated processing Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Updated reference/result.csv Signed-off-by: Shintaro Sakoda <[email protected]> * A minor fix Signed-off-by: Shintaro Sakoda <[email protected]> * refactor: organized ndt params (#43) * Organized ndt params Signed-off-by: Shintaro Sakoda <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> * Coding Signed-off-by: anhnv3991 <[email protected]> * Fixed conflicts after rebasing Signed-off-by: anhnv3991 <[email protected]> * Coding Signed-off-by: anhnv3991 <[email protected]> * Coding Signed-off-by: anhnv3991 <[email protected]> * Fixed some bugs Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Fixed to run workflow Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: anhnv3991 <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]> Co-authored-by: anhnv3991 <[email protected]> Co-authored-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> * fix: add tp to regression test (#44) * Added tp_score to regression_test Signed-off-by: Shintaro Sakoda <[email protected]> * Updated result.csv by result of GitHub Actions Signed-off-by: Shintaro Sakoda <[email protected]> * Updated by the latest result Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> * fix: add a param use line search (#46) * Added a parameter "use_line_search" Signed-off-by: Shintaro Sakoda <[email protected]> * Added default value use_line_search=false Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]> * Refactoring multigrid_ndt_omp Signed-off-by: anhnv3991 <[email protected]> * Refactored multigrid_ndt_omp Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Removed line breaks Signed-off-by: Shintaro Sakoda <[email protected]> * For reference, this should be discared later Signed-off-by: anhnv3991 <[email protected]> * Testing Signed-off-by: anhnv3991 <[email protected]> * Clean up a bit Signed-off-by: anhnv3991 <[email protected]> * Clean up a bit Signed-off-by: anhnv3991 <[email protected]> * Debugging Signed-off-by: anhnv3991 <[email protected]> * Fixed a bug Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Fixed build error Signed-off-by: anhnv3991 <[email protected]> * Updated result.csv Signed-off-by: Shintaro SAKODA <[email protected]> --------- Signed-off-by: anhnv3991 <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: anhnv3991 <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> Co-authored-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> * Coding Signed-off-by: anhnv3991 <[email protected]> * Rearrange the code to avoid duplicated processing Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Added multithread voxel grid build Signed-off-by: anhnv3991 <[email protected]> * Fixed build error Signed-off-by: anhnv3991 <[email protected]> * Fixed build errors Signed-off-by: anhnv3991 <[email protected]> * Applied clang Signed-off-by: anhnv3991 <[email protected]> * Added line break at the end of .h and .hpp files Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix * Removed unused include and comments Signed-off-by: anhnv3991 <[email protected]> * Removed unused include and fixed a segmentation fault when sync in setThreadNum Signed-off-by: anhnv3991 <[email protected]> * Fix build error when compare int with size_t Signed-off-by: anhnv3991 <[email protected]> * Fix build error when compare int with size_t Signed-off-by: anhnv3991 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: anhnv3991 <[email protected]> Signed-off-by: Shintaro Sakoda <[email protected]> Signed-off-by: Shintaro SAKODA <[email protected]> Co-authored-by: anhnv3991 <[email protected]> Co-authored-by: SakodaShintaro <[email protected]> Co-authored-by: Shintaro Sakoda <[email protected]> Co-authored-by: SakodaShintaro <[email protected]>
tier4 · Mar 26, 2024 · 4776042 · 4776042
1 parent e213b40
commit 4776042
Show file tree

Hide file tree

Showing 4 changed files with 155 additions and 10 deletions.
diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h
@@ -61,6 +61,8 @@
 #include <pcl/kdtree/kdtree_flann.h>
 #include <Eigen/Dense>
 #include <Eigen/Cholesky>
+#include <future>
+
 namespace pclomp {
 /** \brief A searchable voxel structure containing the mean and covariance of the data.
  * \note For more information please see
@@ -142,20 +144,69 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
       return *this;
     }
 
+    /** \brief Get the voxel covariance.
+     * \return covariance matrix
+     */
+    const Eigen::Matrix3d &getCov() const {
+      return (cov_);
+    }
+    Eigen::Matrix3d &getCov() {
+      return (cov_);
+    }
+
     /** \brief Get the inverse of the voxel covariance.
      * \return inverse covariance matrix
      */
     const Eigen::Matrix3d &getInverseCov() const {
       return (icov_);
     }
 
+    Eigen::Matrix3d &getInverseCov() {
+      return (icov_);
+    }
+
     /** \brief Get the voxel centroid.
      * \return centroid
      */
     const Eigen::Vector3d &getMean() const {
       return (mean_);
     }
 
+    Eigen::Vector3d &getMean() {
+      return (mean_);
+    }
+
+    /** \brief Get the eigen vectors of the voxel covariance.
+     * \note Order corresponds with \ref getEvals
+     * \return matrix whose columns contain eigen vectors
+     */
+    const Eigen::Matrix3d &getEvecs() const {
+      return (evecs_);
+    }
+
+    Eigen::Matrix3d &getEvecs() {
+      return (evecs_);
+    }
+
+    /** \brief Get the eigen values of the voxel covariance.
+     * \note Order corresponds with \ref getEvecs
+     * \return vector of eigen values
+     */
+    const Eigen::Vector3d &getEvals() const {
+      return (evals_);
+    }
+
+    Eigen::Vector3d &getEvals() {
+      return (evals_);
+    }
+
+    /** \brief Get the number of points contained by this voxel.
+     * \return number of points
+     */
+    int getPointCount() const {
+      return (nr_points_);
+    }
+
     /** \brief Number of points contained by voxel */
     int nr_points_;
 
@@ -205,6 +256,9 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
     min_b_.setZero();
     max_b_.setZero();
     filter_name_ = "MultiVoxelGridCovariance";
+
+    setThreadNum(1);
+    last_check_tid_ = -1;
   }
 
   MultiVoxelGridCovariance(const MultiVoxelGridCovariance &other);
@@ -252,7 +306,66 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
   // Return the string indices of currently loaded map pieces
   std::vector<std::string> getCurrentMapIDs() const;
 
+  void setThreadNum(int thread_num) {
+    sync();
+
+    if(thread_num <= 0) {
+      thread_num_ = 1;
+    }
+
+    thread_num_ = thread_num;
+    thread_futs_.resize(thread_num_);
+    processing_inputs_.resize(thread_num_);
+  }
+
+  ~MultiVoxelGridCovariance() {
+    // Stop all threads in the case an intermediate interrupt occurs
+    sync();
+  }
+
 protected:
+  // Return the index of an idle thread, which is not running any
+  // job, or has already finished its job and waiting for a join.
+  inline int get_idle_tid() {
+    int tid = (last_check_tid_ == thread_num_ - 1) ? 0 : last_check_tid_ + 1;
+    std::chrono::microseconds span(50);
+
+    // Loop until an idle thread is found
+    while(true) {
+      // Return immediately if a thread that has not been given a job is found
+      if(!thread_futs_[tid].valid()) {
+        last_check_tid_ = tid;
+        return tid;
+      }
+
+      // If no such thread is found, wait for the current thread to finish its job
+      if(thread_futs_[tid].wait_for(span) == std::future_status::ready) {
+        last_check_tid_ = tid;
+        return tid;
+      }
+
+      // If the current thread has not finished its job, check the next thread
+      tid = (tid == thread_num_ - 1) ? 0 : tid + 1;
+    }
+  }
+
+  // Wait for all running threads to finish
+  inline void sync() {
+    for(auto &tf : thread_futs_) {
+      if(tf.valid()) {
+        tf.wait();
+      }
+    }
+  }
+
+  // A wraper of the real applyFilter
+  inline bool applyFilterThread(int tid, GridNodeType &node) {
+    applyFilter(processing_inputs_[tid], node);
+    processing_inputs_[tid].reset();
+
+    return true;
+  }
+
   /** \brief Filter cloud and initializes voxel structure.
    * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
    */
@@ -274,6 +387,12 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
   // Used to build a kdtree for radius search
   PointCloudPtr voxel_centroids_ptr_;
 
+  // Thread pooling, for parallel processing
+  int thread_num_;
+  std::vector<std::future<bool>> thread_futs_;
+  std::vector<PointCloudConstPtr> processing_inputs_;
+  int last_check_tid_;
+
   // A map to convert string index to integer index, used for grids
   std::map<std::string, int> sid_to_iid_;
   // Grids of leaves are held in a vector for faster access speed

diff --git a/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp b/include/multigrid_pclomp/multi_voxel_grid_covariance_omp_impl.hpp
@@ -69,13 +69,19 @@ MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(const MultiVoxelGridC
   if(other.voxel_centroids_ptr_) {
     *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
   }
+
+  setThreadNum(other.thread_num_);
+  last_check_tid_ = -1;
 }
 
 template<typename PointT>
 MultiVoxelGridCovariance<PointT>::MultiVoxelGridCovariance(MultiVoxelGridCovariance &&other)
     : pcl::VoxelGrid<PointT>(std::move(other)), voxel_centroids_ptr_(std::move(other.voxel_centroids_ptr_)), sid_to_iid_(std::move(other.sid_to_iid_)), grid_list_(std::move(other.grid_list_)), kdtree_(std::move(other.kdtree_)), leaf_ptrs_(std::move(other.leaf_ptrs_)) {
   min_points_per_voxel_ = other.min_points_per_voxel_;
   min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
+
+  setThreadNum(other.thread_num_);
+  last_check_tid_ = -1;
 }
 
 template<typename PointT>
@@ -96,6 +102,9 @@ MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::oper
     *voxel_centroids_ptr_ = *other.voxel_centroids_ptr_;
   }
 
+  setThreadNum(other.thread_num_);
+  last_check_tid_ = -1;
+
   return *this;
 }
 
@@ -111,6 +120,9 @@ MultiVoxelGridCovariance<PointT> &pclomp::MultiVoxelGridCovariance<PointT>::oper
   min_points_per_voxel_ = other.min_points_per_voxel_;
   min_covar_eigvalue_mult_ = other.min_covar_eigvalue_mult_;
 
+  setThreadNum(other.thread_num_);
+  last_check_tid_ = -1;
+
   return *this;
 }
 
@@ -124,25 +136,36 @@ void MultiVoxelGridCovariance<PointT>::setInputCloudAndFilter(const PointCloudCo
 
   sid_to_iid_[grid_id] = grid_list_.size() - 1;
 
-  applyFilter(cloud, *new_grid);
+  // If single thread, no parallel processing
+  if(thread_num_ == 1) {
+    applyFilter(cloud, *new_grid);
+  } else {
+    int idle_tid = get_idle_tid();
+
+    processing_inputs_[idle_tid] = cloud;
+    thread_futs_[idle_tid] = std::async(std::launch::async, &MultiVoxelGridCovariance<PointT>::applyFilterThread, this, idle_tid, std::ref(*new_grid));
+  }
 }
 
 template<typename PointT>
 void MultiVoxelGridCovariance<PointT>::removeCloud(const std::string &grid_id) {
-  if(sid_to_iid_.count(grid_id) == 0) {
+  auto iid = sid_to_iid_.find(grid_id);
+
+  if(iid == sid_to_iid_.end()) {
     return;
   }
 
-  const auto [sid, iid] = *(sid_to_iid_.find(grid_id));
-
   // Set the pointer corresponding to the specified grid to null
-  grid_list_[iid].reset();
+  grid_list_[iid->second].reset();
   // Remove the specified grid from the conversion map
-  sid_to_iid_.erase(sid);
+  sid_to_iid_.erase(iid);
 }
 
 template<typename PointT>
 void MultiVoxelGridCovariance<PointT>::createKdtree() {
+  // Wait for all threads to finish
+  sync();
+
   // Rebuild the grid_list_ and sid_to_iid_ to delete the data related to the removed clouds
   const int new_grid_num = sid_to_iid_.size();
   std::vector<GridNodePtr> new_grid_list(new_grid_num);
@@ -167,8 +190,8 @@ void MultiVoxelGridCovariance<PointT>::createKdtree() {
   leaf_ptrs_.clear();
   leaf_ptrs_.reserve(total_leaf_num);
 
-  for(const GridNodePtr &grid_ptr : grid_list_) {
-    for(const Leaf &leaf : *grid_ptr) {
+  for(const auto &grid_ptr : grid_list_) {
+    for(const auto &leaf : *grid_ptr) {
       PointT new_leaf;
 
       new_leaf.x = leaf.centroid_[0];
@@ -230,6 +253,7 @@ std::vector<std::string> MultiVoxelGridCovariance<PointT>::getCurrentMapIDs() co
   for(const auto &element : sid_to_iid_) {
     output.push_back(element.first);
   }
+
   return output;
 }
 

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp.h b/include/multigrid_pclomp/multigrid_ndt_omp.h
@@ -121,6 +121,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
 
   void setNumThreads(int n) {
     params_.num_threads = n;
+
+    target_cells_.setThreadNum(params_.num_threads);
   }
 
   inline int getNumThreads() const {
@@ -315,6 +317,8 @@ class MultiGridNormalDistributionsTransform : public pcl::Registration<PointSour
   void setParams(const NdtParams &ndt_params) {
     params_ = ndt_params;
     max_iterations_ = params_.max_iterations;
+
+    target_cells_.setThreadNum(params_.num_threads);
   }
 
   NdtParams getParams() const {

diff --git a/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp b/include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
@@ -611,8 +611,6 @@ void MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeHes
     auto &x_pt = (*input_)[idx];
     // For math
     Eigen::Vector3d x(x_pt.x, x_pt.y, x_pt.z);
-
-    // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
     const Eigen::Vector3d x_trans(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
 
     auto &point_gradient = t_point_gradients[tid];