expose verbose option to python (#77)

* expose verbose option to python

* tweak convergence check

include/small_gicp/ann/incremental_voxelmap.hpp

@@ -141,7 +141,7 @@ public:
 
   /// @brief Calculate the global point index from the voxel index and the point index.
   inline size_t calc_index(const size_t voxel_id, const size_t point_id) const { return (voxel_id << point_id_bits) | point_id; }
-  inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; }                ///< Extract the point ID from a global index.
+  inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; }                 ///< Extract the point ID from a global index.
   inline size_t point_id(const size_t i) const { return i & ((1ull << point_id_bits) - 1); }  ///< Extract the voxel ID from a global index.
 
 public:

include/small_gicp/registration/optimizer.hpp

@@ -118,7 +118,7 @@ struct LevenbergMarquardtOptimizer {
                     << " dr=" << delta.head<3>().norm() << std::endl;
         }
 
-        if (new_e < e) {
+        if (new_e <= e) {
           // Error decreased, decrease lambda
           result.converged = criteria.converged(delta);
           result.T_target_source = new_T;

include/small_gicp/registration/registration_helper.hpp

@@ -40,6 +40,7 @@ struct RegistrationSetting {
   double translation_eps = 1e-3;             ///< Translation tolerance for convergence check
   int num_threads = 4;                       ///< Number of threads
   int max_iterations = 20;                   ///< Maximum number of iterations
+  bool verbose = false;                      ///< Verbose mode
 };
 
 /// @brief Align point clouds

include/small_gicp/registration/termination_criteria.hpp

@@ -14,7 +14,7 @@ struct TerminationCriteria {
   /// @brief Check the convergence
   /// @param delta  Transformation update vector
   /// @return       True if converged
-  bool converged(const Eigen::Matrix<double, 6, 1>& delta) const { return delta.template head<3>().norm() < rotation_eps && delta.template tail<3>().norm() < translation_eps; }
+  bool converged(const Eigen::Matrix<double, 6, 1>& delta) const { return delta.template head<3>().norm() <= rotation_eps && delta.template tail<3>().norm() <= translation_eps; }
 
   double translation_eps;  ///< Rotation tolerance [rad]
   double rotation_eps;     ///< Translation tolerance

include/small_gicp/util/downsampling.hpp

@@ -39,7 +39,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling(const InputPointCloud& poin
     }
 
     // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
-    const std::uint64_t bits =                                 //
+    const std::uint64_t bits =                                                           //
       (static_cast<std::uint64_t>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) |  //
       (static_cast<std::uint64_t>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) |  //
       (static_cast<std::uint64_t>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));

include/small_gicp/util/downsampling_omp.hpp

@@ -39,7 +39,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
       continue;
     }
     // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit)
-    const std::uint64_t bits =                                 //
+    const std::uint64_t bits =                                                           //
       (static_cast<std::uint64_t>(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) |  //
       (static_cast<std::uint64_t>(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) |  //
       (static_cast<std::uint64_t>(coord[2] & coord_bit_mask) << (coord_bit_size * 2));
@@ -47,11 +47,7 @@ std::shared_ptr<OutputPointCloud> voxelgrid_sampling_omp(const InputPointCloud&
   }
 
   // Sort by voxel coords
-  quick_sort_omp(
+  quick_sort_omp(coord_pt.begin(), coord_pt.end(), [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }, num_threads);
-    coord_pt.begin(),
-    coord_pt.end(),
-    [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; },
-    num_threads);
 
   auto downsampled = std::make_shared<OutputPointCloud>();
   traits::resize(*downsampled, traits::size(points));

src/python/align.cpp

@@ -32,7 +32,8 @@ void define_align(py::module& m) {
       double downsampling_resolution,
       double max_correspondence_distance,
       int num_threads,
-      int max_iterations) {
+      int max_iterations,
+      bool verbose) {
       if (target_points.cols() != 3 && target_points.cols() != 4) {
         std::cerr << "target_points must be Nx3 or Nx4" << std::endl;
         return RegistrationResult(Eigen::Isometry3d::Identity());
@@ -60,6 +61,8 @@ void define_align(py::module& m) {
       setting.downsampling_resolution = downsampling_resolution;
       setting.max_correspondence_distance = max_correspondence_distance;
       setting.num_threads = num_threads;
+      setting.max_iterations = max_iterations;
+      setting.verbose = verbose;
 
       std::vector<Eigen::Vector4d> target(target_points.rows());
       if (target_points.cols() == 3) {
@@ -94,6 +97,7 @@ void define_align(py::module& m) {
     py::arg("max_correspondence_distance") = 1.0,
     py::arg("num_threads") = 1,
     py::arg("max_iterations") = 20,
+    py::arg("verbose") = false,
     R"pbdoc(
         Align two point clouds using various ICP-like algorithms.
 
@@ -117,6 +121,8 @@ void define_align(py::module& m) {
             Number of threads to use for parallel processing.
         max_iterations : int = 20
             Maximum number of iterations for the optimization algorithm.
+        verbose : bool = False
+            If True, print debug information during the optimization process.
 
         Returns
         -------
@@ -135,7 +141,8 @@ void define_align(py::module& m) {
       const std::string& registration_type,
       double max_correspondence_distance,
       int num_threads,
-      int max_iterations) {
+      int max_iterations,
+      bool verbose) {
       RegistrationSetting setting;
       if (registration_type == "ICP") {
         setting.type = RegistrationSetting::ICP;
@@ -149,6 +156,8 @@ void define_align(py::module& m) {
       }
       setting.max_correspondence_distance = max_correspondence_distance;
       setting.num_threads = num_threads;
+      setting.max_iterations = max_iterations;
+      setting.verbose = verbose;
 
       if (target_tree == nullptr) {
         target_tree = std::make_shared<KdTree<PointCloud>>(target, KdTreeBuilderOMP(num_threads));
@@ -163,6 +172,7 @@ void define_align(py::module& m) {
     py::arg("max_correspondence_distance") = 1.0,
     py::arg("num_threads") = 1,
     py::arg("max_iterations") = 20,
+    py::arg("verbose") = false,
     R"pbdoc(
     Align two point clouds using specified ICP-like algorithms, utilizing point cloud and KD-tree inputs.
 
@@ -184,6 +194,8 @@ void define_align(py::module& m) {
         Number of threads to use for computation.
     max_iterations : int = 20
         Maximum number of iterations for the optimization algorithm.
+    verbose : bool = False
+        If True, print debug information during the optimization process.
 
     Returns
     -------
@@ -200,11 +212,13 @@ void define_align(py::module& m) {
       const Eigen::Matrix4d& init_T_target_source,
       double max_correspondence_distance,
       int num_threads,
-      int max_iterations) {
+      int max_iterations,
+      bool verbose) {
       Registration<GICPFactor, ParallelReductionOMP> registration;
       registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
       registration.reduction.num_threads = num_threads;
       registration.optimizer.max_iterations = max_iterations;
+      registration.optimizer.verbose = verbose;
 
       return registration.align(target_voxelmap, source, target_voxelmap, Eigen::Isometry3d(init_T_target_source));
     },
@@ -214,6 +228,7 @@ void define_align(py::module& m) {
     py::arg("max_correspondence_distance") = 1.0,
     py::arg("num_threads") = 1,
     py::arg("max_iterations") = 20,
+    py::arg("verbose") = false,
     R"pbdoc(
     Align two point clouds using voxel-based GICP algorithm, utilizing a Gaussian Voxel Map.
 
@@ -231,6 +246,8 @@ void define_align(py::module& m) {
         Number of threads to use for computation.
     max_iterations : int = 20
         Maximum number of iterations for the optimization algorithm.
+    verbose : bool = False
+        If True, print debug information during the optimization process.
 
     Returns
     -------

src/small_gicp/registration/registration_helper.cpp

@@ -91,6 +91,7 @@ align(const PointCloud& target, const PointCloud& source, const KdTree<PointClou
       registration.criteria.rotation_eps = setting.rotation_eps;
       registration.criteria.translation_eps = setting.translation_eps;
       registration.optimizer.max_iterations = setting.max_iterations;
+      registration.optimizer.verbose = setting.verbose;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::PLANE_ICP: {
@@ -100,6 +101,7 @@ align(const PointCloud& target, const PointCloud& source, const KdTree<PointClou
       registration.criteria.rotation_eps = setting.rotation_eps;
       registration.criteria.translation_eps = setting.translation_eps;
       registration.optimizer.max_iterations = setting.max_iterations;
+      registration.optimizer.verbose = setting.verbose;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::GICP: {
@@ -109,6 +111,7 @@ align(const PointCloud& target, const PointCloud& source, const KdTree<PointClou
       registration.criteria.rotation_eps = setting.rotation_eps;
       registration.criteria.translation_eps = setting.translation_eps;
       registration.optimizer.max_iterations = setting.max_iterations;
+      registration.optimizer.verbose = setting.verbose;
       return registration.align(target, source, target_tree, init_T);
     }
     case RegistrationSetting::VGICP: {
@@ -129,6 +132,7 @@ RegistrationResult align(const GaussianVoxelMap& target, const PointCloud& sourc
   registration.criteria.rotation_eps = setting.rotation_eps;
   registration.criteria.translation_eps = setting.translation_eps;
   registration.optimizer.max_iterations = setting.max_iterations;
+  registration.optimizer.verbose = setting.verbose;
   return registration.align(target, source, target, init_T);
 }