expose termination criterion

koide3 · Apr 3, 2024 · 4fda016 · 4fda016
1 parent 8122528
commit 4fda016
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/include/small_gicp/registration/registration_helper.hpp b/include/small_gicp/registration/registration_helper.hpp
@@ -36,6 +36,8 @@ struct RegistrationSetting {
   double voxel_resolution = 1.0;             ///< Voxel resolution for VGICP
   double downsampling_resolution = 0.25;     ///< Downsample resolution (this will be used only in the Eigen-based interface)
   double max_correspondence_distance = 1.0;  ///< Maximum correspondence distance
+  double rotation_eps = 0.1 * M_PI / 180.0;  ///< Rotation tolerance for convergence check [rad]
+  double translation_eps = 1e-3;             ///< Translation tolerance for convergence check
   int num_threads = 4;                       ///< Number of threads
 };
 

diff --git a/src/small_gicp/registration/registration_helper.cpp b/src/small_gicp/registration/registration_helper.cpp
@@ -88,18 +88,24 @@ align(const PointCloud& target, const PointCloud& source, const KdTree<PointClou
       Registration<ICPFactor, ParallelReductionOMP> registration;
       registration.reduction.num_threads = setting.num_threads;
       registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      registration.criteria.rotation_eps = setting.rotation_eps;
+      registration.criteria.translation_eps = setting.translation_eps;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::PLANE_ICP: {
       Registration<PointToPlaneICPFactor, ParallelReductionOMP> registration;
       registration.reduction.num_threads = setting.num_threads;
       registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      registration.criteria.rotation_eps = setting.rotation_eps;
+      registration.criteria.translation_eps = setting.translation_eps;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::GICP: {
       Registration<GICPFactor, ParallelReductionOMP> registration;
       registration.reduction.num_threads = setting.num_threads;
       registration.rejector.max_dist_sq = setting.max_correspondence_distance * setting.max_correspondence_distance;
+      registration.criteria.rotation_eps = setting.rotation_eps;
+      registration.criteria.translation_eps = setting.translation_eps;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::VGICP: {
@@ -117,6 +123,8 @@ RegistrationResult align(const GaussianVoxelMap& target, const PointCloud& sourc
 
   Registration<GICPFactor, ParallelReductionOMP> registration;
   registration.reduction.num_threads = setting.num_threads;
+  registration.criteria.rotation_eps = setting.rotation_eps;
+  registration.criteria.translation_eps = setting.translation_eps;
   return registration.align(target, source, target, init_T);
 }