support arbitrary search method in fast_gicp (#120)

* Link against openmp library target * use march=native on non arm * support arbitrary search method in fast_gicp. de facto will only work when source and target are the same * fix simd flags for macos arm * Add hints how to build on macos with brew * rename kdtree members which dont have to be kdtree * Use PCL_DEFINITIONS to avoid crashes caused by different sse flags between PCL and fast_gicp * fix off stream bug (#123) Co-authored-by: k.koide <[email protected]> --------- Co-authored-by: koide3 <[email protected]> Co-authored-by: k.koide <[email protected]>
koide3 · Oct 1, 2023 · 6d99a9c · 6d99a9c
1 parent bb36054
commit 6d99a9c
Show file tree

Hide file tree

Showing 8 changed files with 63 additions and 63 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -6,17 +6,11 @@ option(BUILD_apps "Build application programs" ON)
 option(BUILD_test "Build test programs" OFF)
 option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF)
 
-if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64")
-else()
-  add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
-  set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-  set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-endif()
-
 set(CMAKE_BUILD_TYPE "Release")
 
 find_package(PCL REQUIRED)
 find_package(Eigen3 REQUIRED)
+add_definitions(${PCL_DEFINITIONS})
 
 if(DEFINED ENV{ROS_VERSION})
   set(ROS_VERSION $ENV{ROS_VERSION})
@@ -150,7 +144,7 @@ if(BUILD_VGICP_CUDA)
   )
   add_dependencies(fast_gicp fast_vgicp_cuda)
   if(catkin_FOUND)
-    install(TARGETS fast_vgicp_cuda  
+    install(TARGETS fast_vgicp_cuda
       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
   endif()
 endif()
@@ -171,7 +165,7 @@ if(catkin_FOUND)
   ###################################
   install(TARGETS ${PROJECT_NAME}
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-  
+
   install(DIRECTORY include/
     DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
     FILES_MATCHING PATTERN "*.hpp")

diff --git a/README.md b/README.md
@@ -23,6 +23,12 @@ This package is a collection of GICP-based fast point cloud registration algorit
 
 We have tested this package on Ubuntu 18.04/20.04 and CUDA 11.1.
 
+On macOS when using `brew`, you might have to set up your depenencies like this
+
+```
+cmake .. "-DCMAKE_PREFIX_PATH=$(brew --prefix libomp)[;other-custom-prefixes]" -DQt5_DIR=$(brew --prefix qt@5)lib/cmake/Qt5
+```
+
 ### CUDA
 
 To enable the CUDA-powered implementations, set ```BUILD_VGICP_CUDA``` cmake option to ```ON```.

diff --git a/include/fast_gicp/gicp/fast_gicp.hpp b/include/fast_gicp/gicp/fast_gicp.hpp
@@ -6,7 +6,7 @@
 
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
-#include <pcl/search/kdtree.h>
+#include <pcl/search/search.h>
 #include <pcl/registration/registration.h>
 #include <fast_gicp/gicp/lsq_registration.hpp>
 #include <fast_gicp/gicp/gicp_settings.hpp>
@@ -16,7 +16,7 @@ namespace fast_gicp {
 /**
  * @brief Fast GICP algorithm optimized for multi threading with OpenMP
  */
-template<typename PointSource, typename PointTarget>
+template<typename PointSource, typename PointTarget, typename SearchMethodSource = pcl::search::KdTree<PointSource>, typename SearchMethodTarget = pcl::search::KdTree<PointTarget>>
 class FastGICP : public LsqRegistration<PointSource, PointTarget> {
 public:
   using Scalar = float;
@@ -79,16 +79,16 @@ class FastGICP : public LsqRegistration<PointSource, PointTarget> {
   virtual double compute_error(const Eigen::Isometry3d& trans) override;
 
   template<typename PointT>
-  bool calculate_covariances(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances);
+  bool calculate_covariances(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, pcl::search::Search<PointT>& kdtree, std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances);
 
 protected:
   int num_threads_;
   int k_correspondences_;
 
   RegularizationMethod regularization_method_;
 
-  std::shared_ptr<pcl::search::KdTree<PointSource>> source_kdtree_;
-  std::shared_ptr<pcl::search::KdTree<PointTarget>> target_kdtree_;
+  std::shared_ptr<SearchMethodSource> search_source_;
+  std::shared_ptr<SearchMethodTarget> search_target_;
 
   std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> source_covs_;
   std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> target_covs_;
@@ -100,4 +100,4 @@ class FastGICP : public LsqRegistration<PointSource, PointTarget> {
 };
 }  // namespace fast_gicp
 
-#endif
+#endif
diff --git a/include/fast_gicp/gicp/fast_gicp_st.hpp b/include/fast_gicp/gicp/fast_gicp_st.hpp
@@ -38,7 +38,7 @@ class FastGICPSingleThread : public FastGICP<PointSource, PointTarget> {
   using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
   using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
 
-  using FastGICP<PointSource, PointTarget>::target_kdtree_;
+  using FastGICP<PointSource, PointTarget>::search_target_;
   using FastGICP<PointSource, PointTarget>::correspondences_;
   using FastGICP<PointSource, PointTarget>::sq_distances_;
   using FastGICP<PointSource, PointTarget>::source_covs_;

diff --git a/include/fast_gicp/gicp/fast_vgicp.hpp b/include/fast_gicp/gicp/fast_vgicp.hpp
@@ -47,8 +47,8 @@ class FastVGICP : public FastGICP<PointSource, PointTarget> {
   using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
 
   using FastGICP<PointSource, PointTarget>::num_threads_;
-  using FastGICP<PointSource, PointTarget>::source_kdtree_;
-  using FastGICP<PointSource, PointTarget>::target_kdtree_;
+  using FastGICP<PointSource, PointTarget>::search_source_;
+  using FastGICP<PointSource, PointTarget>::search_target_;
   using FastGICP<PointSource, PointTarget>::source_covs_;
   using FastGICP<PointSource, PointTarget>::target_covs_;
 

diff --git a/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp b/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp
@@ -5,8 +5,8 @@
 
 namespace fast_gicp {
 
-template <typename PointSource, typename PointTarget>
-FastGICP<PointSource, PointTarget>::FastGICP() {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::FastGICP() {
 #ifdef _OPENMP
   num_threads_ = omp_get_max_threads();
 #else
@@ -18,15 +18,15 @@ FastGICP<PointSource, PointTarget>::FastGICP() {
   corr_dist_threshold_ = std::numeric_limits<float>::max();
 
   regularization_method_ = RegularizationMethod::PLANE;
-  source_kdtree_.reset(new pcl::search::KdTree<PointSource>);
-  target_kdtree_.reset(new pcl::search::KdTree<PointTarget>);
+  search_source_.reset(new SearchMethodSource);
+  search_target_.reset(new SearchMethodTarget);
 }
 
-template <typename PointSource, typename PointTarget>
-FastGICP<PointSource, PointTarget>::~FastGICP() {}
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::~FastGICP() {}
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setNumThreads(int n) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setNumThreads(int n) {
   num_threads_ = n;
 
 #ifdef _OPENMP
@@ -36,86 +36,86 @@ void FastGICP<PointSource, PointTarget>::setNumThreads(int n) {
 #endif
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setCorrespondenceRandomness(int k) {
   k_correspondences_ = k;
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setRegularizationMethod(RegularizationMethod method) {
   regularization_method_ = method;
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::swapSourceAndTarget() {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::swapSourceAndTarget() {
   input_.swap(target_);
-  source_kdtree_.swap(target_kdtree_);
+  search_source_.swap(search_target_);
   source_covs_.swap(target_covs_);
 
   correspondences_.clear();
   sq_distances_.clear();
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::clearSource() {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::clearSource() {
   input_.reset();
   source_covs_.clear();
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::clearTarget() {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::clearTarget() {
   target_.reset();
   target_covs_.clear();
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
   if (input_ == cloud) {
     return;
   }
 
   pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
-  source_kdtree_->setInputCloud(cloud);
+  search_source_->setInputCloud(cloud);
   source_covs_.clear();
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
   if (target_ == cloud) {
     return;
   }
   pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
-  target_kdtree_->setInputCloud(cloud);
+  search_target_->setInputCloud(cloud);
   target_covs_.clear();
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
   source_covs_ = covs;
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
   target_covs_ = covs;
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
   if (output.points.data() == input_->points.data() || output.points.data() == target_->points.data()) {
     throw std::invalid_argument("FastGICP: destination cloud cannot be identical to source or target");
   }
   if (source_covs_.size() != input_->size()) {
-    calculate_covariances(input_, *source_kdtree_, source_covs_);
+    calculate_covariances(input_, *search_source_, source_covs_);
   }
   if (target_covs_.size() != target_->size()) {
-    calculate_covariances(target_, *target_kdtree_, target_covs_);
+    calculate_covariances(target_, *search_target_, target_covs_);
   }
 
   LsqRegistration<PointSource, PointTarget>::computeTransformation(output, guess);
 }
 
-template <typename PointSource, typename PointTarget>
-void FastGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& trans) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+void FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::update_correspondences(const Eigen::Isometry3d& trans) {
   assert(source_covs_.size() == input_->size());
   assert(target_covs_.size() == target_->size());
 
@@ -133,7 +133,7 @@ void FastGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Iso
     PointTarget pt;
     pt.getVector4fMap() = trans_f * input_->at(i).getVector4fMap();
 
-    target_kdtree_->nearestKSearch(pt, 1, k_indices, k_sq_dists);
+    search_target_->nearestKSearch(pt, 1, k_indices, k_sq_dists);
 
     sq_distances_[i] = k_sq_dists[0];
     correspondences_[i] = k_sq_dists[0] < corr_dist_threshold_ * corr_dist_threshold_ ? k_indices[0] : -1;
@@ -154,8 +154,8 @@ void FastGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Iso
   }
 }
 
-template <typename PointSource, typename PointTarget>
-double FastGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+double FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
   update_correspondences(trans);
 
   double sum_errors = 0.0;
@@ -213,8 +213,8 @@ double FastGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& tr
   return sum_errors;
 }
 
-template <typename PointSource, typename PointTarget>
-double FastGICP<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
+double FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::compute_error(const Eigen::Isometry3d& trans) {
   double sum_errors = 0.0;
 
 #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8)
@@ -239,11 +239,11 @@ double FastGICP<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d
   return sum_errors;
 }
 
-template <typename PointSource, typename PointTarget>
+template <typename PointSource, typename PointTarget, typename SearchMethodSource, typename SearchMethodTarget>
 template <typename PointT>
-bool FastGICP<PointSource, PointTarget>::calculate_covariances(
+bool FastGICP<PointSource, PointTarget, SearchMethodSource, SearchMethodTarget>::calculate_covariances(
   const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
-  pcl::search::KdTree<PointT>& kdtree,
+  pcl::search::Search<PointT>& kdtree,
   std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances) {
   if (kdtree.getInputCloud() != cloud) {
     kdtree.setInputCloud(cloud);

diff --git a/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp b/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp
@@ -53,7 +53,7 @@ void FastGICPSingleThread<PointSource, PointTarget>::update_correspondences(cons
       }
     }
 
-    target_kdtree_->nearestKSearch(pt, 2, k_indices, k_sq_dists);
+    search_target_->nearestKSearch(pt, 2, k_indices, k_sq_dists);
 
     correspondences_[i] = k_sq_dists[0] < this->corr_dist_threshold_ * this->corr_dist_threshold_ ? k_indices[0] : -1;
     sq_distances_[i] = k_sq_dists[0];

diff --git a/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp b/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp
@@ -45,7 +45,7 @@ void FastVGICP<PointSource, PointTarget>::setVoxelAccumulationMode(VoxelAccumula
 template <typename PointSource, typename PointTarget>
 void FastVGICP<PointSource, PointTarget>::swapSourceAndTarget() {
   input_.swap(target_);
-  source_kdtree_.swap(target_kdtree_);
+  search_source_.swap(search_target_);
   source_covs_.swap(target_covs_);
   voxelmap_.reset();
   voxel_correspondences_.clear();