diff --git a/README.md b/README.md index 2ab0fc8..c97b2e6 100644 --- a/README.md +++ b/README.md @@ -138,7 +138,8 @@ reg.align(*aligned); It is also possible to directly feed `pcl::PointCloud` to `small_gicp::Registration`. Because all preprocessed data are exposed in this way, you can easily re-use them to obtain the best efficiency. ```cpp -#include +#include +#include pcl::PointCloud::Ptr raw_target = ...; pcl::PointCloud::Ptr raw_source = ...; @@ -167,7 +168,7 @@ auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry -### Using `small_gicp::Registration` template (`registration.hpp`) +### Using `Registration` template ([03_registration_template.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/03_registration_template.cpp)) If you want to fine-control and customize the registration process, use `small_gicp::Registration` template that allows modifying the inner algorithms and parameters.
Expand @@ -193,8 +194,8 @@ auto target = std::make_shared(target_points); auto source = std::make_shared(source_points); // Downsampling -target = voxelgrid_downsampling_omp(*target, downsampling_resolution, num_threads); -source = voxelgrid_downsampling_omp(*source, downsampling_resolution, num_threads); +target = voxelgrid_sampling_omp(*target, downsampling_resolution, num_threads); +source = voxelgrid_sampling_omp(*source, downsampling_resolution, num_threads); // Create KdTree auto target_tree = std::make_shared>(target, num_threads); @@ -218,20 +219,7 @@ size_t num_inliers = result.num_inliers; // Number of inlier source points Eigen::Matrix H = result.H; // Final Hessian matrix (6x6) ``` -Custom registration example: - -```cpp -using PerPointFactor = PointToPlaneICPFactor; // Point-to-plane ICP -using GeneralFactor = RestrictDoFFactor; // DoF restriction -using Reduction = ParallelReductionTBB; // TBB-based parallel reduction -using CorrespondenceRejector = DistanceRejector; // Distance-based correspondence rejection -using Optimizer = LevenbergMarquardtOptimizer; // Levenberg marquardt optimizer - -Registration registration; -registration.general_factor.set_translation_mask(Eigen::Vector3d(1.0, 1.0, 0.0)); // XY-translation only -registration.general_factor.set_ratation_mask(Eigen::Vector3d(0.0, 0.0, 1.0)); // Z-rotation only -registration.optimizer.init_lambda = 1e-3; // Initial damping scale -``` +See [03_registration_template.cpp](https://github.com/koide3/small_gicp/blob/master/src/example/03_registration_template.cpp) for more detailed customization example.
diff --git a/include/small_gicp/factors/gicp_factor.hpp b/include/small_gicp/factors/gicp_factor.hpp index f37e0e9..11671bf 100644 --- a/include/small_gicp/factors/gicp_factor.hpp +++ b/include/small_gicp/factors/gicp_factor.hpp @@ -43,7 +43,7 @@ struct GICPFactor { size_t k_index; double k_sq_dist; - if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(target, source, T, k_index, source_index, k_sq_dist)) { return false; } diff --git a/include/small_gicp/factors/icp_factor.hpp b/include/small_gicp/factors/icp_factor.hpp index 860cfaa..6bee779 100644 --- a/include/small_gicp/factors/icp_factor.hpp +++ b/include/small_gicp/factors/icp_factor.hpp @@ -31,7 +31,7 @@ struct ICPFactor { size_t k_index; double k_sq_dist; - if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(target, source, T, k_index, source_index, k_sq_dist)) { return false; } diff --git a/include/small_gicp/factors/plane_icp_factor.hpp b/include/small_gicp/factors/plane_icp_factor.hpp index 2a94d25..595319d 100644 --- a/include/small_gicp/factors/plane_icp_factor.hpp +++ b/include/small_gicp/factors/plane_icp_factor.hpp @@ -31,7 +31,7 @@ struct PointToPlaneICPFactor { size_t k_index; double k_sq_dist; - if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + if (!traits::nearest_neighbor_search(target_tree, transed_source_pt, &k_index, &k_sq_dist) || rejector(target, source, T, k_index, source_index, k_sq_dist)) { return false; } diff --git a/include/small_gicp/registration/rejector.hpp b/include/small_gicp/registration/rejector.hpp index 99b20c7..b1a8de2 100644 --- a/include/small_gicp/registration/rejector.hpp +++ b/include/small_gicp/registration/rejector.hpp @@ -7,15 +7,20 @@ namespace small_gicp { /// @brief Null correspondence rejector. This class accepts all input correspondences. struct NullRejector { - bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return false; } + template + bool operator()(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { + return false; + } }; /// @brief Rejecting correspondences with large distances. struct DistanceRejector { DistanceRejector() : max_dist_sq(1.0) {} - DistanceRejector(double max_dist) : max_dist_sq(max_dist * max_dist) {} - bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return sq_dist > max_dist_sq; } + template + bool operator()(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { + return sq_dist > max_dist_sq; + } double max_dist_sq; }; diff --git a/src/example/03_registration_template.cpp b/src/example/03_registration_template.cpp new file mode 100644 index 0000000..a0996ae --- /dev/null +++ b/src/example/03_registration_template.cpp @@ -0,0 +1,267 @@ +/// @brief Basic point cloud registration example with small_gicp::align() +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace small_gicp; + +/// @brief Basic registration example using small_gicp::Registration. +void example1(const std::vector& target_points, const std::vector& source_points) { + int num_threads = 4; // Number of threads to be used + double downsampling_resolution = 0.25; // Downsampling resolution + int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation + double max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold) + + // Convert to small_gicp::PointCloud + auto target = std::make_shared(target_points); + auto source = std::make_shared(source_points); + + // Downsampling + target = voxelgrid_sampling_omp(*target, downsampling_resolution, num_threads); + source = voxelgrid_sampling_omp(*source, downsampling_resolution, num_threads); + + // Create KdTree + auto target_tree = std::make_shared>(target, num_threads); + auto source_tree = std::make_shared>(source, num_threads); + + // Estimate point covariances + estimate_covariances_omp(*target, *target_tree, num_neighbors, num_threads); + estimate_covariances_omp(*source, *source_tree, num_neighbors, num_threads); + + // GICP + OMP-based parallel reduction + Registration registration; + registration.reduction.num_threads = num_threads; + registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance; + + // Align point clouds + Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity(); + auto result = registration.align(*target, *source, *target_tree, init_T_target_source); + + std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl; + std::cout << "converged:" << result.converged << std::endl; + std::cout << "error:" << result.error << std::endl; + std::cout << "iterations:" << result.iterations << std::endl; + std::cout << "num_inliers:" << result.num_inliers << std::endl; + std::cout << "--- H ---" << std::endl << result.H << std::endl; + std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl; +} + +/** Custom registration example **/ + +/// @brief Point structure with mean, normal, and features. +struct MyPoint { + std::array point; // Point coorindates + std::array normal; // Point normal + std::array features; // Point features +}; + +/// @brief My point cloud class. +using MyPointCloud = std::vector; + +// Define traits for MyPointCloud so that it can be fed to small_gicp algorithms. +namespace small_gicp { +namespace traits { +template <> +struct Traits { + // *** Setters *** + // The following getters are required for feeding this class to registration algorithms. + + // Number of points in the point cloud. + static size_t size(const MyPointCloud& points) { return points.size(); } + // Check if the point cloud has points. + static bool has_points(const MyPointCloud& points) { return !points.empty(); } + // Check if the point cloud has normals. + static bool has_normals(const MyPointCloud& points) { return !points.empty(); } + + // Get i-th point. The last element should be 1.0. + static Eigen::Vector4d point(const MyPointCloud& points, size_t i) { + const auto& p = points[i].point; + return Eigen::Vector4d(p[0], p[1], p[2], 1.0); + } + // Get i-th normal. The last element should be 0.0. + static Eigen::Vector4d normal(const MyPointCloud& points, size_t i) { + const auto& n = points[i].normal; + return Eigen::Vector4d(n[0], n[1], n[2], 0.0); + } + // To use GICP, the following covariance getter is required. + // static bool has_covs(const MyPointCloud& points) { return !points.empty(); } + // static const Eigen::Matrix4d cov(const MyPointCloud& points, size_t i); + + // *** Setters *** + // The following methods are required for feeding this class to preprocessing algorithms. + // (e.g., downsampling and covariance estimation) + + // Resize the point cloud. + static void resize(MyPointCloud& points, size_t n) { points.resize(n); } + // Set i-th point. pt = [x, y, z, 1.0]. + static void set_point(MyPointCloud& points, size_t i, const Eigen::Vector4d& pt) { Eigen::Map(points[i].point.data()) = pt.head<3>(); } + // Set i-th normal. n = [nx, ny, nz, 0.0]. + static void set_normal(MyPointCloud& points, size_t i, const Eigen::Vector4d& n) { Eigen::Map(points[i].normal.data()) = n.head<3>(); } + // To feed this class to estimate_covariances(), the following setter is required. + // static void set_cov(MyPointCloud& points, size_t i, const Eigen::Matrix4d& cov); +}; +} // namespace traits +} // namespace small_gicp + +/// @brief Custom correspondence rejector. +struct MyCorrespondenceRejector { + MyCorrespondenceRejector() : max_correpondence_dist_sq(1.0), min_feature_cos_dist(0.9) {} + + /// @brief Check if the correspondence should be rejected. + /// @param T Current estimate of T_target_source + /// @param target_index Target point index + /// @param source_index Source point index + /// @param sq_dist Squared distance between the points + /// @return True if the correspondence should be rejected + bool operator()(const MyPointCloud& target, const MyPointCloud& source, const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { + // Reject correspondences with large distances + if (sq_dist > max_correpondence_dist_sq) { + return true; + } + + // You can define your own rejection criteria here (e.g., based on features) + Eigen::Map> target_features(target[target_index].features.data()); + Eigen::Map> source_features(target[target_index].features.data()); + if (target_features.dot(source_features) < min_feature_cos_dist) { + return true; + } + + return false; + } + + double max_correpondence_dist_sq; // Maximum correspondence distance + double min_feature_cos_dist; // Maximum feature distance +}; + +/// @brief Custom general factor that can affect the entire registration process. +struct MyGeneralFactor { + MyGeneralFactor() : lambda(1e8) {} + + /// @brief Update linearized system consisting of linearized per-point factors. + /// @note This method is called just before the linearized system is solved. + /// By modifying the linearized system (H, b, e), you can add constraints to the optimization. + /// @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 { + // Optimization DoF mask [rx, ry, rz, tx, ty, tz] (1.0 = inactive, 0.0 = active) + Eigen::Matrix dof_mask; + dof_mask << 1.0, 1.0, 0.0, 0.0, 0.0, 0.0; + + // Fix roll and pitch rotation by adding a large penalty (soft contraint) + (*H) += dof_mask.asDiagonal() * lambda; + } + + /// @brief Update error consisting of per-point factors. + /// @note This method is called just after the linearized system is solved in LM to check if the objective function is decreased. + /// If you modified the error in update_linearized_system(), you should update the error here to be consistent. + /// @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 { + // No update is required for the error. + } + + double lambda; ///< Regularization parameter (Increasing this makes the constraint stronger) +}; + +/// @brief Example to perform preprocessing and registration separately. +void example2(const std::vector& target_points, const std::vector& source_points) { + int num_threads = 4; // Number of threads to be used + double downsampling_resolution = 0.25; // Downsampling resolution + int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation + double max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold) + + // Convert to MyPointCloud + std::shared_ptr target = std::make_shared(); + target->resize(target_points.size()); + for (size_t i = 0; i < target_points.size(); i++) { + Eigen::Map(target->at(i).point.data()) = target_points[i].head<3>().cast(); + } + + std::shared_ptr source = std::make_shared(); + source->resize(source_points.size()); + for (size_t i = 0; i < source_points.size(); i++) { + Eigen::Map(source->at(i).point.data()) = source_points[i].head<3>().cast(); + } + + // Downsampling + target = voxelgrid_sampling_omp(*target, downsampling_resolution, num_threads); + source = voxelgrid_sampling_omp(*source, downsampling_resolution, num_threads); + + // Create KdTree + auto target_tree = std::make_shared>(target, num_threads); + auto source_tree = std::make_shared>(source, num_threads); + + // Estimate point normals + estimate_normals_omp(*target, *target_tree, num_neighbors, num_threads); + estimate_normals_omp(*source, *source_tree, num_neighbors, num_threads); + + // Compute point features (e.g., FPFH, SHOT, etc.) + for (size_t i = 0; i < target->size(); i++) { + target->at(i).features.fill(1.0); + } + for (size_t i = 0; i < source->size(); i++) { + source->at(i).features.fill(1.0); + } + + // Point-to-plane ICP + OMP-based parallel reduction + using PerPointFactor = PointToPlaneICPFactor; // Use point-to-plane ICP factor. Target must have normals. + using Reduction = ParallelReductionOMP; // Use OMP-based parallel reduction + using GeneralFactor = MyGeneralFactor; // Use custom general factor + using CorrespondenceRejector = MyCorrespondenceRejector; // Use custom correspondence rejector + using Optimizer = LevenbergMarquardtOptimizer; // Use Levenberg-Marquardt optimizer + + Registration registration; + registration.reduction.num_threads = num_threads; + registration.rejector.max_correpondence_dist_sq = max_correspondence_distance * max_correspondence_distance; + registration.general_factor.lambda = 1e8; + + // Align point clouds + Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity(); + auto result = registration.align(*target, *source, *target_tree, init_T_target_source); + + std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl; + std::cout << "converged:" << result.converged << std::endl; + std::cout << "error:" << result.error << std::endl; + std::cout << "iterations:" << result.iterations << std::endl; + std::cout << "num_inliers:" << result.num_inliers << std::endl; + std::cout << "--- H ---" << std::endl << result.H << std::endl; + std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl; +} + +int main(int argc, char** argv) { + std::vector target_points = read_ply("data/target.ply"); + std::vector source_points = read_ply("data/source.ply"); + if (target_points.empty() || source_points.empty()) { + std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl; + return 1; + } + + example1(target_points, source_points); + example2(target_points, source_points); + + return 0; +} \ No newline at end of file