Add param (#17)

ToshikiNakamura0412 · Jun 22, 2024 · 173cead · 173cead
1 parent fb92e85
commit 173cead
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/include/icp_matching/icp_matching.h b/include/icp_matching/icp_matching.h
@@ -8,6 +8,7 @@
 #ifndef ICP_MATCHING_ICP_MATCHING_H
 #define ICP_MATCHING_ICP_MATCHING_H
 
+#include <limits>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_cloud.h>
@@ -59,7 +60,8 @@ class ICPMatching
    */
   void align(
       const PointCloudT::Ptr cloud_src, const PointCloudT::Ptr cloud_target, PointCloudT &cloud_src_registered,
-      const int iterations = 10);
+      const int iterations = 10,
+      const double max_correspondence_distance = std::sqrt(std::numeric_limits<double>::max()));
 
   /**
    * @brief Check if ICP has converged

diff --git a/src/icp_matching.cpp b/src/icp_matching.cpp
@@ -35,13 +35,16 @@ void ICPMatching::voxel_grid_filter(
 
 void ICPMatching::align(
     const PointCloudT::Ptr cloud_src, const PointCloudT::Ptr cloud_target, PointCloudT &cloud_src_registered,
-    const int iterations)
+    const int iterations, const double max_correspondence_distance)
 {
   pcl::IterativeClosestPoint<PointT, PointT> icp;
 
   icp.setInputSource(cloud_src);
   icp.setInputTarget(cloud_target);
   icp.setMaximumIterations(iterations);
+  icp.setMaxCorrespondenceDistance(max_correspondence_distance);
+  icp.setTransformationEpsilon(1e-8);
+  icp.setEuclideanFitnessEpsilon(1e-8);
   icp.align(cloud_src_registered);
 
   has_converged_ = icp.hasConverged();