From 209cdefde065594fd8d2bf9158f1c27f89861a87 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Wed, 12 Jun 2024 09:58:51 +0900 Subject: [PATCH 1/2] update README --- README.md | 29 +++++++++++++++++++++++-- src/example/basic_registration.py | 35 ++++++++++++++++++++++++++++--- 2 files changed, 59 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 8e03fdf..d35f424 100644 --- a/README.md +++ b/README.md @@ -46,14 +46,14 @@ sudo make install #### Install from [PyPI](https://pypi.org/project/small-gicp/) ```bash -pip install small_gicp --user +pip install small_gicp ``` #### Install from source ```bash cd small_gicp -pip install . --user +pip install . # [Optional (linux)] Install stubs for autocomplete (If you know a better way, let me know...) pip install pybind11-stubgen @@ -404,6 +404,31 @@ open3d.visualization.draw_geometries([target_o3d, source_o3d]) - [Scan-to-scan and scan-to-model GICP matching odometry on KITTI](src/example/kitti_odometry.py) +## Running examples + +### C++ + +```bash +cd small_gicp +mkdir build && cd build +cmake .. -DBUILD_EXAMPLES=ON && make -j + +cd .. +./build/01_basic_registration +./build/02_basic_registration_pcl +./build/03_registration_template +``` + + +### Python + +```bash +cd small_gicp +pip install . + +python3 src/example/basic_registration.py +``` + ## [Benchmark](BENCHMARK.md) Processing speed comparison between small_gicp and Open3D ([youtube]((https://youtu.be/LNESzGXPr4c?feature=shared))). diff --git a/src/example/basic_registration.py b/src/example/basic_registration.py index 27de289..02e1d16 100755 --- a/src/example/basic_registration.py +++ b/src/example/basic_registration.py @@ -9,6 +9,8 @@ # Basic registation example with numpy arrays def example_numpy1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray): + print('*** example_numpy1 ***') + # Example A : Perform registration with numpy arrays # Arguments # - target_points : Nx4 or Nx3 numpy array of the target point cloud @@ -22,10 +24,15 @@ def example_numpy1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd # - num_threads : Number of threads result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25) + print('--- registration result ---') + print(result) + return result.T_target_source # Example to perform preprocessing and registration separately def example_numpy2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray): + print('*** example_numpy2 ***') + # Example B : Perform preprocessing and registration separately # Preprocess point clouds @@ -38,6 +45,9 @@ def example_numpy2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25) source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25) + print('preprocessed target=', target) + print('preprocessed source=', source) + # Align point clouds # Arguments # - target : Target point cloud (small_gicp.PointCloud) @@ -48,12 +58,17 @@ def example_numpy2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd # - max_correspondence_distance : Maximum correspondence distance # - num_threads : Number of threads result = small_gicp.align(target, source, target_tree) - + + print('--- registration result ---') + print(result) + return result.T_target_source # Basic registation example with small_gicp.PointCloud def example_small1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray): + print('*** example_small1 ***') + # Convert numpy arrays (Nx3 or Nx4) to small_gicp.PointCloud target_raw = small_gicp.PointCloud(target_raw_numpy) source_raw = small_gicp.PointCloud(source_raw_numpy) @@ -61,13 +76,21 @@ def example_small1(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd # Preprocess point clouds target, target_tree = small_gicp.preprocess_points(target_raw, downsampling_resolution=0.25) source, source_tree = small_gicp.preprocess_points(source_raw, downsampling_resolution=0.25) - + + print('preprocessed target=', target) + print('preprocessed source=', source) + result = small_gicp.align(target, source, target_tree) + + print('--- registration result ---') + print(result) return result.T_target_source # Example to perform each preprocessing and registration separately def example_small2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.ndarray): + print('*** example_small2 ***') + # Convert numpy arrays (Nx3 or Nx4) to small_gicp.PointCloud target_raw = small_gicp.PointCloud(target_raw_numpy) source_raw = small_gicp.PointCloud(source_raw_numpy) @@ -79,13 +102,19 @@ def example_small2(target_raw_numpy : numpy.ndarray, source_raw_numpy : numpy.nd # KdTree construction target_tree = small_gicp.KdTree(target) source_tree = small_gicp.KdTree(source) - + # Estimate covariances small_gicp.estimate_covariances(target, target_tree) small_gicp.estimate_covariances(source, source_tree) + print('preprocessed target=', target) + print('preprocessed source=', source) + # Align point clouds result = small_gicp.align(target, source, target_tree) + + print('--- registration result ---') + print(result) return result.T_target_source From 70fab2387ad3a2890f989c4a6c21a0123e91fe6a Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Wed, 12 Jun 2024 10:22:46 +0900 Subject: [PATCH 2/2] fix exclude path for doxygen --- docs/Doxyfile | 2 +- include/small_gicp/ann/traits.hpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/docs/Doxyfile b/docs/Doxyfile index 6e41898..7f3c96e 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -917,7 +917,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = include/small_gicp/benchmark +EXCLUDE = ../include/small_gicp/benchmark/ # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/include/small_gicp/ann/traits.hpp b/include/small_gicp/ann/traits.hpp index 33daa10..7dc9642 100644 --- a/include/small_gicp/ann/traits.hpp +++ b/include/small_gicp/ann/traits.hpp @@ -16,14 +16,15 @@ struct Traits; /// @param tree Nearest neighbor search (e.g., KdTree) /// @param point Query point /// @param k Number of neighbors -/// @param k_index [out] Index of the nearest neighbor -/// @param k_sq_dist [out] Squared distance to the nearest neighbor +/// @param k_indices [out] Indices of k-nearest neighbors +/// @param k_sq_dists [out] Squared distances to k-nearest neighbors /// @return Number of found neighbors template size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { return Traits::knn_search(tree, point, k, k_indices, k_sq_dists); } +/// @brief Check if T has nearest_neighbor_search method. template struct has_nearest_neighbor_search { template ::nearest_neighbor_search, 0)> @@ -33,7 +34,7 @@ struct has_nearest_neighbor_search { static constexpr bool value = decltype(test((T*)nullptr))::value; }; -/// @brief Find the nearest neighbor. +/// @brief Find the nearest neighbor. If Traits::nearest_neighbor_search is not defined, fallback to knn_search with k=1. /// @param tree Nearest neighbor search (e.g., KdTree) /// @param point Query point /// @param k_index [out] Index of the nearest neighbor