From f5dcd118bedb54bb2b9cd723b5631be8df450338 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Mon, 8 Apr 2024 14:28:04 +0900 Subject: [PATCH] refactor RegistrationPCL --- include/small_gicp/pcl/pcl_registration.hpp | 56 +++++++++++----- .../small_gicp/pcl/pcl_registration_impl.hpp | 65 +++++++++++++++++-- .../registration/registration_result.hpp | 2 +- src/test/registration_test.cpp | 24 ++++++- 4 files changed, 120 insertions(+), 27 deletions(-) diff --git a/include/small_gicp/pcl/pcl_registration.hpp b/include/small_gicp/pcl/pcl_registration.hpp index e7366c6..bb597d9 100644 --- a/include/small_gicp/pcl/pcl_registration.hpp +++ b/include/small_gicp/pcl/pcl_registration.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace small_gicp { @@ -43,42 +44,61 @@ class RegistrationPCL : public pcl::Registration& getFinalHessian() const { return final_hessian_; } + /// @brief Get the final Hessian matrix ([rx, ry, rz, tx, ty, tz]). + const Eigen::Matrix& getFinalHessian() const; + /// @brief Get the detailed registration result. + const RegistrationResult& getRegistrationResult() const; + + /// @brief Set the input source (aligned) point cloud. void setInputSource(const PointCloudSourceConstPtr& cloud) override; + /// @brief Set the input target (fixed) point cloud. void setInputTarget(const PointCloudTargetConstPtr& cloud) override; + /// @brief Swap source and target point clouds and their augmented data (KdTrees, covariances, and voxelmaps). void swapSourceAndTarget(); + /// @brief Clear source point cloud. void clearSource(); + /// @brief Clear target point cloud. void clearTarget(); protected: virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; protected: - int num_threads_; - int k_correspondences_; - double rotation_epsilon_; - double voxel_resolution_; - bool verbose_; - std::string registration_type_; + int num_threads_; ///< Number of threads to use. + int k_correspondences_; ///< Number of neighbors for covariance estimation. + double rotation_epsilon_; ///< Rotation epsilon for convergence check. + double voxel_resolution_; ///< Voxel resolution for VGICP. + std::string registration_type_; ///< Registration type ("GICP" or "VGICP"). + bool verbose_; ///< Verbosity flag. - std::shared_ptr>> target_tree_; - std::shared_ptr>> source_tree_; + std::shared_ptr>> target_tree_; ///< KdTree for target point cloud. + std::shared_ptr>> source_tree_; ///< KdTree for source point cloud. - std::shared_ptr target_voxelmap_; - std::shared_ptr source_voxelmap_; + std::shared_ptr target_voxelmap_; ///< VoxelMap for target point cloud. + std::shared_ptr source_voxelmap_; ///< VoxelMap for source point cloud. - std::vector target_covs_; - std::vector source_covs_; + std::vector target_covs_; ///< Covariances of target points + std::vector source_covs_; ///< Covariances of source points. - Eigen::Matrix final_hessian_; + RegistrationResult result_; ///< Registration result. }; } // namespace small_gicp diff --git a/include/small_gicp/pcl/pcl_registration_impl.hpp b/include/small_gicp/pcl/pcl_registration_impl.hpp index 3d7d4e3..1af5515 100644 --- a/include/small_gicp/pcl/pcl_registration_impl.hpp +++ b/include/small_gicp/pcl/pcl_registration_impl.hpp @@ -33,8 +33,6 @@ RegistrationPCL::RegistrationPCL() { voxel_resolution_ = 1.0; verbose_ = false; registration_type_ = "GICP"; - - final_hessian_.setIdentity(); } template @@ -88,6 +86,45 @@ void RegistrationPCL::clearTarget() { target_voxelmap_.reset(); } +template +void RegistrationPCL::setNumThreads(int n) { + if (n <= 0) { + PCL_ERROR("[RegistrationPCL::setNumThreads] Invalid number of threads: %d\n", n); + n = 1; + } + + num_threads_ = n; +} + +template +void RegistrationPCL::setCorrespondenceRandomness(int k) { + setNumNeighborsForCovariance(k); +} + +template +void RegistrationPCL::setNumNeighborsForCovariance(int k) { + if (k < 5) { + PCL_ERROR("[RegistrationPCL::setNumNeighborsForCovariance] Invalid number of neighbors: %d\n", k); + k = 5; + } + k_correspondences_ = k; +} + +template +void RegistrationPCL::setVoxelResolution(double r) { + if (voxel_resolution_ <= 0) { + PCL_ERROR("[RegistrationPCL::setVoxelResolution] Invalid voxel resolution: %f\n", r); + r = 1.0; + } + + voxel_resolution_ = r; +} + +template +void RegistrationPCL::setRotationEpsilon(double eps) { + rotation_epsilon_ = eps; +} + template void RegistrationPCL::setRegistrationType(const std::string& type) { if (type == "GICP") { @@ -99,6 +136,21 @@ void RegistrationPCL::setRegistrationType(const std::s } } +template +void RegistrationPCL::setVerbosity(bool verbose) { + verbose_ = verbose; +} + +template +const Eigen::Matrix& RegistrationPCL::getFinalHessian() const { + return result_.H; +} + +template +const RegistrationResult& RegistrationPCL::getRegistrationResult() const { + return result_; +} + template void RegistrationPCL::computeTransformation(PointCloudSource& output, const Matrix4& guess) { if (output.points.data() == input_->points.data() || output.points.data() == target_->points.data()) { @@ -123,9 +175,8 @@ void RegistrationPCL::computeTransformation(PointCloud registration.optimizer.verbose = verbose_; registration.optimizer.max_iterations = max_iterations_; - RegistrationResult result(Eigen::Isometry3d::Identity()); if (registration_type_ == "GICP") { - result = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast())); + result_ = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast())); } else if (registration_type_ == "VGICP") { if (!target_voxelmap_) { target_voxelmap_ = std::make_shared(voxel_resolution_); @@ -136,14 +187,14 @@ void RegistrationPCL::computeTransformation(PointCloud source_voxelmap_->insert(source_proxy); } - result = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast())); + result_ = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast())); } else { PCL_ERROR("[RegistrationPCL::computeTransformation] Invalid registration type: %s\n", registration_type_.c_str()); return; } - final_transformation_ = result.T_target_source.matrix().template cast(); - final_hessian_ = result.H; + converged_ = result_.converged; + final_transformation_ = result_.T_target_source.matrix().template cast(); pcl::transformPointCloud(*input_, output, final_transformation_); } diff --git a/include/small_gicp/registration/registration_result.hpp b/include/small_gicp/registration/registration_result.hpp index ddd63e0..96ae620 100644 --- a/include/small_gicp/registration/registration_result.hpp +++ b/include/small_gicp/registration/registration_result.hpp @@ -9,7 +9,7 @@ namespace small_gicp { /// @brief Registration result struct RegistrationResult { - RegistrationResult(const Eigen::Isometry3d& T) + RegistrationResult(const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) : T_target_source(T), converged(false), iterations(0), diff --git a/src/test/registration_test.cpp b/src/test/registration_test.cpp index 6aca3bf..e22a809 100644 --- a/src/test/registration_test.cpp +++ b/src/test/registration_test.cpp @@ -178,8 +178,12 @@ TEST_F(RegistrationTest, LoadCheck) { // PCL interface test TEST_F(RegistrationTest, PCLInterfaceTest) { RegistrationPCL registration; - registration.setRegistrationType("GICP"); + registration.setNumThreads(2); + registration.setCorrespondenceRandomness(20); + registration.setRotationEpsilon(1e-4); + registration.setTransformationEpsilon(1e-4); registration.setMaxCorrespondenceDistance(1.0); + registration.setRegistrationType("GICP"); // Forward align registration.setInputTarget(target_pcl); @@ -209,6 +213,15 @@ TEST_F(RegistrationTest, PCLInterfaceTest) { EXPECT_EQ(aligned.size(), source_pcl->size()); EXPECT_TRUE(compare_transformation(T_target_source, Eigen::Isometry3d(registration.getFinalTransformation().cast()))); + Eigen::Matrix H = registration.getFinalHessian(); + Eigen::SelfAdjointEigenSolver> eig(H); + EXPECT_GT(eig.eigenvalues()[0], 10.0); + for (int i = 0; i < 6; i++) { + for (int j = i + 1; j < 6; j++) { + EXPECT_NEAR(H(i, j), H(j, i), 1e-3); + } + } + registration.setRegistrationType("VGICP"); registration.setVoxelResolution(1.0); @@ -221,6 +234,15 @@ TEST_F(RegistrationTest, PCLInterfaceTest) { EXPECT_EQ(aligned.size(), source_pcl->size()); EXPECT_TRUE(compare_transformation(T_target_source, Eigen::Isometry3d(registration.getFinalTransformation().cast()))); + H = registration.getFinalHessian(); + eig.compute(H); + EXPECT_GT(eig.eigenvalues()[0], 10.0); + for (int i = 0; i < 6; i++) { + for (int j = i + 1; j < 6; j++) { + EXPECT_NEAR(H(i, j), H(j, i), 1e-3); + } + } + // Swap and backward align registration.swapSourceAndTarget(); registration.align(aligned);