Speedup voxel grid build

include/multigrid_pclomp/multi_voxel_grid_covariance_omp.h

@@ -61,6 +61,16 @@
 #include <pcl/kdtree/kdtree_flann.h>
 #include <Eigen/Dense>
 #include <Eigen/Cholesky>
+
+#include <future>
+#include <ctime>
+#include <fstream>
+#include <sstream>
+#include <sys/time.h>
+#include <pcl/io/pcd_io.h>
+#include <queue>
+#include <unistd.h>
+
 namespace pclomp {
 /** \brief A searchable voxel structure containing the mean and covariance of the data.
  * \note For more information please see
@@ -142,20 +152,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 +264,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 +314,65 @@ 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) {
+    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) {
+      // gettimeofday(&start, NULL);
+      // 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(int i = 0; i < thread_num_; ++i) {
+      if(thread_futs_[i].valid()) {
+        thread_futs_[i].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 +394,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
@@ -285,4 +411,4 @@ class MultiVoxelGridCovariance : public pcl::VoxelGrid<PointT> {
 };
 }  // namespace pclomp
 
-#endif  // #ifndef PCL_MULTI_VOXEL_GRID_COVARIANCE_H_
+#endif  // #ifndef PCL_MULTI_VOXEL_GRID_COVARIANCE_H_

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;
 }
 
@@ -386,4 +410,4 @@ void pclomp::MultiVoxelGridCovariance<PointT>::computeLeafParams(const Eigen::Ve
 
 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
 
-#endif  // PCL_MULTI_VOXEL_GRID_COVARIANCE_IMPL_H_
+#endif  // PCL_MULTI_VOXEL_GRID_COVARIANCE_IMPL_H_

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 {

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