From 94d93b9e01ddbab805f00f67ed7ca818b3bff202 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 4 Jun 2024 16:16:25 +0900 Subject: [PATCH] Robust kernel (#54) * pypi * robust kernel * test for robust kernel --- README.md | 8 ++ include/small_gicp/factors/gicp_factor.hpp | 9 +- include/small_gicp/factors/icp_factor.hpp | 4 +- .../small_gicp/factors/plane_icp_factor.hpp | 4 +- include/small_gicp/factors/robust_kernel.hpp | 106 ++++++++++++++++++ .../small_gicp/registration/registration.hpp | 7 +- src/test/registration_test.cpp | 27 ++++- 7 files changed, 157 insertions(+), 8 deletions(-) create mode 100644 include/small_gicp/factors/robust_kernel.hpp diff --git a/README.md b/README.md index 46cf785..d825301 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,14 @@ sudo make install ### Python (Linux / Windows) +#### Install from PyPI + +```bash +pip install small_gicp --user +``` + +#### Install from source + ```bash cd small_gicp pip install . --user diff --git a/include/small_gicp/factors/gicp_factor.hpp b/include/small_gicp/factors/gicp_factor.hpp index b65debd..4b00daf 100644 --- a/include/small_gicp/factors/gicp_factor.hpp +++ b/include/small_gicp/factors/gicp_factor.hpp @@ -12,8 +12,13 @@ namespace small_gicp { /// @brief GICP (distribution-to-distribution) per-point error factor. struct GICPFactor { + struct Setting {}; + /// @brief Constructor - GICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()), mahalanobis(Eigen::Matrix4d::Zero()) {} + GICPFactor(const Setting& setting = Setting()) + : target_index(std::numeric_limits::max()), + source_index(std::numeric_limits::max()), + mahalanobis(Eigen::Matrix4d::Zero()) {} /// @brief Linearize the factor /// @param target Target point cloud @@ -25,7 +30,7 @@ struct GICPFactor { /// @param H Linearized information matrix /// @param b Linearized information vector /// @param e Error at the linearization point - /// @return + /// @return True if the point is inlier template bool linearize( const TargetPointCloud& target, diff --git a/include/small_gicp/factors/icp_factor.hpp b/include/small_gicp/factors/icp_factor.hpp index 7d654c7..a2a12b4 100644 --- a/include/small_gicp/factors/icp_factor.hpp +++ b/include/small_gicp/factors/icp_factor.hpp @@ -12,7 +12,9 @@ namespace small_gicp { /// @brief Point-to-point per-point error factor. struct ICPFactor { - ICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} + struct Setting {}; + + ICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} template bool linearize( diff --git a/include/small_gicp/factors/plane_icp_factor.hpp b/include/small_gicp/factors/plane_icp_factor.hpp index 150dfcf..03a8ca9 100644 --- a/include/small_gicp/factors/plane_icp_factor.hpp +++ b/include/small_gicp/factors/plane_icp_factor.hpp @@ -12,7 +12,9 @@ namespace small_gicp { /// @brief Point-to-plane per-point error factor. struct PointToPlaneICPFactor { - PointToPlaneICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} + struct Setting {}; + + PointToPlaneICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} template bool linearize( diff --git a/include/small_gicp/factors/robust_kernel.hpp b/include/small_gicp/factors/robust_kernel.hpp new file mode 100644 index 0000000..0b27d54 --- /dev/null +++ b/include/small_gicp/factors/robust_kernel.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +/// @brief Huber robust kernel +struct Huber { +public: + /// @brief Huber robust kernel setting + struct Setting { + double c = 1.0; ///< Kernel width + }; + + /// @brief Constructor + Huber(const Setting& setting) : c(setting.c) {} + + /// @brief Compute the weight for an error + /// @param e Error + /// @return Weight + double weight(double e) const { + const double e_abs = std::abs(e); + return e_abs < c ? 1.0 : c / e_abs; + } + +public: + const double c; ///< Kernel width +}; + +/// @brief Cauchy robust kernel +struct Cauchy { +public: + /// @brief Huber robust kernel setting + struct Setting { + double c = 1.0; ///< Kernel width + }; + + /// @brief Constructor + Cauchy(const Setting& setting) : c(setting.c) {} + + /// @brief Compute the weight for an error + /// @param e Error + /// @return Weight + double weight(double e) const { return c / (c + e * e); } + +public: + const double c; ///< Kernel width +}; + +/// @brief Robustify a factor with a robust kernel +/// @tparam Kernel Robust kernel +/// @tparam Factor Factor +template +struct RobustFactor { +public: + /// @brief Robust factor setting + struct Setting { + typename Kernel::Setting robust_kernel; ///< Robust kernel setting + typename Factor::Setting factor; ///< Factor setting + }; + + /// @brief Constructor + RobustFactor(const Setting& setting = Setting()) : robust_kernel(setting.robust_kernel), factor(setting.factor) {} + + /// @brief Linearize the factor + template + bool linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + size_t source_index, + const CorrespondenceRejector& rejector, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) { + if (!factor.linearize(target, source, target_tree, T, source_index, rejector, H, b, e)) { + return false; + } + + // Robustify the linearized factor + const double w = robust_kernel.weight(std::sqrt(*e)); + *H *= w; + *b *= w; + *e *= w; + + return true; + } + + /// @brief Evaluate error + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { + const double e = factor.error(target, source, T); + return robust_kernel.weight(std::sqrt(e)) * e; + } + + /// @brief Check if the factor is inlier + bool inlier() const { return factor.inlier(); } + +public: + Kernel robust_kernel; ///< Robust kernel + Factor factor; ///< Factor +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/registration.hpp b/include/small_gicp/registration/registration.hpp index fab7b7c..f03a53c 100644 --- a/include/small_gicp/registration/registration.hpp +++ b/include/small_gicp/registration/registration.hpp @@ -15,7 +15,7 @@ namespace small_gicp { /// @brief Point cloud registration. template < - typename Factor, + typename PointFactor, typename Reduction, typename GeneralFactor = NullFactor, typename CorrespondenceRejector = DistanceRejector, @@ -38,13 +38,16 @@ struct Registration { std::cerr << "warning: source point cloud is too small. |source|=" << traits::size(source) << std::endl; } - std::vector factors(traits::size(source)); + std::vector factors(traits::size(source), PointFactor(point_factor)); return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor); } public: + using PointFactorSetting = typename PointFactor::Setting; + TerminationCriteria criteria; ///< Termination criteria CorrespondenceRejector rejector; ///< Correspondence rejector + PointFactorSetting point_factor; ///< Factor setting GeneralFactor general_factor; ///< General factor Reduction reduction; ///< Reduction Optimizer optimizer; ///< Optimizer diff --git a/src/test/registration_test.cpp b/src/test/registration_test.cpp index 3f62c5c..2c0d691 100644 --- a/src/test/registration_test.cpp +++ b/src/test/registration_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -283,7 +284,7 @@ TEST_F(RegistrationTest, PCLInterfaceTest) { INSTANTIATE_TEST_SUITE_P( RegistrationTest, RegistrationTest, - testing::Combine(testing::Values("ICP", "PLANE_ICP", "GICP", "VGICP"), testing::Values("SERIAL", "TBB", "OMP")), + testing::Combine(testing::Values("ICP", "PLANE_ICP", "GICP", "VGICP", "HUBER_GICP", "CAUCHY_GICP"), testing::Values("SERIAL", "TBB", "OMP")), [](const auto& info) { std::stringstream sst; sst << std::get<0>(info.param) << "_" << std::get<1>(info.param); @@ -338,7 +339,29 @@ TEST_P(RegistrationTest, RegistrationTest) { Registration reg; test_registration_vgicp(reg); } + } else if (factor == "HUBER_GICP") { + if (reduction == "SERIAL") { + Registration, SerialReduction> reg; + test_registration(reg); + } else if (reduction == "TBB") { + Registration, ParallelReductionTBB> reg; + test_registration(reg); + } else if (reduction == "OMP") { + Registration, ParallelReductionOMP> reg; + test_registration(reg); + } + } else if (factor == "CAUCHY_GICP") { + if (reduction == "SERIAL") { + Registration, SerialReduction> reg; + test_registration(reg); + } else if (reduction == "TBB") { + Registration, ParallelReductionTBB> reg; + test_registration(reg); + } else if (reduction == "OMP") { + Registration, ParallelReductionOMP> reg; + test_registration(reg); + } } else { - std::cerr << "error: unknown factor type " << factor << std::endl; + EXPECT_TRUE(false) << "error: unknown factor type " << factor; } } \ No newline at end of file