diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp index db6a3d5e..73ecba14 100644 --- a/include/small_gicp/ann/incremental_voxelmap.hpp +++ b/include/small_gicp/ann/incremental_voxelmap.hpp @@ -141,7 +141,7 @@ struct IncrementalVoxelMap { /// @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: diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp index c7864057..aa682b0d 100644 --- a/include/small_gicp/registration/optimizer.hpp +++ b/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; diff --git a/include/small_gicp/registration/registration_helper.hpp b/include/small_gicp/registration/registration_helper.hpp index 3fd84639..8cb9ec6d 100644 --- a/include/small_gicp/registration/registration_helper.hpp +++ b/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 diff --git a/include/small_gicp/registration/termination_criteria.hpp b/include/small_gicp/registration/termination_criteria.hpp index 223e7238..87d7447e 100644 --- a/include/small_gicp/registration/termination_criteria.hpp +++ b/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& delta) const { return delta.template head<3>().norm() < rotation_eps && delta.template tail<3>().norm() < translation_eps; } + bool converged(const Eigen::Matrix& 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 diff --git a/include/small_gicp/util/downsampling.hpp b/include/small_gicp/util/downsampling.hpp index 8f7383d4..7d6ff0de 100644 --- a/include/small_gicp/util/downsampling.hpp +++ b/include/small_gicp/util/downsampling.hpp @@ -39,7 +39,7 @@ std::shared_ptr 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(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | // (static_cast(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | // (static_cast(coord[2] & coord_bit_mask) << (coord_bit_size * 2)); diff --git a/include/small_gicp/util/downsampling_omp.hpp b/include/small_gicp/util/downsampling_omp.hpp index 42428e1d..91975525 100644 --- a/include/small_gicp/util/downsampling_omp.hpp +++ b/include/small_gicp/util/downsampling_omp.hpp @@ -39,7 +39,7 @@ std::shared_ptr 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(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | // (static_cast(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | // (static_cast(coord[2] & coord_bit_mask) << (coord_bit_size * 2)); @@ -47,11 +47,7 @@ std::shared_ptr voxelgrid_sampling_omp(const InputPointCloud& } // Sort by voxel coords - quick_sort_omp( - coord_pt.begin(), - coord_pt.end(), - [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }, - num_threads); + quick_sort_omp(coord_pt.begin(), coord_pt.end(), [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }, num_threads); auto downsampled = std::make_shared(); traits::resize(*downsampled, traits::size(points)); diff --git a/src/python/align.cpp b/src/python/align.cpp index 8d304a4c..eecbeb07 100644 --- a/src/python/align.cpp +++ b/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 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>(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 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 ------- diff --git a/src/small_gicp/registration/registration_helper.cpp b/src/small_gicp/registration/registration_helper.cpp index f593da69..73b01bdf 100644 --- a/src/small_gicp/registration/registration_helper.cpp +++ b/src/small_gicp/registration/registration_helper.cpp @@ -91,6 +91,7 @@ align(const PointCloud& target, const PointCloud& source, const KdTree