Data

Tools

Indexes

Applications

Experiments

Open Source Science

small_gicp

small_gicp: Efficient and parallel algorithms for point cloud registration - Published in JOSS (2024)

https://github.com/koide3/small_gicp

Science Score: 98.0%

This score indicates how likely this project is to be science-related based on various indicators:

  • ✓
    CITATION.cff file
    Found CITATION.cff file
  • ✓
    codemeta.json file
    Found codemeta.json file
  • ✓
    .zenodo.json file
    Found .zenodo.json file
  • ✓
    DOI references
    Found 4 DOI reference(s) in README and JOSS metadata
  • ✓
    Academic publication links
    Links to: joss.theoj.org
  • ○
    Committers with academic emails
  • ○
    Institutional organization owner
  • ✓
    JOSS paper metadata
    Published in Journal of Open Source Software

Keywords

cpp icp multi-threading open3d pcl point-cloud-regstration pointcloud python registration scan-matching

Scientific Fields

Earth and Environmental Sciences Physical Sciences - 40% confidence
Last synced: 4 months ago · JSON representation ·

Repository

Efficient and parallel algorithms for point cloud registration [C++, Python]

Basic Info
  • Host: GitHub
  • Owner: koide3
  • License: mit
  • Language: C++
  • Default Branch: master
  • Homepage:
  • Size: 21.8 MB
Statistics
  • Stars: 727
  • Watchers: 12
  • Forks: 90
  • Open Issues: 22
  • Releases: 1
Topics
cpp icp multi-threading open3d pcl point-cloud-regstration pointcloud python registration scan-matching
Created almost 2 years ago · Last pushed 7 months ago
Metadata Files
Readme Contributing License Code of conduct Citation

README.md

small_gicp

small_gicp is a header-only C++ library providing efficient and parallelized algorithms for fine point cloud registration (ICP, Point-to-Plane ICP, GICP, VGICP, etc.). It is a refined and optimized version of its predecessor, fast_gicp, re-written from scratch with the following features.

  • Highly Optimized : The core registration algorithm implementation has been further optimized from fast_gicp, achieving up to 2x speed gain.
  • Fully parallerized : small_gicp offers parallel implementations of several preprocessing algorithms, making the entire registration process parallelized (e.g., Downsampling, KdTree construction, Normal/Covariance estimation). It supports OpenMP and Intel TBB as parallelism backends.
  • Minimum dependencies : The library requires only Eigen along with the bundled nanoflann and Sophus. Optionally, it supports a PCL registration interface for use as a drop-in replacement
  • Customizable : small_gicp allows the integration of any custom point cloud class into the registration algorithm via traits. Its template-based implementation enables customization of the registration process with original correspondence estimators and registration factors.
  • Python bindings : By being isolated from PCL, small_gicp's Python bindings are more portable and can be used seamlessly with other libraries such as Open3D.

Note that GPU-based implementations are NOT included in this package.

If you find this package useful for your project, please consider leaving a comment here. It would help the author receive recognition in his organization and keep working on this project. Please also cite the corresponding software paper if you use this software in an academic work.

status Build(Linux) macos Build(Windows) Test codecov

Requirements

This library uses C++17 features. The PCL interface is not compatible with PCL older than 1.11 that uses boost::shared_ptr.

Dependencies

Installation

C++

small_gicp is a header-only library. You can just download and drop it in your project directory to use it.

If you need only basic point cloud registration functions, you can build and install the helper library as follows.

```bash sudo apt-get install libeigen3-dev libomp-dev

cd smallgicp mkdir build && cd build cmake .. -DCMAKEBUILD_TYPE=Release && make -j sudo make install ```

Python (Linux / Windows / MacOS)

Install from PyPI

bash pip install small_gicp

Install from source

```bash cd small_gicp pip install .

[Optional (linux)] Install stubs for autocomplete (If you know a better way, let me know...)

pip install pybind11-stubgen cd ~/.local/lib/python3.10/site-packages pybind11-stubgen -o . --ignore-invalid=all small_gicp ```

Documentation

Usage (C++)

The following examples assume using namespace small_gicp is placed somewhere.

Using helper library (01basicregistration.cpp)

The helper library (registration_helper.hpp) enables easily processing point clouds represented as std::vector<Eigen::Vector(3|4)(f|d)>.

Expand

small_gicp::align takes two point clouds (std::vectors of Eigen::Vector(3|4)(f|d)) and returns a registration result (estimated transformation and some information on the optimization result). This is the easiest way to use small_gicp but causes an overhead for duplicated preprocessing.

```cpp

include

std::vectorEigen::Vector3d targetpoints = ...; // Any of Eigen::Vector(3|4)(f|d) can be used std::vectorEigen::Vector3d sourcepoints = ...; //

RegistrationSetting setting; setting.numthreads = 4; // Number of threads to be used setting.downsamplingresolution = 0.25; // Downsampling resolution setting.maxcorrespondencedistance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)

Eigen::Isometry3d initTtargetsource = Eigen::Isometry3d::Identity(); RegistrationResult result = align(targetpoints, sourcepoints, initTtargetsource, setting);

Eigen::Isometry3d T = result.Ttargetsource; // Estimated transformation sizet numinliers = result.num_inliers; // Number of inlier source points Eigen::Matrix H = result.H; // Final Hessian matrix (6x6) ```

There is also a way to perform preprocessing and registration separately. This enables saving time for preprocessing in case registration is performed several times for the same point cloud (e.g., typical odometry estimation based on scan-to-scan matching).

```cpp

include

std::vectorEigen::Vector3d targetpoints = ...; // Any of Eigen::Vector(3|4)(f|d) can be used std::vectorEigen::Vector3d sourcepoints = ...; //

int numthreads = 4; // Number of threads to be used double downsamplingresolution = 0.25; // Downsampling resolution int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation

// std::pair::Ptr> auto [target, targettree] = preprocesspoints(targetpoints, downsamplingresolution, numneighbors, numthreads); auto [source, sourcetree] = preprocesspoints(sourcepoints, downsamplingresolution, numneighbors, numthreads);

RegistrationSetting setting; setting.numthreads = numthreads; setting.maxcorrespondencedistance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold)

Eigen::Isometry3d initTtargetsource = Eigen::Isometry3d::Identity(); RegistrationResult result = align(*target, *source, *targettree, initTtarget_source, setting);

Eigen::Isometry3d T = result.Ttargetsource; // Estimated transformation sizet numinliers = result.num_inliers; // Number of inlier source points Eigen::Matrix H = result.H; // Final Hessian matrix (6x6) ```

Using PCL interface (02basicregistration_pcl.cpp)

The PCL interface allows using smallgicp as a drop-in replacement for pcl::Registration. It is also possible to directly feed pcl::PointCloud to algorithms implemented in smallgicp.

Expand ```cpp #include pcl::PointCloud::Ptr raw_target = ...; pcl::PointCloud::Ptr raw_source = ...; // small_gicp::voxelgrid_downsampling can directly operate on pcl::PointCloud. pcl::PointCloud::Ptr target = voxelgrid_sampling_omp(*raw_target, 0.25); pcl::PointCloud::Ptr source = voxelgrid_sampling_omp(*raw_source, 0.25); // RegistrationPCL is derived from pcl::Registration and has mostly the same interface as pcl::GeneralizedIterativeClosestPoint. RegistrationPCL reg; reg.setNumThreads(4); reg.setCorrespondenceRandomness(20); reg.setMaxCorrespondenceDistance(1.0); reg.setVoxelResolution(1.0); reg.setRegistrationType("VGICP"); // or "GICP" (default = "GICP") // Set input point clouds. reg.setInputTarget(target); reg.setInputSource(source); // Align point clouds. auto aligned = pcl::make_shared>(); reg.align(*aligned); // Swap source and target and align again. // This is useful when you want to re-use preprocessed point clouds for successive registrations (e.g., odometry estimation). reg.swapSourceAndTarget(); 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 pcl::PointCloud::Ptr raw_target = ...; pcl::PointCloud::Ptr raw_source = ...; // Downsample points and convert them into pcl::PointCloud. pcl::PointCloud::Ptr target = voxelgrid_sampling_omp, pcl::PointCloud>(*raw_target, 0.25); pcl::PointCloud::Ptr source = voxelgrid_sampling_omp, pcl::PointCloud>(*raw_source, 0.25); // Estimate covariances of points. const int num_threads = 4; const int num_neighbors = 20; estimate_covariances_omp(*target, num_neighbors, num_threads); estimate_covariances_omp(*source, num_neighbors, num_threads); // Create KdTree for target and source. auto target_tree = std::make_shared>>(target, KdTreeBuilderOMP(num_threads)); auto source_tree = std::make_shared>>(source, KdTreeBuilderOMP(num_threads)); Registration registration; registration.reduction.num_threads = num_threads; registration.rejector.max_dist_sq = 1.0; // Align point clouds. Note that the input point clouds are pcl::PointCloud. auto result = registration.align(*target, *source, *target_tree, Eigen::Isometry3d::Identity()); ```

Using Registration template (03registrationtemplate.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

```cpp

include

include

include

include

include

include

std::vectorEigen::Vector3d targetpoints = ...; // Any of Eigen::Vector(3|4)(f|d) can be used std::vectorEigen::Vector3d sourcepoints = ...; //

int numthreads = 4; double downsamplingresolution = 0.25; int numneighbors = 10; double maxcorrespondence_distance = 1.0;

// Convert to smallgicp::PointCloud auto target = std::makeshared(targetpoints); auto source = std::makeshared(source_points);

// Downsampling target = voxelgridsamplingomp(target, downsamplingresolution, numthreads); source = voxelgridsamplingomp(source, downsamplingresolution, numthreads);

// Create KdTree auto targettree = std::makeshared>(target, KdTreeBuilderOMP(numthreads)); auto sourcetree = std::makeshared>(source, KdTreeBuilderOMP(numthreads));

// Estimate point covariances estimatecovariancesomp(target, *targettree, numneighbors, numthreads); estimatecovariances_omp(source, *sourcetree, numneighbors, num_threads);

// GICP + OMP-based parallel reduction Registration registration; registration.reduction.numthreads = numthreads; registration.rejector.maxdistsq = maxcorrespondencedistance * maxcorrespondencedistance;

// Align point clouds Eigen::Isometry3d initTtargetsource = Eigen::Isometry3d::Identity(); auto result = registration.align(*target, *source, *targettree, initTtarget_source);

Eigen::Isometry3d T = result.Ttargetsource; // Estimated transformation sizet numinliers = result.num_inliers; // Number of inlier source points Eigen::Matrix H = result.H; // Final Hessian matrix (6x6) ```

See 03registrationtemplate.cpp for more detailed customization examples.

Cookbook

Usage (Python) basic_registration.py

Expand Example A : Perform registration with numpy arrays ```python # Align two point clouds using various ICP-like algorithms. # # Parameters # ---------- # target_points : NDArray[np.float64] # Nx3 or Nx4 matrix representing the target point cloud. # source_points : NDArray[np.float64] # Nx3 or Nx4 matrix representing the source point cloud. # init_T_target_source : np.ndarray[np.float64] # 4x4 matrix representing the initial transformation from target to source. # registration_type : str = 'GICP' # Type of registration algorithm to use ('ICP', 'PLANE_ICP', 'GICP', 'VGICP'). # voxel_resolution : float = 1.0 # Resolution of voxels used for correspondence search (used only in VGICP). # downsampling_resolution : float = 0.25 # Resolution for downsampling the point clouds. # max_correspondence_distance : float = 1.0 # Maximum distance for matching points between point clouds. # num_threads : int = 1 # Number of threads to use for parallel processing. # # Returns # ------- # RegistrationResult # Object containing the final transformation matrix and convergence status. result = small_gicp.align(target_raw_numpy, source_raw_numpy, downsampling_resolution=0.25) result.T_target_source # Estimated transformation (4x4 numpy array) result.converged # If true, the optimization converged successfully result.iterations # Number of iterations the optimization took result.num_inliers # Number of inlier points result.H # Final Hessian matrix (6x6 matrix) result.b # Final information vector (6D vector) result.e # Final error (float) ``` Example B : Perform preprocessing and registration separately ```python # Preprocess point cloud (downsampling, kdtree construction, and normal/covariance estimation) # # Parameters # ---------- # points : NDArray[np.float64] # Nx3 or Nx4 matrix representing the point cloud. # downsampling_resolution : float = 0.1 # Resolution for downsampling the point clouds. # num_neighbors : int = 20 # Number of neighbor points to usefor point normal/covariance estimation. # num_threads : int = 1 # Number of threads to use for parallel processing. # # Returns # ------- # PointCloud # Downsampled point cloud with estimated normals and covariances. # KdTree # KdTree for the downsampled point cloud 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) # `target` and `source` are small_gicp.PointCloud with the following methods target.size() # Number of points target.points() # Nx4 numpy array [x, y, z, 1] x N target.normals() # Nx4 numpy array [nx, ny, nz, 0] x N target.covs() # Array of 4x4 covariance matrices # Align two point clouds using specified ICP-like algorithms, utilizing point cloud and KD-tree inputs. # # Parameters # ---------- # target : PointCloud::ConstPtr # Pointer to the target point cloud. # source : PointCloud::ConstPtr # Pointer to the source point cloud. # target_tree : KdTree::ConstPtr, optional # Pointer to the KD-tree of the target for nearest neighbor search. If nullptr, a new tree is built. # init_T_target_source : NDArray[np.float64] # 4x4 matrix representing the initial transformation from target to source. # registration_type : str = 'GICP' # Type of registration algorithm to use ('ICP', 'PLANE_ICP', 'GICP'). # max_correspondence_distance : float = 1.0 # Maximum distance for corresponding point pairs. # num_threads : int = 1 # Number of threads to use for computation. # # Returns # ------- # RegistrationResult # Object containing the final transformation matrix and convergence status. result = small_gicp.align(target, source, target_tree) ``` Example C : Perform each of preprocessing steps one-by-one ```python # 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) # Downsampling target = small_gicp.voxelgrid_sampling(target_raw, 0.25) source = small_gicp.voxelgrid_sampling(source_raw, 0.25) # 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) # Align point clouds result = small_gicp.align(target, source, target_tree) ``` Example D: Example with Open3D ```python target_o3d = open3d.io.read_point_cloud('small_gicp/data/target.ply').paint_uniform_color([0, 1, 0]) source_o3d = open3d.io.read_point_cloud('small_gicp/data/source.ply').paint_uniform_color([0, 0, 1]) target, target_tree = small_gicp.preprocess_points(numpy.asarray(target_o3d.points), downsampling_resolution=0.25) source, source_tree = small_gicp.preprocess_points(numpy.asarray(source_o3d.points), downsampling_resolution=0.25) result = small_gicp.align(target, source, target_tree) source_o3d.transform(result.T_target_source) open3d.visualization.draw_geometries([target_o3d, source_o3d]) ```

Cookbook

Running examples

C++

```bash cd smallgicp mkdir build && cd build cmake .. -DBUILDEXAMPLES=ON && make -j

cd .. ./build/01basicregistration ./build/02basicregistrationpcl ./build/03registration_template ```

Python

```bash cd small_gicp pip install .

python3 src/example/basic_registration.py ```

Benchmark

Processing speed comparison between small_gicp and Open3D (youtube). small_comp

Downsampling

  • Single-threaded small_gicp::voxelgrid_sampling is about 1.3x faster than pcl::VoxelGrid.
  • Multi-threaded small_gicp::voxelgrid_sampling_tbb (6 threads) is about 3.2x faster than pcl::VoxelGrid.
  • small_gicp::voxelgrid_sampling provides accurate downsampling results that are nearly identical to those of pcl::VoxelGrid, while pcl::ApproximateVoxelGrid can produce spurious points (up to 2x more points).
  • small_gicp::voxelgrid_sampling can handle larger point clouds with finer voxel resolutions compared to pcl::VoxelGrid. For a point cloud with a width of 1000m, the minimum voxel resolution can be 0.5 mm.

downsampling_comp

KdTree construction

  • Multi-threaded implementation (TBB and OMP) can be up to 6x faster than the single-threaded version. The single-thread version performs almost equivalently to nanoflann.
  • The new KdTree implementation demonstrates good scalability due to its well-balanced task assignment.
  • This benchmark compares only the construction time (query time is not included). Nearest neighbor queries are included and evaluated in the following odometry estimation evaluation.

kdtree_time

Odometry estimation

  • Single-thread small_gicp::GICP is about 2.4x and 1.9x faster than pcl::GICP and fast_gicp::GICP, respectively.
  • small_gicp::(GICP|VGICP) demonstrates better multi-threaded scalability compared to fast_gicp::(GICP|VGICP).
  • small_gicp::GICP parallelized with TBB flow graph shows excellent scalability in many-threads scenarios (~128 threads), though with some latency degradation.
  • Outputs of small_gicp::GICP are almost identical to those of fast_gicp::GICP.

odometry_time

License

This package is released under the MIT license.

If you find this package useful for your project, please consider leaving a comment here. It would help the author receive recognition in his organization and keep working on this project.

Please also cite the following article if you use this software in an academic work.

@article{small_gicp, author = {Kenji Koide}, title = {{small\_gicp: Efficient and parallel algorithms for point cloud registration}}, journal = {Journal of Open Source Software}, month = aug, number = {100}, pages = {6948}, volume = {9}, year = {2024}, doi = {10.21105/joss.06948} }

Contact

Kenji Koide, National Institute of Advanced Industrial Science and Technology (AIST)

Owner

  • Login: koide3
  • Kind: user
  • Location: Japan
  • Company: AIST

JOSS Publication

small_gicp: Efficient and parallel algorithms for point cloud registration
Published
August 10, 2024
DOI
Volume 9, Issue 100, Page 6948
Authors
Kenji Koide ORCID
National Institute of Advanced Industrial Science and Technology (AIST), Japan
Editor
Patrick Diehl ORCID
Tags
Point cloud registration
View PDF Review Thread Software Archive

Citation (CITATION.cff) 

cff-version: "1.2.0"
authors:
- family-names: Koide
  given-names: Kenji
  orcid: "https://orcid.org/0000-0001-5361-1428"
contact:
- family-names: Koide
  given-names: Kenji
  orcid: "https://orcid.org/0000-0001-5361-1428"
doi: 10.5281/zenodo.13283012
message: If you use this software, please cite our article in the
  Journal of Open Source Software.
preferred-citation:
  authors:
  - family-names: Koide
    given-names: Kenji
    orcid: "https://orcid.org/0000-0001-5361-1428"
  date-published: 2024-08-10
  doi: 10.21105/joss.06948
  issn: 2475-9066
  issue: 100
  journal: Journal of Open Source Software
  publisher:
    name: Open Journals
  start: 6948
  title: "small_gicp: Efficient and parallel algorithms for point cloud
    registration"
  type: article
  url: "https://joss.theoj.org/papers/10.21105/joss.06948"
  volume: 9
title: "small_gicp: Efficient and parallel algorithms for point cloud
  registration"

GitHub Events

Total
  • Issues event: 26
  • Watch event: 283
  • Delete event: 5
  • Issue comment event: 68
  • Push event: 16
  • Pull request event: 20
  • Fork event: 36
  • Create event: 8
Last Year
  • Issues event: 26
  • Watch event: 283
  • Delete event: 5
  • Issue comment event: 68
  • Push event: 16
  • Pull request event: 20
  • Fork event: 36
  • Create event: 8

Committers

Last synced: 5 months ago

All Time
  • Total Commits: 151
  • Total Committers: 9
  • Avg Commits per committer: 16.778
  • Development Distribution Score (DDS): 0.106
Past Year
  • Commits: 16
  • Committers: 5
  • Avg Commits per committer: 3.2
  • Development Distribution Score (DDS): 0.25
Top Committers
Name Email Commits
k.koide k****e@a****p 135
Martin Valgur m****r@g****m 5
Atticus Zhou 1****z 3
Nikhil Khedekar n****r 2
Daisuke Nishimatsu 4****1 2
unclearness u****v@g****m 1
fateshelled 5****d 1
atm 7****a 1
Artem Voronov 4****t 1
Committer Domains (Top 20 + Academic)
aist.go.jp: 1

Issues and Pull Requests

Last synced: 4 months ago

All Time
  • Total issues: 49
  • Total pull requests: 141
  • Average time to close issues: 5 days
  • Average time to close pull requests: 1 day
  • Total issue authors: 37
  • Total pull request authors: 11
  • Average comments per issue: 2.43
  • Average comments per pull request: 0.95
  • Merged pull requests: 128
  • Bot issues: 0
  • Bot pull requests: 0
Past Year
  • Issues: 22
  • Pull requests: 24
  • Average time to close issues: 4 days
  • Average time to close pull requests: 3 days
  • Issue authors: 18
  • Pull request authors: 5
  • Average comments per issue: 1.18
  • Average comments per pull request: 0.75
  • Merged pull requests: 19
  • Bot issues: 0
  • Bot pull requests: 0
View more stats
Top Authors
Issue Authors
  • mcmingchang (4)
  • zhao-zhibo (3)
  • Wuqiqi123 (2)
  • LimHyungTae (2)
  • asoteman (2)
  • deliangye (2)
  • themightyoarfish (2)
  • G4419 (2)
  • grischi (1)
  • JACKLiuDay (1)
  • Seekerzero (1)
  • huiyan-dev (1)
  • VTrencsenyi (1)
  • InguChoi (1)
  • KAL-AL (1)
Pull Request Authors
  • koide3 (107)
  • valgur (9)
  • Atticuszz (5)
  • wep21 (4)
  • danielskatz (4)
  • nkhedekar (3)
  • Vor-Art (2)
  • fateshelled (2)
  • sutatoruta (2)
  • unclearness (2)
  • LimHyungTae (1)
Top Labels
Issue Labels
bug (14) enhancement (8)
Pull Request Labels

Packages

  • Total packages: 1
  • Total downloads:
    • pypi 3,752 last-month
  • Total dependent packages: 1
  • Total dependent repositories: 0
  • Total versions: 9
  • Total maintainers: 1
pypi.org: small-gicp

Efficient and parallelized algorithms for fine point cloud registration

  • Versions: 9
  • Dependent Packages: 1
  • Dependent Repositories: 0
  • Downloads: 3,752 Last month
Rankings
Dependent packages count: 9.6%
Average: 36.3%
Dependent repos count: 63.1%
Maintainers (1)
koide3
Last synced: 4 months ago

Dependencies

.github/workflows/test.yml actions
  • actions/checkout v2 composite
pyproject.toml pypi
.github/workflows/coverage.yml actions
  • actions/checkout v2 composite
  • codecov/codecov-action v4.0.1 composite
.github/workflows/build-linux.yml actions
  • actions/checkout v2 composite
  • docker/build-push-action v2 composite
  • docker/login-action v1 composite
.github/workflows/build-macos.yml actions
  • actions/checkout v4 composite
  • actions/setup-python v5 composite
.github/workflows/build-windows.yml actions
  • actions/checkout v4 composite
  • actions/setup-python v5 composite
  • microsoft/setup-msbuild v2 composite
.github/workflows/wheels.yml actions
  • actions/checkout v4 composite
  • actions/download-artifact v4 composite
  • actions/setup-python v5 composite
  • actions/upload-artifact v4 composite
  • docker/setup-qemu-action v3 composite
  • microsoft/setup-msbuild v2 composite
  • pypa/gh-action-pypi-publish release/v1 composite