From 31ad85c2d1a9076c04abf560906ab65ed4f11052 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Fri, 29 Mar 2024 11:36:17 +0900 Subject: [PATCH] general factor --- include/small_gicp/factors/general_factor.hpp | 75 +++++++++++++++++++ include/small_gicp/factors/gicp_factor.hpp | 2 +- include/small_gicp/factors/icp_factor.hpp | 2 +- .../small_gicp/factors/plane_icp_factor.hpp | 2 +- .../small_gicp/{points => pcl}/pcl_point.hpp | 0 .../{points => pcl}/pcl_point_traits.hpp | 0 include/small_gicp/points/point_cloud.hpp | 24 +++--- include/small_gicp/points/traits.hpp | 20 ++--- include/small_gicp/registration/optimizer.hpp | 21 ++++-- .../small_gicp/registration/reduction_omp.hpp | 1 + .../small_gicp/registration/registration.hpp | 15 +++- src/downsampling_benchmark.cpp | 4 +- src/odometry_benchmark_small_gicp.cpp | 2 +- src/odometry_benchmark_small_gicp_omp.cpp | 2 +- src/odometry_benchmark_small_gicp_tbb.cpp | 2 +- ...ometry_benchmark_small_vgicp_model_tbb.cpp | 2 +- src/odometry_benchmark_small_vgicp_omp.cpp | 2 +- src/odometry_benchmark_small_vgicp_tbb.cpp | 2 +- src/test/downsampling_test.cpp | 4 +- src/test/kdtree_test.cpp | 2 +- src/test/points_test.cpp | 4 +- src/test/registration_test.cpp | 4 +- 22 files changed, 141 insertions(+), 51 deletions(-) create mode 100644 include/small_gicp/factors/general_factor.hpp rename include/small_gicp/{points => pcl}/pcl_point.hpp (100%) rename include/small_gicp/{points => pcl}/pcl_point_traits.hpp (100%) diff --git a/include/small_gicp/factors/general_factor.hpp b/include/small_gicp/factors/general_factor.hpp new file mode 100644 index 0000000..6c4d8b0 --- /dev/null +++ b/include/small_gicp/factors/general_factor.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +/// @brief Null factor that gives no constraints. +struct NullFactor { + NullFactor() = default; + + /// @brief Update linearized system consisting of linearized per-point factors. + /// @param target Target point cloud + /// @param source Source point cloud + /// @param target_tree Nearest neighbor search for the target point cloud + /// @param T Linearization point + /// @param H [in/out] Linearized precision matrix. + /// @param b [in/out] Linearized information vector. + /// @param e [in/out] Error at the linearization point. + template + void update_linearized_system( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) const {} + + /// @brief Update error consisting of per-point factors. + /// @param target Target point cloud + /// @param source Source point cloud + /// @param T Evaluation point + /// @param e [in/out] Error at the evaluation point. + template + void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {} +}; + +/// @brief Factor to restrict the degrees of freedom of optimization (e.g., fixing roll, pitch rotation). +/// @note This factor only enables *soft* constraints. The source point cloud can move to the restricted directions slightly). +struct RestrictDoFFactor { + /// @brief Constructor. + RestrictDoFFactor() { + lambda = 1e9; + mask.setOnes(); + } + + /// @brief Set rotation mask. (1.0 = active, 0.0 = inactive) + void set_rotation_mask(const Eigen::Array3d& rot_mask) { mask.head<3>() = rot_mask; } + + /// @brief Set translation mask. (1.0 = active, 0.0 = inactive) + void set_translation_mask(const Eigen::Array3d& trans_mask) { mask.tail<3>() = trans_mask; } + + /// @brief Update linearized system consisting of linearized per-point factors. + template + void update_linearized_system( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) const { + *H += lambda * (mask - 1.0).abs().matrix().asDiagonal(); + } + + /// @brief Update error consisting of per-point factors. + template + void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {} + + double lambda; ///< Regularization parameter (Increasing this makes the constraint stronger) + Eigen::Array mask; ///< Mask for restricting DoF (rx, ry, rz, tx, ty, tz) +}; + +} // namespace small_gicp diff --git a/include/small_gicp/factors/gicp_factor.hpp b/include/small_gicp/factors/gicp_factor.hpp index ab999a5..f37e0e9 100644 --- a/include/small_gicp/factors/gicp_factor.hpp +++ b/include/small_gicp/factors/gicp_factor.hpp @@ -8,7 +8,7 @@ namespace small_gicp { -/// @brief GICP (distribution-to-distribution) error factor +/// @brief GICP (distribution-to-distribution) per-point error factor. struct GICPFactor { /// @brief Constructor GICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()), mahalanobis(Eigen::Matrix4d::Zero()) {} diff --git a/include/small_gicp/factors/icp_factor.hpp b/include/small_gicp/factors/icp_factor.hpp index 7118abf..860cfaa 100644 --- a/include/small_gicp/factors/icp_factor.hpp +++ b/include/small_gicp/factors/icp_factor.hpp @@ -8,7 +8,7 @@ namespace small_gicp { -/// @brief Point-to-point error factor +/// @brief Point-to-point per-point error factor. struct ICPFactor { ICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} diff --git a/include/small_gicp/factors/plane_icp_factor.hpp b/include/small_gicp/factors/plane_icp_factor.hpp index 2d1821a..2a94d25 100644 --- a/include/small_gicp/factors/plane_icp_factor.hpp +++ b/include/small_gicp/factors/plane_icp_factor.hpp @@ -8,7 +8,7 @@ namespace small_gicp { -/// @brief Point-to-plane error factor +/// @brief Point-to-plane per-point error factor. struct PointToPlaneICPFactor { PointToPlaneICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} diff --git a/include/small_gicp/points/pcl_point.hpp b/include/small_gicp/pcl/pcl_point.hpp similarity index 100% rename from include/small_gicp/points/pcl_point.hpp rename to include/small_gicp/pcl/pcl_point.hpp diff --git a/include/small_gicp/points/pcl_point_traits.hpp b/include/small_gicp/pcl/pcl_point_traits.hpp similarity index 100% rename from include/small_gicp/points/pcl_point_traits.hpp rename to include/small_gicp/pcl/pcl_point_traits.hpp diff --git a/include/small_gicp/points/point_cloud.hpp b/include/small_gicp/points/point_cloud.hpp index e72f049..7095eeb 100644 --- a/include/small_gicp/points/point_cloud.hpp +++ b/include/small_gicp/points/point_cloud.hpp @@ -30,13 +30,13 @@ public: /// @brief Destructor ~PointCloud() {} - /// @brief Number of points + /// @brief Number of points. size_t size() const { return points.size(); } - /// @brief Check if the point cloud is empty + /// @brief Check if the point cloud is empty. bool empty() const { return points.empty(); } - /// @brief Resize point/normal/cov buffers + /// @brief Resize point/normal/cov buffers. /// @param n Number of points void resize(size_t n) { points.resize(n); @@ -44,28 +44,28 @@ public: covs.resize(n); } - /// @brief Get i-th point + /// @brief Get i-th point. Eigen::Vector4d& point(size_t i) { return points[i]; } - /// @brief Get i-th normal + /// @brief Get i-th normal. Eigen::Vector4d& normal(size_t i) { return normals[i]; } - /// @brief Get i-th covariance + /// @brief Get i-th covariance. Eigen::Matrix4d& cov(size_t i) { return covs[i]; } - /// @brief Get i-th point (const) + /// @brief Get i-th point (const). const Eigen::Vector4d& point(size_t i) const { return points[i]; } - /// @brief Get i-th normal (const) + /// @brief Get i-th normal (const). const Eigen::Vector4d& normal(size_t i) const { return normals[i]; } - /// @brief Get i-th covariance (const) + /// @brief Get i-th covariance (const). const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; } public: - std::vector points; ///< Point coordinates - std::vector normals; ///< Point normals - std::vector covs; ///< Point covariances + std::vector points; ///< Point coordinates (x, y, z, 1) + std::vector normals; ///< Point normals (nx, ny, nz, 0) + std::vector covs; ///< Point covariances (3x3 matrix) + zero padding }; namespace traits { diff --git a/include/small_gicp/points/traits.hpp b/include/small_gicp/points/traits.hpp index 1dedad8..bf5caca 100644 --- a/include/small_gicp/points/traits.hpp +++ b/include/small_gicp/points/traits.hpp @@ -9,43 +9,43 @@ namespace traits { template struct Traits; -/// @brief Get the number of points +/// @brief Get the number of points. template size_t size(const T& points) { return Traits::size(points); } -/// @brief Check if the point cloud has points +/// @brief Check if the point cloud has points. template bool has_points(const T& points) { return Traits::has_points(points); } -/// @brief Check if the point cloud has normals +/// @brief Check if the point cloud has normals. template bool has_normals(const T& points) { return Traits::has_normals(points); } -/// @brief Check if the point cloud has covariances +/// @brief Check if the point cloud has covariances. template bool has_covs(const T& points) { return Traits::has_covs(points); } -/// @brief Get i-th point +/// @brief Get i-th point. 4D vector is used to take advantage of SIMD intrinsics. The last element must be filled by one (x, y, z, 1). template auto point(const T& points, size_t i) { return Traits::point(points, i); } -/// @brief Get i-th normal +/// @brief Get i-th normal. 4D vector is used to take advantage of SIMD intrinsics. The last element must be filled by zero (nx, ny, nz, 0). template auto normal(const T& points, size_t i) { return Traits::normal(points, i); } -/// @brief Get i-th covariance +/// @brief Get i-th covariance. Only the top-left 3x3 matrix is filled, and the bottom row and the right col must be filled by zero. template auto cov(const T& points, size_t i) { return Traits::cov(points, i); @@ -57,19 +57,19 @@ void resize(T& points, size_t n) { Traits::resize(points, n); } -/// @brief Set i-th point +/// @brief Set i-th point. (x, y, z, 1) template void set_point(T& points, size_t i, const Eigen::Vector4d& pt) { Traits::set_point(points, i, pt); } -/// @brief Set i-th normal +/// @brief Set i-th normal. (nx, nz, nz, 0) template void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) { Traits::set_normal(points, i, pt); } -/// @brief Set i-th covariance +/// @brief Set i-th covariance. Only the top-left 3x3 matrix should be filled. template void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) { Traits::set_cov(points, i, cov); diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp index 6899d88..88bc0f8 100644 --- a/include/small_gicp/registration/optimizer.hpp +++ b/include/small_gicp/registration/optimizer.hpp @@ -17,7 +17,8 @@ struct GaussNewtonOptimizer { typename CorrespondenceRejector, typename TerminationCriteria, typename Reduction, - typename Factor> + typename Factor, + typename GeneralFactor> RegistrationResult optimize( const TargetPointCloud& target, const SourcePointCloud& source, @@ -26,7 +27,8 @@ struct GaussNewtonOptimizer { const TerminationCriteria& criteria, Reduction& reduction, const Eigen::Isometry3d& init_T, - std::vector& factors) const { + std::vector& factors, + GeneralFactor& general_factor) const { // if (verbose) { std::cout << "--- GN optimization ---" << std::endl; @@ -34,7 +36,8 @@ struct GaussNewtonOptimizer { RegistrationResult result(init_T); for (int i = 0; i < max_iterations && !result.converged; i++) { - const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e); const Eigen::Matrix delta = (H + lambda * Eigen ::Matrix::Identity()).ldlt().solve(-b); if (verbose) { @@ -70,7 +73,8 @@ struct LevenbergMarquardtOptimizer { typename CorrespondenceRejector, typename TerminationCriteria, typename Reduction, - typename Factor> + typename Factor, + typename GeneralFactor> RegistrationResult optimize( const TargetPointCloud& target, const SourcePointCloud& source, @@ -79,7 +83,8 @@ struct LevenbergMarquardtOptimizer { const TerminationCriteria& criteria, Reduction& reduction, const Eigen::Isometry3d& init_T, - std::vector& factors) const { + std::vector& factors, + GeneralFactor& general_factor) const { // if (verbose) { std::cout << "--- LM optimization ---" << std::endl; @@ -88,13 +93,15 @@ struct LevenbergMarquardtOptimizer { double lambda = init_lambda; RegistrationResult result(init_T); for (int i = 0; i < max_iterations && !result.converged; i++) { - const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e); bool success = false; for (int j = 0; j < max_inner_iterations; j++) { const Eigen::Matrix delta = (H + lambda * Eigen ::Matrix::Identity()).ldlt().solve(-b); const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta); - const double new_e = reduction.error(target, source, new_T, factors); + double new_e = reduction.error(target, source, new_T, factors); + general_factor.update_error(target, source, new_T, &e); if (verbose) { std::cout << "iter=" << i << " inner=" << j << " e=" << e << " new_e=" << new_e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm() diff --git a/include/small_gicp/registration/reduction_omp.hpp b/include/small_gicp/registration/reduction_omp.hpp index 5238271..5cded99 100644 --- a/include/small_gicp/registration/reduction_omp.hpp +++ b/include/small_gicp/registration/reduction_omp.hpp @@ -5,6 +5,7 @@ namespace small_gicp { #ifndef _OPENMP +#warning "OpenMP is not available. Parallel reduction will be disabled." inline int omp_get_thread_num() { return 0; } diff --git a/include/small_gicp/registration/registration.hpp b/include/small_gicp/registration/registration.hpp index 2720e47..525f0da 100644 --- a/include/small_gicp/registration/registration.hpp +++ b/include/small_gicp/registration/registration.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -10,11 +11,16 @@ namespace small_gicp { -/// @brief Point cloud registration -template +/// @brief Point cloud registration. +template < + typename Factor, + typename Reduction, + typename GeneralFactor = NullFactor, + typename CorrespondenceRejector = DistanceRejector, + typename Optimizer = LevenbergMarquardtOptimizer> struct Registration { public: - /// @brief Align point clouds + /// @brief Align point clouds. /// @param target Target point cloud /// @param source Source point cloud /// @param target_tree Nearest neighbor search for the target point cloud @@ -24,12 +30,13 @@ public: RegistrationResult align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) const { std::vector factors(traits::size(source)); - return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors); + return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor); } public: TerminationCriteria criteria; ///< Termination criteria CorrespondenceRejector rejector; ///< Correspondence rejector + GeneralFactor general_factor; ///< General factor Reduction reduction; ///< Reduction Optimizer optimizer; ///< Optimizer }; diff --git a/src/downsampling_benchmark.cpp b/src/downsampling_benchmark.cpp index 22dec6a..212ada6 100644 --- a/src/downsampling_benchmark.cpp +++ b/src/downsampling_benchmark.cpp @@ -11,8 +11,8 @@ #include #endif #include -#include -#include +#include +#include #include namespace small_gicp { diff --git a/src/odometry_benchmark_small_gicp.cpp b/src/odometry_benchmark_small_gicp.cpp index b8d8ff3..e76c39f 100644 --- a/src/odometry_benchmark_small_gicp.cpp +++ b/src/odometry_benchmark_small_gicp.cpp @@ -26,7 +26,7 @@ public: return T; } - Registration registration; + Registration registration; registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); diff --git a/src/odometry_benchmark_small_gicp_omp.cpp b/src/odometry_benchmark_small_gicp_omp.cpp index 565e0e4..11309e0 100644 --- a/src/odometry_benchmark_small_gicp_omp.cpp +++ b/src/odometry_benchmark_small_gicp_omp.cpp @@ -26,7 +26,7 @@ public: return T; } - Registration registration; + Registration registration; registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; registration.reduction.num_threads = params.num_threads; diff --git a/src/odometry_benchmark_small_gicp_tbb.cpp b/src/odometry_benchmark_small_gicp_tbb.cpp index b446e0c..d204856 100644 --- a/src/odometry_benchmark_small_gicp_tbb.cpp +++ b/src/odometry_benchmark_small_gicp_tbb.cpp @@ -32,7 +32,7 @@ public: return T; } - Registration registration; + Registration registration; registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); diff --git a/src/odometry_benchmark_small_vgicp_model_tbb.cpp b/src/odometry_benchmark_small_vgicp_model_tbb.cpp index ab047fe..f7b6692 100644 --- a/src/odometry_benchmark_small_vgicp_model_tbb.cpp +++ b/src/odometry_benchmark_small_vgicp_model_tbb.cpp @@ -31,7 +31,7 @@ public: return T; } - Registration registration; + Registration registration; auto result = registration.align(*voxelmap, *points, *voxelmap, T); T = result.T_target_source; diff --git a/src/odometry_benchmark_small_vgicp_omp.cpp b/src/odometry_benchmark_small_vgicp_omp.cpp index cedcd3c..c647d5f 100644 --- a/src/odometry_benchmark_small_vgicp_omp.cpp +++ b/src/odometry_benchmark_small_vgicp_omp.cpp @@ -30,7 +30,7 @@ public: return T; } - Registration registration; + Registration registration; registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity()); diff --git a/src/odometry_benchmark_small_vgicp_tbb.cpp b/src/odometry_benchmark_small_vgicp_tbb.cpp index 23506a9..e064c18 100644 --- a/src/odometry_benchmark_small_vgicp_tbb.cpp +++ b/src/odometry_benchmark_small_vgicp_tbb.cpp @@ -34,7 +34,7 @@ public: return T; } - Registration registration; + Registration registration; registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity()); diff --git a/src/test/downsampling_test.cpp b/src/test/downsampling_test.cpp index 9457115..8743827 100644 --- a/src/test/downsampling_test.cpp +++ b/src/test/downsampling_test.cpp @@ -4,8 +4,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/test/kdtree_test.cpp b/src/test/kdtree_test.cpp index 8aa81cc..465e950 100644 --- a/src/test/kdtree_test.cpp +++ b/src/test/kdtree_test.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include using namespace small_gicp; diff --git a/src/test/points_test.cpp b/src/test/points_test.cpp index f001df7..d3d712b 100644 --- a/src/test/points_test.cpp +++ b/src/test/points_test.cpp @@ -3,8 +3,8 @@ #include #include #include -#include -#include +#include +#include using namespace small_gicp; diff --git a/src/test/registration_test.cpp b/src/test/registration_test.cpp index 960f671..1c04a96 100644 --- a/src/test/registration_test.cpp +++ b/src/test/registration_test.cpp @@ -3,8 +3,8 @@ #include #include #include -#include -#include +#include +#include #include #include