├── .clang-format ├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md └── workflows │ ├── build-linux.yml │ ├── build-macos.yml │ ├── build-windows.yml │ ├── coverage.yml │ ├── gendoc.yml │ ├── license.yml │ ├── paper.yml │ ├── test.yml │ └── wheels.yml ├── .gitignore ├── BENCHMARK.md ├── CITATION.cff ├── CMakeLists.txt ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── cmake └── small_gicp-config.cmake.in ├── codecov.yml ├── data ├── T_target_source.txt ├── source.ply └── target.ply ├── docker ├── Dockerfile.pytest.gcc └── Dockerfile.pytest.llvm ├── docs ├── Doxyfile ├── Makefile ├── assets │ ├── downsampling_comp.png │ ├── downsampling_threads.png │ ├── kdtree_time.png │ ├── odometry_native.png │ └── odometry_time.png ├── conf.py ├── index.md ├── index.rst ├── paper.bib ├── paper.md └── small_gicp.rst ├── include └── small_gicp │ ├── ann │ ├── flat_container.hpp │ ├── gaussian_voxelmap.hpp │ ├── incremental_voxelmap.hpp │ ├── kdtree.hpp │ ├── kdtree_omp.hpp │ ├── kdtree_tbb.hpp │ ├── knn_result.hpp │ ├── projection.hpp │ ├── projective_search.hpp │ ├── sequential_voxelmap_accessor.hpp │ └── traits.hpp │ ├── benchmark │ ├── benchmark.hpp │ ├── benchmark_odom.hpp │ └── read_points.hpp │ ├── factors │ ├── general_factor.hpp │ ├── gicp_factor.hpp │ ├── icp_factor.hpp │ ├── plane_icp_factor.hpp │ └── robust_kernel.hpp │ ├── pcl │ ├── pcl_point.hpp │ ├── pcl_point_traits.hpp │ ├── pcl_proxy.hpp │ ├── pcl_registration.hpp │ └── pcl_registration_impl.hpp │ ├── points │ ├── eigen.hpp │ ├── point_cloud.hpp │ └── traits.hpp │ ├── registration │ ├── optimizer.hpp │ ├── reduction.hpp │ ├── reduction_omp.hpp │ ├── reduction_tbb.hpp │ ├── registration.hpp │ ├── registration_helper.hpp │ ├── registration_result.hpp │ ├── rejector.hpp │ └── termination_criteria.hpp │ └── util │ ├── downsampling.hpp │ ├── downsampling_omp.hpp │ ├── downsampling_tbb.hpp │ ├── fast_floor.hpp │ ├── lie.hpp │ ├── normal_estimation.hpp │ ├── normal_estimation_omp.hpp │ ├── normal_estimation_tbb.hpp │ ├── sort_omp.hpp │ ├── sort_tbb.hpp │ └── vector3i_hash.hpp ├── mkdocs.yml ├── package.xml ├── pyproject.toml ├── scripts ├── plot_downsampling.py ├── plot_kdtree.py ├── plot_odometry.py ├── plot_odometry_accuracy.py ├── plot_odometry_native.py ├── run_downsampling_benchmark.sh ├── run_kdtree_benchmark.sh ├── run_odometry_benchmark.sh └── run_odometry_benchmark_native.sh └── src ├── benchmark ├── downsampling_benchmark.cpp ├── kdtree_benchmark.cpp ├── odometry_benchmark.cpp ├── odometry_benchmark_fast_gicp.cpp ├── odometry_benchmark_fast_vgicp.cpp ├── odometry_benchmark_pcl.cpp ├── odometry_benchmark_small_gicp.cpp ├── odometry_benchmark_small_gicp_model_omp.cpp ├── odometry_benchmark_small_gicp_model_tbb.cpp ├── odometry_benchmark_small_gicp_omp.cpp ├── odometry_benchmark_small_gicp_pcl.cpp ├── odometry_benchmark_small_gicp_tbb.cpp ├── odometry_benchmark_small_gicp_tbb_flow.cpp ├── odometry_benchmark_small_vgicp_model_omp.cpp ├── odometry_benchmark_small_vgicp_model_tbb.cpp ├── odometry_benchmark_small_vgicp_omp.cpp └── odometry_benchmark_small_vgicp_tbb.cpp ├── example ├── 01_basic_registration.cpp ├── 02_basic_registration_pcl.cpp ├── 03_registration_template.cpp ├── basic_registration.py └── kitti_odometry.py ├── python ├── align.cpp ├── factors.cpp ├── kdtree.cpp ├── misc.cpp ├── pointcloud.cpp ├── preprocess.cpp ├── python.cpp ├── result.cpp └── voxelmap.cpp ├── small_gicp ├── benchmark │ └── benchmark_odom.cpp └── registration │ ├── registration.cpp │ └── registration_helper.cpp └── test ├── downsampling_test.cpp ├── helper_test.cpp ├── kdtree_synthetic_test.cpp ├── kdtree_test.cpp ├── normal_estimation_test.cpp ├── points_test.cpp ├── python_test.py ├── registration_test.cpp ├── sort_omp_test.cpp ├── sort_tbb_test.cpp └── vector_test.cpp /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Build the package 16 | 2. Run '...' 17 | 18 | **Expected behavior** 19 | A clear and concise description of what you expected to happen. 20 | 21 | **Screenshots** 22 | If applicable, add screenshots to help explain your problem. 23 | 24 | **Environment (please complete the following information):** 25 | - OS: [e.g. Ubuntu] 26 | - Version [e.g. 22.04] 27 | - Language [C++ / Python] 28 | 29 | **Additional context** 30 | Add any other context about the problem here. 31 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: enhancement 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Additional context** 17 | Add any other context or screenshots about the feature request here. 18 | -------------------------------------------------------------------------------- /.github/workflows/build-linux.yml: -------------------------------------------------------------------------------- 1 | name: linux 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | build: 16 | runs-on: ubuntu-latest 17 | strategy: 18 | matrix: 19 | DISTRO: [noble, jammy, focal] 20 | TOOLCHAIN: [gcc, llvm] 21 | 22 | steps: 23 | - uses: actions/checkout@v2 24 | with: 25 | submodules: recursive 26 | 27 | - name: Docker login 28 | continue-on-error: true 29 | uses: docker/login-action@v1 30 | with: 31 | username: ${{ secrets.DOCKER_USERNAME }} 32 | password: ${{ secrets.DOCKER_TOKEN }} 33 | 34 | - name: Docker build 35 | uses: docker/build-push-action@v2 36 | with: 37 | file: ${{github.workspace}}/docker/Dockerfile.pytest.${{ matrix.TOOLCHAIN }} 38 | build-args: BASE_IMAGE=ubuntu:${{ matrix.DISTRO }} 39 | context: . 40 | push: false 41 | -------------------------------------------------------------------------------- /.github/workflows/build-macos.yml: -------------------------------------------------------------------------------- 1 | name: macos 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | build: 16 | strategy: 17 | matrix: 18 | OS: [macos-14] 19 | 20 | runs-on: ${{ matrix.OS }} 21 | steps: 22 | - uses: actions/checkout@v4 23 | 24 | - uses: actions/setup-python@v5 25 | with: 26 | python-version: '3.12' 27 | cache: 'pip' 28 | 29 | - run: brew install eigen libomp 30 | 31 | - run: pip install numpy scipy pytest 32 | 33 | - run: pip install . 34 | env: 35 | OpenMP_ROOT: /opt/homebrew/opt/libomp 36 | 37 | - run: python -m pytest src/test/python_test.py -v 38 | -------------------------------------------------------------------------------- /.github/workflows/build-windows.yml: -------------------------------------------------------------------------------- 1 | name: windows 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | build: 16 | runs-on: windows-latest 17 | 18 | steps: 19 | - uses: actions/checkout@v4 20 | 21 | - uses: microsoft/setup-msbuild@v2 22 | 23 | - uses: actions/setup-python@v5 24 | with: 25 | python-version: '3.12' 26 | cache: 'pip' 27 | 28 | - run: pip install numpy scipy pytest 29 | 30 | - run: pip install . 31 | 32 | - run: python -m pytest src/test/python_test.py -v 33 | -------------------------------------------------------------------------------- /.github/workflows/coverage.yml: -------------------------------------------------------------------------------- 1 | name: Coverage 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | coverage: 16 | runs-on: ubuntu-24.04 17 | 18 | steps: 19 | - uses: actions/checkout@v2 20 | with: 21 | submodules: recursive 22 | 23 | - name: Install Dependencies 24 | run: | 25 | sudo apt-get -y update 26 | sudo apt-get install --no-install-recommends -y build-essential cmake python3-pip pybind11-dev libeigen3-dev libfmt-dev libtbb-dev libomp-dev libpcl-dev libgtest-dev lcov ninja-build 27 | pip install -U setuptools pytest pytest-cov numpy scipy 28 | 29 | - name: Build (C++) 30 | run: | 31 | mkdir build && cd build 32 | cmake .. -DBUILD_TESTS=ON -DBUILD_EXAMPLES=ON -DBUILD_BENCHMARKS=ON -DBUILD_WITH_TBB=ON -DBUILD_WITH_PCL=ON -DENABLE_COVERAGE=ON 33 | make -j$(nproc) 34 | 35 | - name: Test (C++) 36 | run: | 37 | cd build 38 | ctest -j$(nproc) 39 | make coverage 40 | 41 | - name: Build (Python) 42 | run: | 43 | pip install . --user 44 | 45 | - name: Test (Python) 46 | run: | 47 | pytest src/example/basic_registration.py --cov=src --cov-report=xml 48 | pytest src/test/python_test.py --cov=src --cov-report=xml 49 | 50 | - name: Upload coverage reports to Codecov 51 | uses: codecov/codecov-action@v4.0.1 52 | with: 53 | token: ${{ secrets.CODECOV_TOKEN }} 54 | slug: koide3/small_gicp 55 | files: build/coverage.info,coverage.xml 56 | -------------------------------------------------------------------------------- /.github/workflows/gendoc.yml: -------------------------------------------------------------------------------- 1 | name: gendoc 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | # Allows you to run this workflow manually from the Actions tab 8 | workflow_dispatch: 9 | 10 | jobs: 11 | gendoc: 12 | name: Generate documentation 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - uses: actions/checkout@v4 17 | 18 | - name: Configure Git Credentials 19 | run: | 20 | git config user.name github-actions[bot] 21 | git config user.email 41898282+github-actions[bot]@users.noreply.github.com 22 | 23 | - name : Install dependencies 24 | run: sudo apt-get install -y git cmake build-essential doxygen graphviz python3-dev python3-pip pybind11-dev libeigen3-dev libomp-dev python3-numpy python3-sphinx python3-sphinx-rtd-theme 25 | 26 | - name: Install python dependencies 27 | run: python -m pip install mkdocs mkdocs-material 28 | 29 | - name: Generate documentation 30 | run: cd docs && make all 31 | 32 | - uses: actions/upload-artifact@v4 33 | with: 34 | name: site 35 | path: ./site/* 36 | 37 | - name: Deploy documentation 38 | run: cd docs && make deploy 39 | -------------------------------------------------------------------------------- /.github/workflows/license.yml: -------------------------------------------------------------------------------- 1 | name: license 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | jobs: 12 | license_check: 13 | name: License check 14 | runs-on: ubuntu-latest 15 | 16 | steps: 17 | - uses: actions/checkout@v4 18 | 19 | - name: Check license 20 | run: | 21 | find include/small_gicp src/small_gicp -type f | xargs -I filename bash -c 'if ! grep -q SPDX-License-Identifier filename; then echo filename : lisence not found; exit 1; fi' -------------------------------------------------------------------------------- /.github/workflows/paper.yml: -------------------------------------------------------------------------------- 1 | name: Draft PDF 2 | on: 3 | push: 4 | branches: [ paper ] 5 | 6 | # Allows you to run this workflow manually from the Actions tab 7 | workflow_dispatch: 8 | 9 | jobs: 10 | paper: 11 | runs-on: ubuntu-latest 12 | name: Paper Draft 13 | steps: 14 | - name: Checkout 15 | uses: actions/checkout@v4 16 | - name: Build draft PDF 17 | uses: openjournals/openjournals-draft-action@master 18 | with: 19 | journal: joss 20 | # This should be the path to the paper within your repo. 21 | paper-path: docs/paper.md 22 | - name: Upload 23 | uses: actions/upload-artifact@v3 24 | with: 25 | name: paper 26 | # This is the output path where Pandoc will write the compiled 27 | # PDF. Note, this should be the same directory as the input 28 | # paper.md 29 | path: docs/paper.pdf 30 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: test 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: '**.md' 7 | pull_request: 8 | branches: [ master ] 9 | paths-ignore: '**.md' 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test: 16 | runs-on: ubuntu-24.04 17 | 18 | steps: 19 | - uses: actions/checkout@v2 20 | with: 21 | submodules: recursive 22 | 23 | - name: Install Dependencies 24 | run: | 25 | sudo apt-get -y update 26 | sudo apt-get install --no-install-recommends -y build-essential cmake python3-pip pybind11-dev libeigen3-dev libfmt-dev libtbb-dev libomp-dev libpcl-dev libgtest-dev ninja-build 27 | pip install -U setuptools pytest numpy scipy 28 | 29 | - name: Build 30 | run: | 31 | mkdir build && cd build 32 | cmake .. -DBUILD_TESTS=ON -DBUILD_EXAMPLES=ON -DBUILD_BENCHMARKS=ON -DBUILD_WITH_TBB=ON -DBUILD_WITH_PCL=ON 33 | cmake --build . -j$(nproc) 34 | ctest -j$(nproc) 35 | cd .. 36 | pip install . --user 37 | pytest src/example/basic_registration.py 38 | pytest src/test/python_test.py 39 | -------------------------------------------------------------------------------- /.github/workflows/wheels.yml: -------------------------------------------------------------------------------- 1 | name: wheels 2 | 3 | on: 4 | workflow_dispatch: 5 | push: 6 | tags: 7 | - '*' 8 | 9 | jobs: 10 | build_wheels: 11 | name: Build wheels on ${{ matrix.os }} 12 | runs-on: ${{ matrix.os }} 13 | strategy: 14 | matrix: 15 | os: [ubuntu-latest, windows-latest, macos-14] 16 | pyver: [cp38, cp39, cp310, cp311, cp312] 17 | 18 | steps: 19 | - uses: actions/checkout@v4 20 | 21 | - name: Set up QEMU 22 | if: runner.os == 'Linux' 23 | uses: docker/setup-qemu-action@v3 24 | with: 25 | platforms: arm64 26 | 27 | - if: runner.os == 'Windows' 28 | uses: microsoft/setup-msbuild@v2 29 | 30 | - uses: actions/setup-python@v5 31 | 32 | - name: Install pytest and cibuildwheel 33 | run: python -m pip install cibuildwheel==2.17.0 34 | 35 | - name: Build wheels 36 | run: python -m cibuildwheel --output-dir wheelhouse 37 | env: 38 | CIBW_BUILD: ${{ matrix.pyver }}-* 39 | CIBW_ARCHS_LINUX: auto aarch64 40 | # CIBW_ARCHS_LINUX: x86_64 41 | 42 | - uses: actions/upload-artifact@v4 43 | with: 44 | name: cibw-wheels-${{ matrix.os }}-${{ strategy.job-index }} 45 | path: ./wheelhouse/*.whl 46 | 47 | build_sdist: 48 | name: Build source distribution 49 | runs-on: ubuntu-latest 50 | steps: 51 | - uses: actions/checkout@v4 52 | 53 | - name: Build sdist 54 | run: pipx run build --sdist 55 | 56 | - uses: actions/upload-artifact@v4 57 | with: 58 | name: cibw-sdist 59 | path: dist/*.tar.gz 60 | 61 | upload_pypi: 62 | needs: [build_wheels, build_sdist] 63 | runs-on: ubuntu-latest 64 | environment: pypi 65 | permissions: 66 | id-token: write 67 | 68 | steps: 69 | - uses: actions/download-artifact@v4 70 | with: 71 | # unpacks all CIBW artifacts into dist/ 72 | pattern: cibw-* 73 | path: dist 74 | merge-multiple: true 75 | 76 | - uses: pypa/gh-action-pypi-publish@release/v1 77 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | build/* 3 | imgui.ini 4 | 5 | *.pyc 6 | 7 | dist/* 8 | small_gicp.egg-info/* 9 | 10 | scripts/results/* 11 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: "1.2.0" 2 | authors: 3 | - family-names: Koide 4 | given-names: Kenji 5 | orcid: "https://orcid.org/0000-0001-5361-1428" 6 | contact: 7 | - family-names: Koide 8 | given-names: Kenji 9 | orcid: "https://orcid.org/0000-0001-5361-1428" 10 | doi: 10.5281/zenodo.13283012 11 | message: If you use this software, please cite our article in the 12 | Journal of Open Source Software. 13 | preferred-citation: 14 | authors: 15 | - family-names: Koide 16 | given-names: Kenji 17 | orcid: "https://orcid.org/0000-0001-5361-1428" 18 | date-published: 2024-08-10 19 | doi: 10.21105/joss.06948 20 | issn: 2475-9066 21 | issue: 100 22 | journal: Journal of Open Source Software 23 | publisher: 24 | name: Open Journals 25 | start: 6948 26 | title: "small_gicp: Efficient and parallel algorithms for point cloud 27 | registration" 28 | type: article 29 | url: "https://joss.theoj.org/papers/10.21105/joss.06948" 30 | volume: 9 31 | title: "small_gicp: Efficient and parallel algorithms for point cloud 32 | registration" 33 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | ## Contributing 2 | 3 | Thank you for your support of this package. Your contribution for improving the package is welcome as long as you follow the [Code of Conduct](CODE_OF_CONDUCT.md). 4 | 5 | ## Issues 6 | 7 | Please open issues to report bugs and propose feature requests. When creating a new issue, please use a issue template and fill each item to better tell your situation to the maintainers. 8 | 9 | ## Pull Requests 10 | 11 | Please take a look at the Github official document [GitHub](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/creating-a-pull-request) for general recommendations to create a PR. 12 | In addition, please let your PR focus on a single topic (improvement or bugfix) and keep it as minimal as possible to reduce the review effort. 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Kenji Koide 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cmake/small_gicp-config.cmake.in: -------------------------------------------------------------------------------- 1 | # Config file for the small_gicp package 2 | # 3 | # Usage from an external project: 4 | # 5 | # find_package(small_gicp REQUIRED) 6 | # target_link_libraries(MY_TARGET_NAME small_gicp::small_gicp) 7 | # 8 | # Optionally, for TBB support in *_tbb.hpp headers also add: 9 | # 10 | # find_package(TBB REQUIRED) 11 | # target_link_libraries(MY_TARGET_NAME TBB::tbb TBB::tbbmalloc) 12 | # 13 | @PACKAGE_INIT@ 14 | 15 | include_guard() 16 | 17 | set(BUILD_WITH_OPENMP @BUILD_WITH_OPENMP@) 18 | 19 | include(CMakeFindDependencyMacro) 20 | find_dependency(Eigen3 REQUIRED) 21 | if (BUILD_WITH_OPENMP) 22 | find_dependency(OpenMP REQUIRED COMPONENTS CXX) 23 | endif() 24 | 25 | # For FindTBB.cmake 26 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") 27 | 28 | include("${CMAKE_CURRENT_LIST_DIR}/small_gicp-targets.cmake") 29 | -------------------------------------------------------------------------------- /codecov.yml: -------------------------------------------------------------------------------- 1 | ignore: 2 | - "include/small_gicp/benchmark/*" 3 | - "src/test/*" 4 | - "src/example/*" 5 | - "src/benchmark/*" 6 | - "src/small_gicp/benchmark/*" 7 | -------------------------------------------------------------------------------- /data/T_target_source.txt: -------------------------------------------------------------------------------- 1 | 0.999925 0.0121483 -0.00177009 0.488882 2 | -0.0121523 0.999924 -0.00228657 0.121214 3 | 0.00174218 0.00230791 0.999996 -0.0253342 4 | 0 0 0 1 -------------------------------------------------------------------------------- /data/source.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/data/source.ply -------------------------------------------------------------------------------- /data/target.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/data/target.ply -------------------------------------------------------------------------------- /docker/Dockerfile.pytest.gcc: -------------------------------------------------------------------------------- 1 | ARG BASE_IMAGE 2 | 3 | FROM ${BASE_IMAGE} 4 | 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | 7 | RUN apt-get update && apt-get install --no-install-recommends -y \ 8 | && apt-get install --no-install-recommends -y \ 9 | wget nano build-essential git cmake python3-dev python3-pip pybind11-dev \ 10 | libeigen3-dev libomp-dev ninja-build 11 | 12 | RUN mkdir -p ~/.config/pip 13 | RUN echo "[global]\nbreak-system-packages=true" > ~/.config/pip/pip.conf 14 | 15 | RUN pip install pytest numpy scipy 16 | 17 | COPY . /root/small_gicp 18 | 19 | WORKDIR /root/small_gicp 20 | RUN rm -rf build 21 | 22 | RUN pip install . 23 | RUN python3 -m pytest src/example/basic_registration.py 24 | RUN python3 -m pytest src/test/python_test.py 25 | 26 | WORKDIR / 27 | 28 | CMD ["bash"] -------------------------------------------------------------------------------- /docker/Dockerfile.pytest.llvm: -------------------------------------------------------------------------------- 1 | ARG BASE_IMAGE 2 | 3 | FROM ${BASE_IMAGE} 4 | 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | 7 | RUN apt-get update && apt-get install --no-install-recommends -y \ 8 | && apt-get install --no-install-recommends -y \ 9 | wget nano build-essential git cmake python3-dev python3-pip pybind11-dev \ 10 | libeigen3-dev libomp-dev ninja-build 11 | 12 | RUN apt-get update && apt-get install --no-install-recommends -y \ 13 | && apt-get install --no-install-recommends -y \ 14 | clang lld 15 | 16 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 17 | ENV CC=clang 18 | ENV CXX=clang++ 19 | 20 | RUN mkdir -p ~/.config/pip 21 | RUN echo "[global]\nbreak-system-packages=true" > ~/.config/pip/pip.conf 22 | 23 | RUN pip install pytest numpy scipy 24 | 25 | COPY . /root/small_gicp 26 | 27 | WORKDIR /root/small_gicp 28 | RUN rm -rf build 29 | 30 | RUN pip install . 31 | RUN python3 -m pytest src/example/basic_registration.py 32 | RUN python3 -m pytest src/test/python_test.py 33 | 34 | WORKDIR / 35 | 36 | CMD ["bash"] -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: help 2 | help: 3 | @echo "make cpp|py|all" 4 | 5 | .PHONY: cpp 6 | cpp: 7 | @echo "Building C++ documentation..." 8 | doxygen Doxyfile doc_cpp 9 | 10 | .PHONY: py 11 | py: 12 | @echo "Building Python documentation..." 13 | mkdir -p build && cd build && cmake ../../ -DBUILD_PYTHON_BINDINGS=ON && make -j 14 | sphinx-build -b singlehtml . ./doc_py/ 15 | 16 | .PHONY: mkdocs 17 | mkdocs: 18 | @echo "Building MkDocs documentation..." 19 | cd .. && mkdocs build 20 | 21 | .PHONY: all 22 | all: cpp py mkdocs 23 | @echo "All documentation built." 24 | 25 | .PHONY: deploy 26 | deploy: 27 | @echo "Deploying documentation..." 28 | cd .. && mkdocs gh-deploy --force 29 | -------------------------------------------------------------------------------- /docs/assets/downsampling_comp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/docs/assets/downsampling_comp.png -------------------------------------------------------------------------------- /docs/assets/downsampling_threads.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/docs/assets/downsampling_threads.png -------------------------------------------------------------------------------- /docs/assets/kdtree_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/docs/assets/kdtree_time.png -------------------------------------------------------------------------------- /docs/assets/odometry_native.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/docs/assets/odometry_native.png -------------------------------------------------------------------------------- /docs/assets/odometry_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/small_gicp/08bc50beff6d77b7f663e57632b5a33e167d0905/docs/assets/odometry_time.png -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | # -- Project information ----------------------------------------------------- 7 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 8 | 9 | project = 'small_gicp' 10 | copyright = '2024, k.koide' 11 | author = 'k.koide' 12 | version = '1.0.0' 13 | 14 | import os 15 | import sys 16 | sys.path.insert(0, os.path.abspath('./build/')) 17 | 18 | # -- General configuration --------------------------------------------------- 19 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 20 | 21 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.napoleon'] 22 | 23 | templates_path = ['_templates'] 24 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 25 | 26 | autoclass_content = 'both' 27 | 28 | # -- Options for HTML output ------------------------------------------------- 29 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 30 | 31 | html_theme = 'sphinx_rtd_theme' 32 | html_static_path = ['_static'] 33 | -------------------------------------------------------------------------------- /docs/index.md: -------------------------------------------------------------------------------- 1 | # small_gicp 2 | 3 | ## Documentation 4 | 5 | - [C++](doc_cpp/index.html) 6 | - [Python](doc_py/index.html) -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. small_gicp documentation master file, created by 2 | sphinx-quickstart on Wed Jun 5 11:07:43 2024. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to small_gicp's documentation! 7 | ====================================== 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | small_gicp 14 | 15 | 16 | 17 | Indices and tables 18 | ================== 19 | 20 | * :ref:`genindex` 21 | * :ref:`modindex` 22 | * :ref:`search` 23 | -------------------------------------------------------------------------------- /docs/paper.bib: -------------------------------------------------------------------------------- 1 | @InProceedings{Rusu, 2 | author = {Radu Bogdan Rusu and Steve Cousins}, 3 | title = {{3D is here: Point Cloud Library (PCL)}}, 4 | booktitle = {{IEEE International Conference on Robotics and Automation (ICRA2011)}}, 5 | month = {May}, 6 | year = {2011}, 7 | address = {Shanghai, China}, 8 | doi = {10.1109/ICRA.2011.5980567} 9 | } 10 | 11 | @article{Zhou, 12 | author = {Qian-Yi Zhou and Jaesik Park and Vladlen Koltun}, 13 | title = {{Open3D}: {A} Modern Library for {3D} Data Processing}, 14 | journal = {arXiv:1801.09847}, 15 | year = {2018}, 16 | doi = {10.48550/arXiv.1801.09847} 17 | } 18 | 19 | @ARTICLE{Bai, 20 | author={Bai, Chunge and Xiao, Tao and Chen, Yajie and Wang, Haoqian and Zhang, Fang and Gao, Xiang}, 21 | journal={IEEE Robotics and Automation Letters}, 22 | title={Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels}, 23 | year={2022}, 24 | volume={7}, 25 | number={2}, 26 | pages={4861-4868}, 27 | doi={10.1109/LRA.2022.3152830} 28 | } 29 | 30 | @inproceedings{Koide, 31 | title = {Voxelized GICP for Fast and Accurate 3D Point Cloud Registration}, 32 | author = {Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno}, 33 | booktitle = {IEEE International Conference on Robotics and Automation (ICRA2021)}, 34 | pages = {11054--11059}, 35 | year = {2021}, 36 | month = {May}, 37 | doi = {10.1109/ICRA48506.2021.9560835} 38 | } 39 | 40 | @article{Zhang, 41 | title={Iterative point matching for registration of free-form curves and surfaces}, 42 | author={Zhang, Zhengyou}, 43 | journal={International journal of computer vision}, 44 | volume={13}, 45 | number={2}, 46 | pages={119--152}, 47 | year={1994}, 48 | publisher={Springer}, 49 | doi={10.1007/BF01427149} 50 | } 51 | 52 | @inproceedings{Segal, 53 | title={Generalized-{ICP}}, 54 | author={Segal, Aleksandr and Haehnel, Dirk and Thrun, Sebastian}, 55 | booktitle={Robotics: science and systems}, 56 | volume={2}, 57 | number={4}, 58 | pages={435}, 59 | year={2009}, 60 | doi={10.15607/rss.2009.v.021} 61 | } 62 | 63 | @INPROCEEDINGS{Wang, 64 | author={Wang, Han and Wang, Chen and Xie, Lihua}, 65 | booktitle={IEEE International Conference on Robotics and Automation (ICRA2020)}, 66 | title={Intensity Scan Context: Coding Intensity and Geometry Relations for Loop Closure Detection}, 67 | year={2020}, 68 | pages={2095-2101}, 69 | doi={10.1109/ICRA40945.2020.9196764} 70 | } 71 | 72 | @inproceedings{Izadinia, 73 | title={Scene recomposition by learning-based icp}, 74 | author={Izadinia, Hamid and Seitz, Steven M}, 75 | booktitle={IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR2020)}, 76 | pages={930--939}, 77 | year={2020}, 78 | doi={10.1109/cvpr42600.2020.00101} 79 | } 80 | 81 | @ARTICLE{Kim, 82 | author={Kim, Kyuwon and Im, Junhyuck and Jee, Gyuin}, 83 | journal={IEEE Transactions on Intelligent Transportation Systems}, 84 | title={Tunnel Facility Based Vehicle Localization in Highway Tunnel Using 3D LIDAR}, 85 | year={2022}, 86 | volume={23}, 87 | number={10}, 88 | pages={17575-17583}, 89 | doi={10.1109/TITS.2022.3160235} 90 | } 91 | 92 | @article{Pomerleau, 93 | author = {Pomerleau, Fran{\c c}ois and Colas, Francis and Siegwart, Roland and Magnenat, St{\'e}phane}, 94 | title = {{Comparing ICP Variants on Real-World Data Sets}}, 95 | journal = {Autonomous Robots}, 96 | year = {2013}, 97 | volume = {34}, 98 | number = {3}, 99 | pages = {133--148}, 100 | month = feb, 101 | doi={10.1007/s10514-013-9327-2} 102 | } 103 | 104 | @INPROCEEDINGS{Serafin, 105 | author={Serafin, Jacopo and Grisetti, Giorgio}, 106 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS2015)}, 107 | title={NICP: Dense normal based point cloud registration}, 108 | year={2015}, 109 | pages={742-749}, 110 | doi={10.1109/IROS.2015.7353455}} 111 | 112 | 113 | @inproceedings{Datar, 114 | author = {Datar, Mayur and Immorlica, Nicole and Indyk, Piotr and Mirrokni, Vahab S.}, 115 | title = {Locality-sensitive hashing scheme based on p-stable distributions}, 116 | year = {2004}, 117 | doi = {10.1145/997817.997857}, 118 | booktitle = {Twentieth Annual Symposium on Computational Geometry}, 119 | pages = {253–262}, 120 | } -------------------------------------------------------------------------------- /docs/small_gicp.rst: -------------------------------------------------------------------------------- 1 | small\_gicp module 2 | ================== 3 | 4 | .. automodule:: small_gicp 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | -------------------------------------------------------------------------------- /include/small_gicp/ann/gaussian_voxelmap.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace small_gicp { 13 | 14 | /// @brief Gaussian voxel that computes and stores voxel mean and covariance. 15 | struct GaussianVoxel { 16 | public: 17 | struct Setting {}; 18 | 19 | /// @brief Constructor. 20 | GaussianVoxel() : finalized(false), num_points(0), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {} 21 | 22 | /// @brief Number of points in the voxel (Always 1 for GaussianVoxel). 23 | size_t size() const { return 1; } 24 | 25 | /// @brief Add a point to the voxel. 26 | /// @param setting Setting 27 | /// @param transformed_pt Transformed point mean 28 | /// @param points Point cloud 29 | /// @param i Index of the point 30 | /// @param T Transformation matrix 31 | template 32 | void add(const Setting& setting, const Eigen::Vector4d& transformed_pt, const PointCloud& points, size_t i, const Eigen::Isometry3d& T) { 33 | if (finalized) { 34 | this->finalized = false; 35 | this->mean *= num_points; 36 | this->cov *= num_points; 37 | } 38 | 39 | num_points++; 40 | this->mean += transformed_pt; 41 | this->cov += T.matrix() * traits::cov(points, i) * T.matrix().transpose(); 42 | } 43 | 44 | /// @brief Finalize the voxel mean and covariance. 45 | void finalize() { 46 | if (finalized) { 47 | return; 48 | } 49 | 50 | mean /= num_points; 51 | cov /= num_points; 52 | finalized = true; 53 | } 54 | 55 | public: 56 | bool finalized; ///< If true, mean and cov are finalized, otherwise they represent the sum of input points 57 | size_t num_points; ///< Number of input points 58 | Eigen::Vector4d mean; ///< Mean 59 | Eigen::Matrix4d cov; ///< Covariance 60 | }; 61 | 62 | namespace traits { 63 | 64 | template <> 65 | struct Traits { 66 | static size_t size(const GaussianVoxel& voxel) { return 1; } 67 | static bool has_points(const GaussianVoxel& voxel) { return true; } 68 | static bool has_covs(const GaussianVoxel& voxel) { return true; } 69 | 70 | static const Eigen::Vector4d& point(const GaussianVoxel& voxel, size_t i) { return voxel.mean; } 71 | static const Eigen::Matrix4d& cov(const GaussianVoxel& voxel, size_t i) { return voxel.cov; } 72 | 73 | static size_t nearest_neighbor_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, size_t* k_index, double* k_sq_dist) { 74 | *k_index = 0; 75 | *k_sq_dist = (voxel.mean - pt).squaredNorm(); 76 | return 1; 77 | } 78 | 79 | static size_t knn_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, size_t k, size_t* k_index, double* k_sq_dist) { 80 | return nearest_neighbor_search(voxel, pt, k_index, k_sq_dist); 81 | } 82 | 83 | template 84 | static void knn_search(const GaussianVoxel& voxel, const Eigen::Vector4d& pt, Result& result) { 85 | result.push(0, (voxel.mean - pt).squaredNorm()); 86 | } 87 | }; 88 | 89 | } // namespace traits 90 | 91 | using GaussianVoxelMap = IncrementalVoxelMap; 92 | 93 | } // namespace small_gicp 94 | -------------------------------------------------------------------------------- /include/small_gicp/ann/kdtree_omp.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #ifdef _MSC_VER 9 | #pragma message("warning: Task-based OpenMP parallelism causes run-time memory errors with Eigen matrices.") 10 | #pragma message("warning: Thus, OpenMP-based multi-threading for KdTree construction is disabled on MSVC.") 11 | #endif 12 | 13 | namespace small_gicp { 14 | 15 | /// @brief Kd-tree builder with OpenMP. 16 | struct KdTreeBuilderOMP { 17 | public: 18 | /// @brief Constructor 19 | /// @param num_threads Number of threads 20 | KdTreeBuilderOMP(int num_threads = 4) : num_threads(num_threads), max_leaf_size(20) {} 21 | 22 | /// @brief Build KdTree 23 | template 24 | void build_tree(KdTree& kdtree, const PointCloud& points) const { 25 | kdtree.indices.resize(traits::size(points)); 26 | std::iota(kdtree.indices.begin(), kdtree.indices.end(), 0); 27 | 28 | std::atomic_uint64_t node_count = 0; 29 | kdtree.nodes.resize(traits::size(points)); 30 | 31 | #ifndef _MSC_VER 32 | #pragma omp parallel num_threads(num_threads) 33 | { 34 | #pragma omp single nowait 35 | { kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); } 36 | } 37 | #else 38 | kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); 39 | #endif 40 | 41 | kdtree.nodes.resize(node_count); 42 | } 43 | 44 | /// @brief Create a Kd-tree node from the given point indices. 45 | /// @param global_first Global first point index iterator (i.e., this->indices.begin()). 46 | /// @param first First point index iterator to be scanned. 47 | /// @param last Last point index iterator to be scanned. 48 | /// @return Index of the created node. 49 | template 50 | NodeIndexType create_node( 51 | KdTree& kdtree, 52 | std::atomic_uint64_t& node_count, 53 | const PointCloud& points, 54 | IndexConstIterator global_first, 55 | IndexConstIterator first, 56 | IndexConstIterator last) const { 57 | const size_t N = std::distance(first, last); 58 | const NodeIndexType node_index = node_count++; 59 | auto& node = kdtree.nodes[node_index]; 60 | 61 | // Create a leaf node. 62 | if (N <= max_leaf_size) { 63 | // std::sort(first, last); 64 | node.node_type.lr.first = std::distance(global_first, first); 65 | node.node_type.lr.last = std::distance(global_first, last); 66 | 67 | return node_index; 68 | } 69 | 70 | // Find the best axis to split the input points. 71 | using Projection = typename KdTree::Projection; 72 | const auto proj = Projection::find_axis(points, first, last, projection_setting); 73 | const auto median_itr = first + N / 2; 74 | std::nth_element(first, median_itr, last, [&](size_t i, size_t j) { return proj(traits::point(points, i)) < proj(traits::point(points, j)); }); 75 | 76 | // Create a non-leaf node. 77 | node.node_type.sub.proj = proj; 78 | node.node_type.sub.thresh = proj(traits::point(points, *median_itr)); 79 | 80 | // Create left and right child nodes. 81 | #ifndef _MSC_VER 82 | #pragma omp task default(shared) if (N > 512) 83 | node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); 84 | #pragma omp task default(shared) if (N > 512) 85 | node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); 86 | #pragma omp taskwait 87 | #else 88 | node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); 89 | node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); 90 | #endif 91 | 92 | return node_index; 93 | } 94 | 95 | public: 96 | int num_threads; ///< Number of threads 97 | int max_leaf_size; ///< Maximum number of points in a leaf node. 98 | ProjectionSetting projection_setting; ///< Projection setting. 99 | }; 100 | 101 | } // namespace small_gicp 102 | -------------------------------------------------------------------------------- /include/small_gicp/ann/kdtree_tbb.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace small_gicp { 10 | 11 | /// @brief Kd-tree builder with TBB. 12 | struct KdTreeBuilderTBB { 13 | public: 14 | /// @brief Build KdTree 15 | template 16 | void build_tree(KdTree& kdtree, const PointCloud& points) const { 17 | kdtree.indices.resize(traits::size(points)); 18 | std::iota(kdtree.indices.begin(), kdtree.indices.end(), 0); 19 | 20 | std::atomic_uint64_t node_count = 0; 21 | kdtree.nodes.resize(traits::size(points)); 22 | kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); 23 | kdtree.nodes.resize(node_count); 24 | } 25 | 26 | /// @brief Create a Kd-tree node from the given point indices. 27 | /// @param global_first Global first point index iterator (i.e., this->indices.begin()). 28 | /// @param first First point index iterator to be scanned. 29 | /// @param last Last point index iterator to be scanned. 30 | /// @return Index of the created node. 31 | template 32 | NodeIndexType create_node( 33 | KdTree& kdtree, 34 | std::atomic_uint64_t& node_count, 35 | const PointCloud& points, 36 | IndexConstIterator global_first, 37 | IndexConstIterator first, 38 | IndexConstIterator last) const { 39 | const size_t N = std::distance(first, last); 40 | const NodeIndexType node_index = node_count++; 41 | auto& node = kdtree.nodes[node_index]; 42 | 43 | // Create a leaf node. 44 | if (N <= max_leaf_size) { 45 | // std::sort(first, last); 46 | node.node_type.lr.first = std::distance(global_first, first); 47 | node.node_type.lr.last = std::distance(global_first, last); 48 | 49 | return node_index; 50 | } 51 | 52 | // Find the best axis to split the input points. 53 | using Projection = typename KdTree::Projection; 54 | const auto proj = Projection::find_axis(points, first, last, projection_setting); 55 | const auto median_itr = first + N / 2; 56 | std::nth_element(first, median_itr, last, [&](size_t i, size_t j) { return proj(traits::point(points, i)) < proj(traits::point(points, j)); }); 57 | 58 | // Create a non-leaf node. 59 | node.node_type.sub.proj = proj; 60 | node.node_type.sub.thresh = proj(traits::point(points, *median_itr)); 61 | 62 | // Create left and right child nodes. 63 | if (N > 512) { 64 | tbb::parallel_invoke( 65 | [&] { node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); }, 66 | [&] { node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); }); 67 | } else { 68 | node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); 69 | node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); 70 | } 71 | 72 | return node_index; 73 | } 74 | 75 | public: 76 | int max_leaf_size = 20; ///< Maximum number of points in a leaf node. 77 | ProjectionSetting projection_setting; ///< Projection setting. 78 | }; 79 | 80 | } // namespace small_gicp 81 | -------------------------------------------------------------------------------- /include/small_gicp/ann/knn_result.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | /// @brief K-nearest neighbor search setting. 13 | struct KnnSetting { 14 | public: 15 | /// @brief Check if the result satisfies the early termination condition. 16 | template 17 | bool fulfilled(const Result& result) const { 18 | return result.worst_distance() < epsilon; 19 | } 20 | 21 | public: 22 | double epsilon = 0.0; ///< Early termination threshold 23 | }; 24 | 25 | /// @brief Identity transform (alternative to std::identity in C++20). 26 | struct identity_transform { 27 | size_t operator()(size_t i) const { return i; } 28 | }; 29 | 30 | /// @brief K-nearest neighbor search result container. 31 | /// @tparam N Number of neighbors to search. If N == -1, the number of neighbors is dynamicaly determined. 32 | template 33 | struct KnnResult { 34 | public: 35 | static constexpr size_t INVALID = std::numeric_limits::max(); 36 | 37 | /// @brief Constructor 38 | /// @param indices Buffer to store indices (must be larger than k=max(N, num_neighbors)) 39 | /// @param distances Buffer to store distances (must be larger than k=max(N, num_neighbors)) 40 | /// @param num_neighbors Number of neighbors to search (must be -1 for static case N > 0) 41 | explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1, const IndexTransform& index_transform = identity_transform()) 42 | : index_transform(index_transform), 43 | capacity(num_neighbors), 44 | num_found_neighbors(0), 45 | indices(indices), 46 | distances(distances) { 47 | if constexpr (N > 0) { 48 | if (num_neighbors >= 0) { 49 | std::cerr << "warning: Specifying dynamic num_neighbors=" << num_neighbors << " for a static KNN result container (N=" << N << ")" << std::endl; 50 | abort(); 51 | } 52 | } else { 53 | if (num_neighbors <= 0) { 54 | std::cerr << "error: Specifying invalid num_neighbors=" << num_neighbors << " for a dynamic KNN result container" << std::endl; 55 | abort(); 56 | } 57 | } 58 | 59 | std::fill(this->indices, this->indices + buffer_size(), INVALID); 60 | std::fill(this->distances, this->distances + buffer_size(), std::numeric_limits::max()); 61 | } 62 | 63 | /// @brief Buffer size (i.e., Maximum number of neighbors) 64 | size_t buffer_size() const { 65 | if constexpr (N > 0) { 66 | return N; 67 | } else { 68 | return capacity; 69 | } 70 | } 71 | 72 | /// @brief Number of found neighbors. 73 | size_t num_found() const { return num_found_neighbors; } 74 | 75 | /// @brief Worst distance in the result. 76 | double worst_distance() const { return distances[buffer_size() - 1]; } 77 | 78 | /// @brief Push a pair of point index and distance to the result. 79 | /// @note The result is sorted by distance in ascending order. 80 | void push(size_t index, double distance) { 81 | if (distance >= worst_distance()) { 82 | return; 83 | } 84 | 85 | if constexpr (N == 1) { 86 | indices[0] = index_transform(index); 87 | distances[0] = distance; 88 | } else { 89 | int insert_loc = std::min(num_found_neighbors, buffer_size() - 1); 90 | for (; insert_loc > 0 && distance < distances[insert_loc - 1]; insert_loc--) { 91 | indices[insert_loc] = indices[insert_loc - 1]; 92 | distances[insert_loc] = distances[insert_loc - 1]; 93 | } 94 | 95 | indices[insert_loc] = index_transform(index); 96 | distances[insert_loc] = distance; 97 | } 98 | 99 | num_found_neighbors = std::min(num_found_neighbors + 1, buffer_size()); 100 | } 101 | 102 | public: 103 | const IndexTransform index_transform; ///< Point index transformation (e.g., local point index to global point/voxel index) 104 | const int capacity; ///< Maximum number of neighbors to search 105 | int num_found_neighbors; ///< Number of found neighbors 106 | size_t* indices; ///< Indices of neighbors 107 | double* distances; ///< Distances to neighbors 108 | }; 109 | 110 | } // namespace small_gicp 111 | -------------------------------------------------------------------------------- /include/small_gicp/ann/projection.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace small_gicp { 10 | 11 | /// @brief Parameters to control the projection axis search. 12 | struct ProjectionSetting { 13 | int max_scan_count = 128; ///< Maximum number of points to use for the axis search. 14 | }; 15 | 16 | /// @brief Conventional axis-aligned projection (i.e., selecting any of XYZ axes with the largest variance). 17 | /// @note Up to max_scan_count samples are used to estimate the variance. 18 | struct AxisAlignedProjection { 19 | public: 20 | /// @brief Project the point to the selected axis. 21 | /// @param pt Point to project 22 | /// @return Projected value 23 | double operator()(const Eigen::Vector4d& pt) const { return pt[axis]; } 24 | 25 | /// @brief Find the axis with the largest variance. 26 | /// @param points Point cloud 27 | /// @param first First point index iterator 28 | /// @param last Last point index iterator 29 | /// @param setting Search setting 30 | /// @return Projection with the largest variance axis 31 | template 32 | static AxisAlignedProjection find_axis(const PointCloud& points, IndexConstIterator first, IndexConstIterator last, const ProjectionSetting& setting) { 33 | const size_t N = std::distance(first, last); 34 | Eigen::Vector4d sum_pt = Eigen::Vector4d::Zero(); 35 | Eigen::Vector4d sum_sq = Eigen::Vector4d::Zero(); 36 | 37 | const size_t step = N < setting.max_scan_count ? 1 : N / setting.max_scan_count; 38 | const size_t num_steps = N / step; 39 | for (int i = 0; i < num_steps; i++) { 40 | const auto itr = first + step * i; 41 | const Eigen::Vector4d pt = traits::point(points, *itr); 42 | sum_pt += pt; 43 | sum_sq += pt.cwiseProduct(pt); 44 | } 45 | 46 | const Eigen::Vector4d mean = sum_pt / sum_pt.w(); 47 | const Eigen::Vector4d var = (sum_sq - mean.cwiseProduct(sum_pt)); 48 | 49 | return AxisAlignedProjection{var[0] > var[1] ? (var[0] > var[2] ? 0 : 2) : (var[1] > var[2] ? 1 : 2)}; 50 | } 51 | 52 | public: 53 | int axis; ///< Axis index (0: X, 1: Y, 2: Z) 54 | }; 55 | 56 | /// @brief Normal projection (i.e., selecting the 3D direction with the largest variance of the points). 57 | /// @note Up to max_scan_count samples are used to estimate the variance along the axis. 58 | struct NormalProjection { 59 | public: 60 | /// @brief Project the point to the normal direction. 61 | /// @param pt Point to project 62 | /// @return Projected value 63 | double operator()(const Eigen::Vector4d& pt) const { return Eigen::Map(normal.data()).dot(pt.head<3>()); } 64 | 65 | /// @brief Find the direction with the largest variance. 66 | /// @param points Point cloud 67 | /// @param first First point index iterator 68 | /// @param last Last point index iterator 69 | /// @param setting Search setting 70 | /// @return Projection with the largest variance direction 71 | template 72 | static NormalProjection find_axis(const PointCloud& points, IndexConstIterator first, IndexConstIterator last, const ProjectionSetting& setting) { 73 | const size_t N = std::distance(first, last); 74 | Eigen::Vector4d sum_pt = Eigen::Vector4d::Zero(); 75 | Eigen::Matrix4d sum_sq = Eigen::Matrix4d::Zero(); 76 | 77 | const size_t step = N < setting.max_scan_count ? 1 : N / setting.max_scan_count; 78 | const size_t num_steps = N / step; 79 | for (int i = 0; i < num_steps; i++) { 80 | const auto itr = first + step * i; 81 | const Eigen::Vector4d pt = traits::point(points, *itr); 82 | sum_pt += pt; 83 | sum_sq += pt * pt.transpose(); 84 | } 85 | 86 | const Eigen::Vector4d mean = sum_pt / sum_pt.w(); 87 | const Eigen::Matrix4d cov = (sum_sq - mean * sum_pt.transpose()) / sum_pt.w(); 88 | 89 | Eigen::SelfAdjointEigenSolver eig; 90 | eig.computeDirect(cov.block<3, 3>(0, 0)); 91 | 92 | NormalProjection p; 93 | Eigen::Map(p.normal.data()) = eig.eigenvectors().col(2); 94 | return p; 95 | } 96 | 97 | public: 98 | std::array normal; ///< Projection direction 99 | }; 100 | 101 | } // namespace small_gicp 102 | -------------------------------------------------------------------------------- /include/small_gicp/ann/sequential_voxelmap_accessor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /** 11 | * @brief A wrapper class to sequentially access points in a voxelmap (e.g., using points in a voxelmap as source point cloud for registration). 12 | * @note If the contents of the voxelmap are changed, the accessor must be recreated. 13 | * @example 14 | small_gicp::IncrementalVoxelMap::Ptr last_voxelmap = ...; 15 | small_gicp::IncrementalVoxelMap::Ptr voxelmap = ...; 16 | 17 | // Create a sequential accessor 18 | const auto accessor = small_gicp::create_sequential_accessor(*voxelmap); 19 | 20 | // Use the voxelmap as a source point cloud 21 | small_gicp::Registration registration; 22 | auto result = registration.align(*last_voxelmap, accessor, *last_voxelmap, Eigen::Isometry3d::Identity()); 23 | */ 24 | template 25 | class SequentialVoxelMapAccessor { 26 | public: 27 | /// @brief Constructor. 28 | /// @param voxelmap Voxelmap 29 | SequentialVoxelMapAccessor(const VoxelMap& voxelmap) : voxelmap(voxelmap), indices(traits::point_indices(voxelmap)) {} 30 | 31 | /// @brief Number of points in the voxelmap. 32 | size_t size() const { return indices.size(); } 33 | 34 | public: 35 | const VoxelMap& voxelmap; ///< Voxelmap 36 | const std::vector indices; ///< Indices of points in the voxelmap 37 | }; 38 | 39 | /// @brief Create a sequential voxelmap accessor. 40 | template 41 | SequentialVoxelMapAccessor create_sequential_accessor(const VoxelMap& voxelmap) { 42 | return SequentialVoxelMapAccessor(voxelmap); 43 | } 44 | 45 | template 46 | struct traits::Traits> { 47 | static size_t size(const SequentialVoxelMapAccessor& accessor) { return accessor.size(); } 48 | 49 | static bool has_points(const SequentialVoxelMapAccessor& accessor) { return traits::has_points(accessor.voxelmap); } 50 | static bool has_normals(const SequentialVoxelMapAccessor& accessor) { return traits::has_normals(accessor.voxelmap); } 51 | static bool has_covs(const SequentialVoxelMapAccessor& accessor) { return traits::has_covs(accessor.voxelmap); } 52 | 53 | static Eigen::Vector4d point(const SequentialVoxelMapAccessor& accessor, size_t i) { return traits::point(accessor.voxelmap, accessor.indices[i]); } 54 | static Eigen::Vector4d normal(const SequentialVoxelMapAccessor& accessor, size_t i) { return traits::normal(accessor.voxelmap, accessor.indices[i]); } 55 | static Eigen::Matrix4d cov(const SequentialVoxelMapAccessor& accessor, size_t i) { return traits::cov(accessor.voxelmap, accessor.indices[i]); } 56 | }; 57 | 58 | } // namespace small_gicp 59 | -------------------------------------------------------------------------------- /include/small_gicp/ann/traits.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | namespace traits { 11 | 12 | template 13 | struct Traits; 14 | 15 | /// @brief Find k-nearest neighbors. 16 | /// @param tree Nearest neighbor search (e.g., KdTree) 17 | /// @param point Query point 18 | /// @param k Number of neighbors 19 | /// @param k_indices [out] Indices of k-nearest neighbors 20 | /// @param k_sq_dists [out] Squared distances to k-nearest neighbors 21 | /// @return Number of found neighbors 22 | template 23 | size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { 24 | return Traits::knn_search(tree, point, k, k_indices, k_sq_dists); 25 | } 26 | 27 | /// @brief Check if T has nearest_neighbor_search method. 28 | template 29 | struct has_nearest_neighbor_search { 30 | template ::nearest_neighbor_search, 0)> 31 | static std::true_type test(U*); 32 | static std::false_type test(...); 33 | 34 | static constexpr bool value = decltype(test((T*)nullptr))::value; 35 | }; 36 | 37 | /// @brief Find the nearest neighbor. If Traits::nearest_neighbor_search is not defined, fallback to knn_search with k=1. 38 | /// @param tree Nearest neighbor search (e.g., KdTree) 39 | /// @param point Query point 40 | /// @param k_index [out] Index of the nearest neighbor 41 | /// @param k_sq_dist [out] Squared distance to the nearest neighbor 42 | /// @return 1 if a neighbor is found else 0 43 | template ::value, bool> = true> 44 | size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) { 45 | return Traits::nearest_neighbor_search(tree, point, k_index, k_sq_dist); 46 | } 47 | 48 | /// @brief Find the nearest neighbor. If Traits::nearest_neighbor_search is not defined, fallback to knn_search with k=1. 49 | /// @param tree Nearest neighbor search (e.g., KdTree) 50 | /// @param point Query point 51 | /// @param k_index [out] Index of the nearest neighbor 52 | /// @param k_sq_dist [out] Squared distance to the nearest neighbor 53 | /// @return 1 if a neighbor is found else 0 54 | template ::value, bool> = true> 55 | size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) { 56 | return Traits::knn_search(tree, point, 1, k_index, k_sq_dist); 57 | } 58 | 59 | } // namespace traits 60 | 61 | } // namespace small_gicp 62 | -------------------------------------------------------------------------------- /include/small_gicp/benchmark/benchmark.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace small_gicp { 17 | 18 | struct Stopwatch { 19 | public: 20 | void start() { t1 = t2 = std::chrono::high_resolution_clock::now(); } 21 | void stop() { t2 = std::chrono::high_resolution_clock::now(); } 22 | void lap() { 23 | t1 = t2; 24 | t2 = std::chrono::high_resolution_clock::now(); 25 | } 26 | 27 | double sec() const { return std::chrono::duration_cast(t2 - t1).count() / 1e9; } 28 | double msec() const { return std::chrono::duration_cast(t2 - t1).count() / 1e6; } 29 | 30 | public: 31 | std::chrono::high_resolution_clock::time_point t1; 32 | std::chrono::high_resolution_clock::time_point t2; 33 | }; 34 | 35 | struct Summarizer { 36 | public: 37 | Summarizer(bool full_log = false) : full_log(full_log), num_data(0), sum(0.0), sq_sum(0.0), last_x(0.0) {} 38 | 39 | void push(double x) { 40 | num_data++; 41 | sum += x; 42 | sq_sum += x * x; 43 | last_x = x; 44 | 45 | full_data.emplace_back(x); 46 | } 47 | 48 | std::pair mean_std() const { 49 | const double mean = sum / num_data; 50 | const double var = (sq_sum - mean * sum) / num_data; 51 | return {mean, std::sqrt(var)}; 52 | } 53 | 54 | double median() const { 55 | if (!full_log || full_data.empty()) { 56 | return std::nan(""); 57 | } 58 | 59 | std::vector sorted(full_data.begin(), full_data.end()); 60 | std::nth_element(sorted.begin(), sorted.end(), sorted.begin() + sorted.size() / 2); 61 | return sorted[sorted.size() / 2]; 62 | } 63 | 64 | double last() const { return last_x; } 65 | 66 | std::string str() const { 67 | if (full_log) { 68 | const auto [mean, std] = mean_std(); 69 | const double med = median(); 70 | return fmt::format("{:.3f} +- {:.3f} (median={:.3f})", mean, std, med); 71 | } 72 | 73 | const auto [mean, std] = mean_std(); 74 | return fmt::format("{:.3f} +- {:.3f} (last={:.3f})", mean, std, last_x); 75 | } 76 | 77 | private: 78 | bool full_log; 79 | std::deque full_data; 80 | 81 | size_t num_data; 82 | double sum; 83 | double sq_sum; 84 | double last_x; 85 | }; 86 | 87 | template 88 | std::string summarize(const Container& container, const Transform& transform) { 89 | Summarizer summarizer; 90 | for (auto itr = std::begin(container); itr != std::end(container); itr++) { 91 | summarizer.push(transform(*itr)); 92 | } 93 | return summarizer.str(); 94 | } 95 | 96 | struct KittiDataset { 97 | public: 98 | explicit KittiDataset(const std::string& dataset_path, size_t max_num_data = 1000000) { 99 | std::vector filenames; 100 | for (auto path : std::filesystem::directory_iterator(dataset_path)) { 101 | if (path.path().extension() != ".bin") { 102 | continue; 103 | } 104 | 105 | filenames.emplace_back(path.path().string()); 106 | } 107 | 108 | std::sort(filenames.begin(), filenames.end()); 109 | if (filenames.size() > max_num_data) { 110 | filenames.resize(max_num_data); 111 | } 112 | 113 | points.resize(filenames.size()); 114 | std::transform(filenames.begin(), filenames.end(), points.begin(), [](const std::string& filename) { return read_points(filename); }); 115 | } 116 | 117 | template 118 | std::vector> convert(bool release = false) { 119 | std::vector> converted(points.size()); 120 | std::transform(points.begin(), points.end(), converted.begin(), [=](auto& raw_points) { 121 | auto points = std::make_shared(); 122 | traits::resize(*points, raw_points.size()); 123 | for (size_t i = 0; i < raw_points.size(); i++) { 124 | traits::set_point(*points, i, raw_points[i].template cast()); 125 | } 126 | 127 | if (release) { 128 | raw_points.clear(); 129 | raw_points.shrink_to_fit(); 130 | } 131 | return points; 132 | }); 133 | 134 | if (release) { 135 | points.clear(); 136 | points.shrink_to_fit(); 137 | } 138 | 139 | return converted; 140 | } 141 | 142 | public: 143 | std::vector> points; 144 | }; 145 | 146 | } // namespace small_gicp 147 | -------------------------------------------------------------------------------- /include/small_gicp/benchmark/benchmark_odom.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #ifdef BUILD_WITH_IRIDESCENCE 14 | #include 15 | #endif 16 | 17 | namespace small_gicp { 18 | 19 | struct OdometryEstimationParams { 20 | public: 21 | bool visualize = false; 22 | int num_threads = 4; 23 | int num_neighbors = 20; 24 | double downsampling_resolution = 0.25; 25 | double voxel_resolution = 1.0; 26 | double max_correspondence_distance = 1.0; 27 | }; 28 | 29 | class OdometryEstimation { 30 | public: 31 | using Ptr = std::shared_ptr; 32 | 33 | OdometryEstimation(const OdometryEstimationParams& params) : params(params) {} 34 | virtual ~OdometryEstimation() = default; 35 | 36 | virtual std::vector estimate(std::vector& points) = 0; 37 | 38 | virtual void report() {} 39 | 40 | protected: 41 | const OdometryEstimationParams params; 42 | }; 43 | 44 | class OnlineOdometryEstimation : public OdometryEstimation { 45 | public: 46 | OnlineOdometryEstimation(const OdometryEstimationParams& params) : OdometryEstimation(params), z_range(-5.0f, 5.0f) {} 47 | ~OnlineOdometryEstimation() {} 48 | 49 | std::vector estimate(std::vector& points) override { 50 | std::vector traj; 51 | 52 | Stopwatch sw; 53 | sw.start(); 54 | for (size_t i = 0; i < points.size(); i++) { 55 | if (i && i % 256 == 0) { 56 | report(); 57 | } 58 | 59 | auto downsampled = voxelgrid_sampling(*points[i], params.downsampling_resolution); 60 | const Eigen::Isometry3d T = estimate(downsampled); 61 | traj.emplace_back(T); 62 | 63 | #ifdef BUILD_WITH_IRIDESCENCE 64 | if (params.visualize) { 65 | auto async_viewer = guik::async_viewer(); 66 | z_range[0] = std::min(z_range[0], T.translation().z() - 5.0f); 67 | z_range[1] = std::max(z_range[1], T.translation().z() + 5.0f); 68 | async_viewer->invoke([=] { guik::viewer()->shader_setting().add("z_range", z_range); }); 69 | async_viewer->update_points("current", downsampled->points, guik::FlatOrange(T).set_point_scale(2.0f)); 70 | async_viewer->update_points(guik::anon(), voxelgrid_sampling(*downsampled, 1.0)->points, guik::Rainbow(T)); 71 | async_viewer->lookat(T.translation().cast()); 72 | } 73 | #endif 74 | 75 | points[i].reset(); 76 | 77 | sw.lap(); 78 | total_times.push(sw.msec()); 79 | } 80 | 81 | return traj; 82 | } 83 | 84 | void update_model_points(const Eigen::Isometry3d& T, const std::vector& points) { 85 | if (!params.visualize) { 86 | return; 87 | } 88 | 89 | #ifdef BUILD_WITH_IRIDESCENCE 90 | if (!async_sub_initialized) { 91 | async_sub_initialized = true; 92 | async_sub = guik::async_viewer()->async_sub_viewer("model"); 93 | } 94 | 95 | async_sub.update_points("model", points, guik::Rainbow()); 96 | async_sub.lookat(T.translation().cast()); 97 | #endif 98 | } 99 | 100 | virtual Eigen::Isometry3d estimate(const PointCloud::Ptr& points) = 0; 101 | 102 | protected: 103 | Eigen::Vector2f z_range; 104 | Summarizer total_times; 105 | 106 | #ifdef BUILD_WITH_IRIDESCENCE 107 | bool async_sub_initialized = false; 108 | guik::AsyncLightViewerContext async_sub; 109 | #endif 110 | }; 111 | 112 | size_t register_odometry(const std::string& name, std::function factory); 113 | 114 | std::vector odometry_names(); 115 | 116 | OdometryEstimation::Ptr create_odometry(const std::string& name, const OdometryEstimationParams& params); 117 | 118 | } // namespace small_gicp 119 | -------------------------------------------------------------------------------- /include/small_gicp/benchmark/read_points.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | /// @brief Read points from file (simple float4 array). 13 | /// @param filename Filename 14 | /// @return Points 15 | static std::vector read_points(const std::string& filename) { 16 | std::ifstream ifs(filename, std::ios::binary | std::ios::ate); 17 | if (!ifs) { 18 | std::cerr << "error: failed to open " << filename << std::endl; 19 | return {}; 20 | } 21 | 22 | std::streamsize points_bytes = ifs.tellg(); 23 | size_t num_points = points_bytes / (sizeof(Eigen::Vector4f)); 24 | 25 | ifs.seekg(0, std::ios::beg); 26 | std::vector points(num_points); 27 | ifs.read(reinterpret_cast(points.data()), sizeof(Eigen::Vector4f) * num_points); 28 | for (auto& pt : points) { 29 | pt(3) = 1.0; 30 | } 31 | 32 | return points; 33 | } 34 | 35 | /// @brief Write points to file (simple float4 array). 36 | /// @param filename Filename 37 | /// @param points Points 38 | static void write_points(const std::string& filename, const std::vector& points) { 39 | std::ofstream ofs(filename, std::ios::binary); 40 | if (!ofs) { 41 | std::cerr << "error: failed to open " << filename << std::endl; 42 | return; 43 | } 44 | 45 | ofs.write(reinterpret_cast(points.data()), sizeof(Eigen::Vector4f) * points.size()); 46 | } 47 | 48 | /// @brief Read point cloud from a PLY file. 49 | /// @note This function can only handle simple PLY files with float properties (Property names must be "x", "y", "z"). Do not use this for general PLY IO. 50 | /// @param filename Filename 51 | /// @return Points 52 | static std::vector read_ply(const std::string& filename) { 53 | std::ifstream ifs(filename, std::ios::binary); 54 | if (!ifs) { 55 | std::cerr << "error: failed to open " << filename << std::endl; 56 | return {}; 57 | } 58 | 59 | std::vector properties; 60 | std::vector points; 61 | 62 | std::string line; 63 | while (!ifs.eof() && std::getline(ifs, line) && !line.empty()) { 64 | if (line == "end_header") { 65 | break; 66 | } 67 | 68 | if (line.find("element") == 0) { 69 | std::stringstream sst(line); 70 | std::string token, vertex, num_points; 71 | sst >> token >> vertex >> num_points; 72 | if (token != "element" || vertex != "vertex") { 73 | std::cerr << "error: invalid ply format (line=" << line << ")" << std::endl; 74 | return {}; 75 | } 76 | 77 | points.resize(std::stol(num_points)); 78 | } else if (line.find("property") == 0) { 79 | std::stringstream sst(line); 80 | std::string token, type, name; 81 | sst >> token >> type >> name; 82 | if (type != "float") { 83 | std::cerr << "error: only float properties are supported!! (line=" << line << ")" << std::endl; 84 | return {}; 85 | } 86 | 87 | properties.emplace_back(name); 88 | } 89 | } 90 | 91 | if ( 92 | line.size() < 3 || properties[0].size() != 1 || properties[1].size() != 1 || properties[2].size() != 1 || std::tolower(properties[0][0]) != 'x' || 93 | std::tolower(properties[1][0]) != 'y' || std::tolower(properties[2][0]) != 'z') { 94 | std::cerr << "warning: invalid properties!!" << std::endl; 95 | for (const auto& prop : properties) { 96 | std::cerr << " - " << prop << std::endl; 97 | } 98 | } 99 | 100 | std::vector buffer(properties.size() * points.size()); 101 | ifs.read(reinterpret_cast(buffer.data()), sizeof(Eigen::Vector4f) * points.size()); 102 | 103 | for (size_t i = 0; i < points.size(); i++) { 104 | const int stride = properties.size(); 105 | points[i] = Eigen::Vector4f(buffer[i * stride + 0], buffer[i * stride + 1], buffer[i * stride + 2], 1.0); 106 | } 107 | 108 | return points; 109 | } 110 | 111 | } // namespace small_gicp 112 | -------------------------------------------------------------------------------- /include/small_gicp/factors/general_factor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Null factor that gives no constraints. 11 | struct NullFactor { 12 | NullFactor() = default; 13 | 14 | /// @brief Update linearized system consisting of linearized per-point factors. 15 | /// @param target Target point cloud 16 | /// @param source Source point cloud 17 | /// @param target_tree Nearest neighbor search for the target point cloud 18 | /// @param T Linearization point 19 | /// @param H [in/out] Linearized information matrix. 20 | /// @param b [in/out] Linearized information vector. 21 | /// @param e [in/out] Error at the linearization point. 22 | template 23 | void update_linearized_system( 24 | const TargetPointCloud& target, 25 | const SourcePointCloud& source, 26 | const TargetTree& target_tree, 27 | const Eigen::Isometry3d& T, 28 | Eigen::Matrix* H, 29 | Eigen::Matrix* b, 30 | double* e) const {} 31 | 32 | /// @brief Update error consisting of per-point factors. 33 | /// @param target Target point cloud 34 | /// @param source Source point cloud 35 | /// @param T Evaluation point 36 | /// @param e [in/out] Error at the evaluation point. 37 | template 38 | void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {} 39 | }; 40 | 41 | /// @brief Factor to restrict the degrees of freedom of optimization (e.g., fixing roll, pitch rotation). 42 | /// @note This factor only enables *soft* constraints. The source point cloud can move to the restricted directions slightly). 43 | struct RestrictDoFFactor { 44 | /// @brief Constructor. 45 | RestrictDoFFactor() { 46 | lambda = 1e9; 47 | mask.setOnes(); 48 | } 49 | 50 | /// @brief Set rotation mask. (1.0 = active, 0.0 = inactive) 51 | void set_rotation_mask(const Eigen::Array3d& rot_mask) { mask.head<3>() = rot_mask; } 52 | 53 | /// @brief Set translation mask. (1.0 = active, 0.0 = inactive) 54 | void set_translation_mask(const Eigen::Array3d& trans_mask) { mask.tail<3>() = trans_mask; } 55 | 56 | /// @brief Update linearized system consisting of linearized per-point factors. 57 | template 58 | void update_linearized_system( 59 | const TargetPointCloud& target, 60 | const SourcePointCloud& source, 61 | const TargetTree& target_tree, 62 | const Eigen::Isometry3d& T, 63 | Eigen::Matrix* H, 64 | Eigen::Matrix* b, 65 | double* e) const { 66 | *H += lambda * (mask - 1.0).abs().matrix().asDiagonal(); 67 | } 68 | 69 | /// @brief Update error consisting of per-point factors. 70 | template 71 | void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {} 72 | 73 | double lambda; ///< Regularization parameter (Increasing this makes the constraint stronger) 74 | Eigen::Array mask; ///< Mask for restricting DoF (rx, ry, rz, tx, ty, tz) 75 | }; 76 | 77 | } // namespace small_gicp 78 | -------------------------------------------------------------------------------- /include/small_gicp/factors/gicp_factor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace small_gicp { 12 | 13 | /// @brief GICP (distribution-to-distribution) per-point error factor. 14 | struct GICPFactor { 15 | struct Setting {}; 16 | 17 | /// @brief Constructor 18 | GICPFactor(const Setting& setting = Setting()) 19 | : target_index(std::numeric_limits::max()), 20 | source_index(std::numeric_limits::max()), 21 | mahalanobis(Eigen::Matrix4d::Zero()) {} 22 | 23 | /// @brief Linearize the factor 24 | /// @param target Target point cloud 25 | /// @param source Source point cloud 26 | /// @param target_tree Nearest neighbor search for the target point cloud 27 | /// @param T Linearization point 28 | /// @param source_index Source point index 29 | /// @param rejector Correspondence rejector 30 | /// @param H Linearized information matrix 31 | /// @param b Linearized information vector 32 | /// @param e Error at the linearization point 33 | /// @return True if the point is inlier 34 | template 35 | bool linearize( 36 | const TargetPointCloud& target, 37 | const SourcePointCloud& source, 38 | const TargetTree& target_tree, 39 | const Eigen::Isometry3d& T, 40 | size_t source_index, 41 | const CorrespondenceRejector& rejector, 42 | Eigen::Matrix* H, 43 | Eigen::Matrix* b, 44 | double* e) { 45 | // 46 | this->source_index = source_index; 47 | this->target_index = std::numeric_limits::max(); 48 | 49 | const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); 50 | 51 | size_t k_index; 52 | double k_sq_dist; 53 | 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)) { 54 | return false; 55 | } 56 | 57 | target_index = k_index; 58 | 59 | const Eigen::Matrix4d RCR = traits::cov(target, target_index) + T.matrix() * traits::cov(source, source_index) * T.matrix().transpose(); 60 | mahalanobis.block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse(); 61 | 62 | const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; 63 | 64 | Eigen::Matrix J = Eigen::Matrix::Zero(); 65 | J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>()); 66 | J.block<3, 3>(0, 3) = -T.linear(); 67 | 68 | *H = J.transpose() * mahalanobis * J; 69 | *b = J.transpose() * mahalanobis * residual; 70 | *e = 0.5 * residual.transpose() * mahalanobis * residual; 71 | 72 | return true; 73 | } 74 | 75 | /// @brief Evaluate error 76 | /// @param target Target point cloud 77 | /// @param source Source point cloud 78 | /// @param T Evaluation point 79 | /// @return Error 80 | template 81 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { 82 | if (target_index == std::numeric_limits::max()) { 83 | return 0.0; 84 | } 85 | 86 | const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); 87 | const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; 88 | return 0.5 * residual.transpose() * mahalanobis * residual; 89 | } 90 | 91 | /// @brief Returns true if this factor is not rejected as an outlier 92 | bool inlier() const { return target_index != std::numeric_limits::max(); } 93 | 94 | size_t target_index; ///< Target point index 95 | size_t source_index; ///< Source point index 96 | Eigen::Matrix4d mahalanobis; ///< Fused precision matrix 97 | }; 98 | } // namespace small_gicp 99 | -------------------------------------------------------------------------------- /include/small_gicp/factors/icp_factor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace small_gicp { 12 | 13 | /// @brief Point-to-point per-point error factor. 14 | struct ICPFactor { 15 | struct Setting {}; 16 | 17 | ICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} 18 | 19 | template 20 | bool linearize( 21 | const TargetPointCloud& target, 22 | const SourcePointCloud& source, 23 | const TargetTree& target_tree, 24 | const Eigen::Isometry3d& T, 25 | size_t source_index, 26 | const CorrespondenceRejector& rejector, 27 | Eigen::Matrix* H, 28 | Eigen::Matrix* b, 29 | double* e) { 30 | // 31 | this->source_index = source_index; 32 | this->target_index = std::numeric_limits::max(); 33 | 34 | const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); 35 | 36 | size_t k_index; 37 | double k_sq_dist; 38 | 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)) { 39 | return false; 40 | } 41 | 42 | target_index = k_index; 43 | const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; 44 | 45 | Eigen::Matrix J = Eigen::Matrix::Zero(); 46 | J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>()); 47 | J.block<3, 3>(0, 3) = -T.linear(); 48 | 49 | *H = J.transpose() * J; 50 | *b = J.transpose() * residual; 51 | *e = 0.5 * residual.squaredNorm(); 52 | 53 | return true; 54 | } 55 | 56 | template 57 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { 58 | if (target_index == std::numeric_limits::max()) { 59 | return 0.0; 60 | } 61 | 62 | const Eigen::Vector4d residual = traits::point(target, target_index) - T * traits::point(source, source_index); 63 | return 0.5 * residual.squaredNorm(); 64 | } 65 | 66 | bool inlier() const { return target_index != std::numeric_limits::max(); } 67 | 68 | size_t target_index; 69 | size_t source_index; 70 | }; 71 | } // namespace small_gicp 72 | -------------------------------------------------------------------------------- /include/small_gicp/factors/plane_icp_factor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace small_gicp { 12 | 13 | /// @brief Point-to-plane per-point error factor. 14 | struct PointToPlaneICPFactor { 15 | struct Setting {}; 16 | 17 | PointToPlaneICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} 18 | 19 | template 20 | bool linearize( 21 | const TargetPointCloud& target, 22 | const SourcePointCloud& source, 23 | const TargetTree& target_tree, 24 | const Eigen::Isometry3d& T, 25 | size_t source_index, 26 | const CorrespondenceRejector& rejector, 27 | Eigen::Matrix* H, 28 | Eigen::Matrix* b, 29 | double* e) { 30 | // 31 | this->source_index = source_index; 32 | this->target_index = std::numeric_limits::max(); 33 | 34 | const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); 35 | 36 | size_t k_index; 37 | double k_sq_dist; 38 | 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)) { 39 | return false; 40 | } 41 | 42 | target_index = k_index; 43 | const auto& target_normal = traits::normal(target, target_index); 44 | 45 | const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; 46 | const Eigen::Vector4d err = target_normal.array() * residual.array(); 47 | 48 | Eigen::Matrix J = Eigen::Matrix::Zero(); 49 | J.block<3, 3>(0, 0) = target_normal.template head<3>().asDiagonal() * T.linear() * skew(traits::point(source, source_index).template head<3>()); 50 | J.block<3, 3>(0, 3) = target_normal.template head<3>().asDiagonal() * (-T.linear()); 51 | 52 | *H = J.transpose() * J; 53 | *b = J.transpose() * err; 54 | *e = 0.5 * err.squaredNorm(); 55 | 56 | return true; 57 | } 58 | 59 | template 60 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { 61 | if (target_index == std::numeric_limits::max()) { 62 | return 0.0; 63 | } 64 | 65 | const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); 66 | const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; 67 | const Eigen::Vector4d error = traits::normal(target, target_index).array() * residual.array(); 68 | return 0.5 * error.squaredNorm(); 69 | } 70 | 71 | bool inlier() const { return target_index != std::numeric_limits::max(); } 72 | 73 | size_t target_index; 74 | size_t source_index; 75 | }; 76 | } // namespace small_gicp 77 | -------------------------------------------------------------------------------- /include/small_gicp/factors/robust_kernel.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Huber robust kernel 11 | struct Huber { 12 | public: 13 | /// @brief Huber robust kernel setting 14 | struct Setting { 15 | double c = 1.0; ///< Kernel width 16 | }; 17 | 18 | /// @brief Constructor 19 | Huber(const Setting& setting) : c(setting.c) {} 20 | 21 | /// @brief Compute the weight for an error 22 | /// @param e Error 23 | /// @return Weight 24 | double weight(double e) const { 25 | const double e_abs = std::abs(e); 26 | return e_abs < c ? 1.0 : c / e_abs; 27 | } 28 | 29 | public: 30 | const double c; ///< Kernel width 31 | }; 32 | 33 | /// @brief Cauchy robust kernel 34 | struct Cauchy { 35 | public: 36 | /// @brief Huber robust kernel setting 37 | struct Setting { 38 | double c = 1.0; ///< Kernel width 39 | }; 40 | 41 | /// @brief Constructor 42 | Cauchy(const Setting& setting) : c(setting.c) {} 43 | 44 | /// @brief Compute the weight for an error 45 | /// @param e Error 46 | /// @return Weight 47 | double weight(double e) const { return c / (c + e * e); } 48 | 49 | public: 50 | const double c; ///< Kernel width 51 | }; 52 | 53 | /// @brief Robustify a factor with a robust kernel 54 | /// @tparam Kernel Robust kernel 55 | /// @tparam Factor Factor 56 | template 57 | struct RobustFactor { 58 | public: 59 | /// @brief Robust factor setting 60 | struct Setting { 61 | typename Kernel::Setting robust_kernel; ///< Robust kernel setting 62 | typename Factor::Setting factor; ///< Factor setting 63 | }; 64 | 65 | /// @brief Constructor 66 | RobustFactor(const Setting& setting = Setting()) : robust_kernel(setting.robust_kernel), factor(setting.factor) {} 67 | 68 | /// @brief Linearize the factor 69 | template 70 | bool linearize( 71 | const TargetPointCloud& target, 72 | const SourcePointCloud& source, 73 | const TargetTree& target_tree, 74 | const Eigen::Isometry3d& T, 75 | size_t source_index, 76 | const CorrespondenceRejector& rejector, 77 | Eigen::Matrix* H, 78 | Eigen::Matrix* b, 79 | double* e) { 80 | if (!factor.linearize(target, source, target_tree, T, source_index, rejector, H, b, e)) { 81 | return false; 82 | } 83 | 84 | // Robustify the linearized factor 85 | const double w = robust_kernel.weight(std::sqrt(*e)); 86 | *H *= w; 87 | *b *= w; 88 | *e *= w; 89 | 90 | return true; 91 | } 92 | 93 | /// @brief Evaluate error 94 | template 95 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { 96 | const double e = factor.error(target, source, T); 97 | return robust_kernel.weight(std::sqrt(e)) * e; 98 | } 99 | 100 | /// @brief Check if the factor is inlier 101 | bool inlier() const { return factor.inlier(); } 102 | 103 | public: 104 | Kernel robust_kernel; ///< Robust kernel 105 | Factor factor; ///< Factor 106 | }; 107 | 108 | } // namespace small_gicp 109 | -------------------------------------------------------------------------------- /include/small_gicp/pcl/pcl_point.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace pcl { 8 | 9 | using Matrix4fMap = Eigen::Map; 10 | using Matrix4fMapConst = const Eigen::Map; 11 | 12 | /// @brief Point with covariance for PCL 13 | struct PointCovariance { 14 | PCL_ADD_POINT4D; ///< Point coordinates 15 | Eigen::Matrix4f cov; ///< 4x4 covariance matrix 16 | 17 | /// @brief Get covariance matrix as Matrix4fMap 18 | Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } 19 | 20 | /// @brief Get covariance matrix as Matrix4fMapConst 21 | Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } 22 | 23 | PCL_MAKE_ALIGNED_OPERATOR_NEW 24 | }; 25 | 26 | /// @brief Point with normal and covariance for PCL 27 | struct PointNormalCovariance { 28 | PCL_ADD_POINT4D; ///< Point coordinates 29 | PCL_ADD_NORMAL4D ///< Point normal 30 | Eigen::Matrix4f cov; ///< 4x4 covariance matrix 31 | 32 | /// @brief Get covariance matrix as Matrix4fMap 33 | Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } 34 | 35 | /// @brief Get covariance matrix as Matrix4fMapConst 36 | Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } 37 | 38 | PCL_MAKE_ALIGNED_OPERATOR_NEW 39 | }; 40 | 41 | } // namespace pcl 42 | -------------------------------------------------------------------------------- /include/small_gicp/pcl/pcl_point_traits.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | namespace traits { 13 | 14 | template 15 | struct Traits> { 16 | static_assert(std::is_same_v, std::shared_ptr>, "Old PCL version detected. Please update PCL to 1.11 or later."); 17 | 18 | using Points = pcl::PointCloud; 19 | 20 | static size_t size(const Points& points) { return points.size(); } 21 | static void resize(Points& points, size_t n) { points.resize(n); } 22 | 23 | static bool has_points(const Points& points) { return !points.empty(); } 24 | static bool has_normals(const Points& points) { return !points.empty(); } 25 | static bool has_covs(const Points& points) { return !points.empty(); } 26 | 27 | static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getVector4fMap() = pt.template cast(); } 28 | static void set_normal(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getNormalVector4fMap() = pt.template cast(); } 29 | static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.at(i).getCovariance4fMap() = cov.template cast(); } 30 | 31 | static Eigen::Vector4d point(const Points& points, size_t i) { return points.at(i).getVector4fMap().template cast(); } 32 | static Eigen::Vector4d normal(const Points& points, size_t i) { return points.at(i).getNormalVector4fMap().template cast(); } 33 | static Eigen::Matrix4d cov(const Points& points, size_t i) { return points.at(i).getCovariance4fMap().template cast(); } 34 | }; 35 | 36 | } // namespace traits 37 | 38 | } // namespace small_gicp 39 | -------------------------------------------------------------------------------- /include/small_gicp/pcl/pcl_proxy.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Proxy class to access PCL point cloud with external covariance matrices. 11 | template 12 | struct PointCloudProxy { 13 | PointCloudProxy(const pcl::PointCloud& cloud, std::vector& covs) : cloud(cloud), covs(covs) {} 14 | 15 | const pcl::PointCloud& cloud; ///< Point cloud 16 | std::vector& covs; ///< Covariance matrices 17 | }; 18 | 19 | namespace traits { 20 | template 21 | struct Traits> { 22 | using Points = PointCloudProxy; 23 | 24 | static size_t size(const Points& points) { return points.cloud.size(); } 25 | 26 | static bool has_points(const Points& points) { return !points.cloud.points.empty(); } 27 | static bool has_covs(const Points& points) { return !points.covs.empty(); } 28 | 29 | static const Eigen::Vector4d point(const Points& points, size_t i) { return points.cloud.at(i).getVector4fMap().template cast(); } 30 | static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.covs[i]; } 31 | 32 | static void resize(Points& points, size_t n) { points.covs.resize(n); } 33 | static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.covs[i] = cov; } 34 | }; 35 | 36 | } // namespace traits 37 | 38 | } // namespace small_gicp 39 | -------------------------------------------------------------------------------- /include/small_gicp/points/eigen.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace small_gicp { 10 | namespace traits { 11 | 12 | template <> 13 | struct Traits { 14 | static size_t size(const Eigen::MatrixXd& points) { return points.rows(); } 15 | static bool has_points(const Eigen::MatrixXd& points) { return points.rows(); } 16 | static Eigen::Vector4d point(const Eigen::MatrixXd& points, size_t i) { return Eigen::Vector4d(points(i, 0), points(i, 1), points(i, 2), 1.0); } 17 | }; 18 | 19 | template 20 | struct Traits>> { 21 | static_assert(D == 3 || D == 4, "Only 3D and 4D (homogeneous) points are supported"); 22 | static_assert(std::is_floating_point::value, "Only floating point types are supported"); 23 | 24 | using Point = Eigen::Matrix; 25 | static size_t size(const std::vector& points) { return points.size(); } 26 | static bool has_points(const std::vector& points) { return points.size(); } 27 | static Eigen::Vector4d point(const std::vector& points, size_t i) { 28 | if constexpr (std::is_same_v) { 29 | if constexpr (D == 3) { 30 | return points[i].homogeneous(); 31 | } else { 32 | return points[i]; 33 | } 34 | } else { 35 | if constexpr (D == 3) { 36 | return points[i].homogeneous().template cast(); 37 | } else { 38 | return points[i].template cast(); 39 | } 40 | } 41 | } 42 | }; 43 | 44 | } // namespace traits 45 | } // namespace small_gicp -------------------------------------------------------------------------------- /include/small_gicp/points/point_cloud.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | /** 13 | * @brief Point cloud 14 | */ 15 | struct PointCloud { 16 | public: 17 | using Ptr = std::shared_ptr; 18 | using ConstPtr = std::shared_ptr; 19 | 20 | /// @brief Constructor 21 | PointCloud() {} 22 | 23 | /// @brief Constructor 24 | /// @param points Points to initialize the point cloud 25 | template 26 | explicit PointCloud(const std::vector, Allocator>& points) { 27 | this->resize(points.size()); 28 | for (size_t i = 0; i < points.size(); i++) { 29 | this->point(i) << points[i].template cast().template head<3>(), 1.0; 30 | } 31 | } 32 | 33 | /// @brief Destructor 34 | ~PointCloud() {} 35 | 36 | /// @brief Number of points. 37 | size_t size() const { return points.size(); } 38 | 39 | /// @brief Check if the point cloud is empty. 40 | bool empty() const { return points.empty(); } 41 | 42 | /// @brief Resize point/normal/cov buffers. 43 | /// @param n Number of points 44 | void resize(size_t n) { 45 | points.resize(n); 46 | normals.resize(n); 47 | covs.resize(n); 48 | } 49 | 50 | /// @brief Get i-th point. 51 | Eigen::Vector4d& point(size_t i) { return points[i]; } 52 | 53 | /// @brief Get i-th normal. 54 | Eigen::Vector4d& normal(size_t i) { return normals[i]; } 55 | 56 | /// @brief Get i-th covariance. 57 | Eigen::Matrix4d& cov(size_t i) { return covs[i]; } 58 | 59 | /// @brief Get i-th point (const). 60 | const Eigen::Vector4d& point(size_t i) const { return points[i]; } 61 | 62 | /// @brief Get i-th normal (const). 63 | const Eigen::Vector4d& normal(size_t i) const { return normals[i]; } 64 | 65 | /// @brief Get i-th covariance (const). 66 | const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; } 67 | 68 | public: 69 | std::vector points; ///< Point coordinates (x, y, z, 1) 70 | std::vector normals; ///< Point normals (nx, ny, nz, 0) 71 | std::vector covs; ///< Point covariances (3x3 matrix) + zero padding 72 | }; 73 | 74 | namespace traits { 75 | 76 | template <> 77 | struct Traits { 78 | using Points = PointCloud; 79 | 80 | static size_t size(const Points& points) { return points.size(); } 81 | 82 | static bool has_points(const Points& points) { return !points.points.empty(); } 83 | static bool has_normals(const Points& points) { return !points.normals.empty(); } 84 | static bool has_covs(const Points& points) { return !points.covs.empty(); } 85 | 86 | static const Eigen::Vector4d& point(const Points& points, size_t i) { return points.point(i); } 87 | static const Eigen::Vector4d& normal(const Points& points, size_t i) { return points.normal(i); } 88 | static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.cov(i); } 89 | 90 | static void resize(Points& points, size_t n) { points.resize(n); } 91 | static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.point(i) = pt; } 92 | static void set_normal(Points& points, size_t i, const Eigen::Vector4d& n) { points.normal(i) = n; } 93 | static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.cov(i) = cov; } 94 | }; 95 | 96 | } // namespace traits 97 | 98 | } // namespace small_gicp 99 | -------------------------------------------------------------------------------- /include/small_gicp/points/traits.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace small_gicp { 8 | 9 | namespace traits { 10 | 11 | template 12 | struct Traits; 13 | 14 | /// @brief Get the number of points. 15 | template 16 | size_t size(const T& points) { 17 | return Traits::size(points); 18 | } 19 | 20 | /// @brief Check if the point cloud has points. 21 | template 22 | bool has_points(const T& points) { 23 | return Traits::has_points(points); 24 | } 25 | 26 | /// @brief Check if the point cloud has normals. 27 | template 28 | bool has_normals(const T& points) { 29 | return Traits::has_normals(points); 30 | } 31 | 32 | /// @brief Check if the point cloud has covariances. 33 | template 34 | bool has_covs(const T& points) { 35 | return Traits::has_covs(points); 36 | } 37 | 38 | /// @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). 39 | template 40 | auto point(const T& points, size_t i) { 41 | return Traits::point(points, i); 42 | } 43 | 44 | /// @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). 45 | template 46 | auto normal(const T& points, size_t i) { 47 | return Traits::normal(points, i); 48 | } 49 | 50 | /// @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. 51 | template 52 | auto cov(const T& points, size_t i) { 53 | return Traits::cov(points, i); 54 | } 55 | 56 | /// @brief Resize the point cloud (this function should resize all attributes) 57 | template 58 | void resize(T& points, size_t n) { 59 | Traits::resize(points, n); 60 | } 61 | 62 | /// @brief Set i-th point. (x, y, z, 1) 63 | template 64 | void set_point(T& points, size_t i, const Eigen::Vector4d& pt) { 65 | Traits::set_point(points, i, pt); 66 | } 67 | 68 | /// @brief Set i-th normal. (nx, nz, nz, 0) 69 | template 70 | void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) { 71 | Traits::set_normal(points, i, pt); 72 | } 73 | 74 | /// @brief Set i-th covariance. Only the top-left 3x3 matrix should be filled. 75 | template 76 | void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) { 77 | Traits::set_cov(points, i, cov); 78 | } 79 | 80 | } // namespace traits 81 | } // namespace small_gicp 82 | -------------------------------------------------------------------------------- /include/small_gicp/registration/reduction.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Single-thread reduction 11 | struct SerialReduction { 12 | /// @brief Sum up linearized systems 13 | /// @param target Target point cloud 14 | /// @param source Source point cloud 15 | /// @param target_tree Nearest neighbor search for the target point cloud 16 | /// @param rejector Correspondence rejector 17 | /// @param T Linearization point 18 | /// @param factors Factors to be linearized 19 | /// @return Sum of the linearized systems (information matrix, information vector, and error) 20 | template 21 | std::tuple, Eigen::Matrix, double> linearize( 22 | const TargetPointCloud& target, 23 | const SourcePointCloud& source, 24 | const TargetTree& target_tree, 25 | const CorrespondenceRejector& rejector, 26 | const Eigen::Isometry3d& T, 27 | std::vector& factors) const { 28 | Eigen::Matrix sum_H = Eigen::Matrix::Zero(); 29 | Eigen::Matrix sum_b = Eigen::Matrix::Zero(); 30 | double sum_e = 0.0; 31 | 32 | for (size_t i = 0; i < factors.size(); i++) { 33 | Eigen::Matrix H; 34 | Eigen::Matrix b; 35 | double e; 36 | 37 | if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) { 38 | continue; 39 | } 40 | 41 | sum_H += H; 42 | sum_b += b; 43 | sum_e += e; 44 | } 45 | 46 | return {sum_H, sum_b, sum_e}; 47 | } 48 | 49 | /// @brief Sum up evaluated errors 50 | /// @param target Target point cloud 51 | /// @param source Source point cloud 52 | /// @param T Evaluation point 53 | /// @param factors Factors to be evaluated 54 | /// @return Sum of the evaluated errors 55 | template 56 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) const { 57 | double sum_e = 0.0; 58 | for (size_t i = 0; i < factors.size(); i++) { 59 | sum_e += factors[i].error(target, source, T); 60 | } 61 | return sum_e; 62 | } 63 | }; 64 | 65 | } // namespace small_gicp 66 | -------------------------------------------------------------------------------- /include/small_gicp/registration/reduction_omp.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace small_gicp { 8 | 9 | #ifndef _OPENMP 10 | #warning "OpenMP is not available. Parallel reduction will be disabled." 11 | inline int omp_get_thread_num() { 12 | return 0; 13 | } 14 | #endif 15 | 16 | /// @brief Parallel reduction with OpenMP backend 17 | struct ParallelReductionOMP { 18 | ParallelReductionOMP() : num_threads(4) {} 19 | 20 | template 21 | std::tuple, Eigen::Matrix, double> linearize( 22 | const TargetPointCloud& target, 23 | const SourcePointCloud& source, 24 | const TargetTree& target_tree, 25 | const CorrespondenceRejector& rejector, 26 | const Eigen::Isometry3d& T, 27 | std::vector& factors) const { 28 | std::vector> Hs(num_threads, Eigen::Matrix::Zero()); 29 | std::vector> bs(num_threads, Eigen::Matrix::Zero()); 30 | std::vector es(num_threads, 0.0); 31 | 32 | #pragma omp parallel for num_threads(num_threads) schedule(guided, 8) 33 | for (std::int64_t i = 0; i < factors.size(); i++) { 34 | Eigen::Matrix H; 35 | Eigen::Matrix b; 36 | double e; 37 | 38 | if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) { 39 | continue; 40 | } 41 | 42 | const int thread_id = omp_get_thread_num(); 43 | Hs[thread_id] += H; 44 | bs[thread_id] += b; 45 | es[thread_id] += e; 46 | } 47 | 48 | for (int i = 1; i < num_threads; i++) { 49 | Hs[0] += Hs[i]; 50 | bs[0] += bs[i]; 51 | es[0] += es[i]; 52 | } 53 | 54 | return {Hs[0], bs[0], es[0]}; 55 | } 56 | 57 | template 58 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) const { 59 | double sum_e = 0.0; 60 | 61 | #pragma omp parallel for num_threads(num_threads) schedule(guided, 8) reduction(+ : sum_e) 62 | for (std::int64_t i = 0; i < factors.size(); i++) { 63 | sum_e += factors[i].error(target, source, T); 64 | } 65 | return sum_e; 66 | } 67 | 68 | int num_threads; ///< Number of threads 69 | }; 70 | 71 | } // namespace small_gicp 72 | -------------------------------------------------------------------------------- /include/small_gicp/registration/reduction_tbb.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Summation for linearized systems 11 | template 12 | struct LinearizeSum { 13 | LinearizeSum( 14 | const TargetPointCloud& target, 15 | const SourcePointCloud& source, 16 | const TargetTree& target_tree, 17 | const CorrespondenceRejector& rejector, 18 | const Eigen::Isometry3d& T, 19 | std::vector& factors) 20 | : target(target), 21 | source(source), 22 | target_tree(target_tree), 23 | rejector(rejector), 24 | T(T), 25 | factors(factors), 26 | H(Eigen::Matrix::Zero()), 27 | b(Eigen::Matrix::Zero()), 28 | e(0.0) {} 29 | 30 | LinearizeSum(LinearizeSum& x, tbb::split) 31 | : target(x.target), 32 | source(x.source), 33 | target_tree(x.target_tree), 34 | rejector(x.rejector), 35 | T(x.T), 36 | factors(x.factors), 37 | H(Eigen::Matrix::Zero()), 38 | b(Eigen::Matrix::Zero()), 39 | e(0.0) {} 40 | 41 | void operator()(const tbb::blocked_range& r) { 42 | Eigen::Matrix Ht = H; 43 | Eigen::Matrix bt = b; 44 | double et = e; 45 | 46 | for (size_t i = r.begin(); i != r.end(); i++) { 47 | Eigen::Matrix Hi; 48 | Eigen::Matrix bi; 49 | double ei; 50 | 51 | if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &Hi, &bi, &ei)) { 52 | continue; 53 | } 54 | 55 | Ht += Hi; 56 | bt += bi; 57 | et += ei; 58 | } 59 | 60 | H = Ht; 61 | b = bt; 62 | e = et; 63 | } 64 | 65 | void join(const LinearizeSum& y) { 66 | H += y.H; 67 | b += y.b; 68 | e += y.e; 69 | } 70 | 71 | const TargetPointCloud& target; 72 | const SourcePointCloud& source; 73 | const TargetTree& target_tree; 74 | const CorrespondenceRejector& rejector; 75 | const Eigen::Isometry3d& T; 76 | std::vector& factors; 77 | 78 | Eigen::Matrix H; 79 | Eigen::Matrix b; 80 | double e; 81 | }; 82 | 83 | /// @brief Summation for evaluated errors 84 | template 85 | struct ErrorSum { 86 | ErrorSum(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) 87 | : target(target), 88 | source(source), 89 | T(T), 90 | factors(factors), 91 | e(0.0) {} 92 | 93 | ErrorSum(ErrorSum& x, tbb::split) : target(x.target), source(x.source), T(x.T), factors(x.factors), e(0.0) {} 94 | 95 | void operator()(const tbb::blocked_range& r) { 96 | double et = e; 97 | for (size_t i = r.begin(); i != r.end(); i++) { 98 | et += factors[i].error(target, source, T); 99 | } 100 | e = et; 101 | } 102 | 103 | void join(const ErrorSum& y) { e += y.e; } 104 | 105 | const TargetPointCloud& target; 106 | const SourcePointCloud& source; 107 | const Eigen::Isometry3d& T; 108 | std::vector& factors; 109 | 110 | double e; 111 | }; 112 | 113 | /// @brief Parallel reduction with TBB backend 114 | struct ParallelReductionTBB { 115 | ParallelReductionTBB() {} 116 | 117 | template 118 | std::tuple, Eigen::Matrix, double> linearize( 119 | const TargetPointCloud& target, 120 | const SourcePointCloud& source, 121 | const TargetTree& target_tree, 122 | const CorrespondenceRejector& rejector, 123 | const Eigen::Isometry3d& T, 124 | std::vector& factors) const { 125 | // 126 | LinearizeSum sum(target, source, target_tree, rejector, T, factors); 127 | 128 | tbb::parallel_reduce(tbb::blocked_range(0, factors.size(), 8), sum); 129 | 130 | return {sum.H, sum.b, sum.e}; 131 | } 132 | 133 | template 134 | double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) const { 135 | ErrorSum sum(target, source, T, factors); 136 | tbb::parallel_reduce(tbb::blocked_range(0, factors.size(), 16), sum); 137 | return sum.e; 138 | } 139 | }; 140 | 141 | } // namespace small_gicp 142 | -------------------------------------------------------------------------------- /include/small_gicp/registration/registration.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace small_gicp { 15 | 16 | /// @brief Point cloud registration. 17 | template < 18 | typename PointFactor, 19 | typename Reduction, 20 | typename GeneralFactor = NullFactor, 21 | typename CorrespondenceRejector = DistanceRejector, 22 | typename Optimizer = LevenbergMarquardtOptimizer> 23 | struct Registration { 24 | public: 25 | /// @brief Align point clouds. 26 | /// @param target Target point cloud 27 | /// @param source Source point cloud 28 | /// @param target_tree Nearest neighbor search for the target point cloud 29 | /// @param init_T Initial guess 30 | /// @return Registration result 31 | template 32 | RegistrationResult 33 | align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) const { 34 | if (traits::size(target) <= 10) { 35 | std::cerr << "warning: target point cloud is too small. |target|=" << traits::size(target) << std::endl; 36 | } 37 | if (traits::size(source) <= 10) { 38 | std::cerr << "warning: source point cloud is too small. |source|=" << traits::size(source) << std::endl; 39 | } 40 | 41 | std::vector factors(traits::size(source), PointFactor(point_factor)); 42 | return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor); 43 | } 44 | 45 | public: 46 | using PointFactorSetting = typename PointFactor::Setting; 47 | 48 | TerminationCriteria criteria; ///< Termination criteria 49 | CorrespondenceRejector rejector; ///< Correspondence rejector 50 | PointFactorSetting point_factor; ///< Factor setting 51 | GeneralFactor general_factor; ///< General factor 52 | Reduction reduction; ///< Reduction 53 | Optimizer optimizer; ///< Optimizer 54 | }; 55 | 56 | } // namespace small_gicp 57 | -------------------------------------------------------------------------------- /include/small_gicp/registration/registration_helper.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | /// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation). 13 | /// @note When num_threads >= 2, this function has minor run-by-run non-determinism due to the parallel downsampling. 14 | /// @see small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp 15 | /// @param points Input points 16 | /// @param downsampling_resolution Downsample resolution 17 | /// @param num_neighbors Number of neighbors for normal/covariance estimation 18 | /// @param num_threads Number of threads 19 | std::pair>> 20 | preprocess_points(const PointCloud& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4); 21 | 22 | /// @brief Preprocess point cloud (downsampling, kdtree creation, and normal and covariance estimation) 23 | /// @note This function only accepts Eigen::Vector(3|4)(f|d) as input 24 | /// @note When num_threads >= 2, this function has minor run-by-run non-determinism due to the parallel downsampling. 25 | /// @see small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp 26 | template 27 | std::pair>> 28 | preprocess_points(const std::vector>& points, double downsampling_resolution, int num_neighbors = 10, int num_threads = 4); 29 | 30 | /// @brief Create an incremental Gaussian voxel map. 31 | /// @see small_gicp::IncrementalVoxelMap 32 | /// @param points Input points 33 | /// @param voxel_resolution Voxel resolution 34 | GaussianVoxelMap::Ptr create_gaussian_voxelmap(const PointCloud& points, double voxel_resolution); 35 | 36 | /// @brief Registration setting 37 | struct RegistrationSetting { 38 | enum RegistrationType { ICP, PLANE_ICP, GICP, VGICP }; 39 | 40 | RegistrationType type = GICP; ///< Registration type 41 | double voxel_resolution = 1.0; ///< Voxel resolution for VGICP 42 | double downsampling_resolution = 0.25; ///< Downsample resolution (this will be used only in the Eigen-based interface) 43 | double max_correspondence_distance = 1.0; ///< Maximum correspondence distance 44 | double rotation_eps = 0.1 * M_PI / 180.0; ///< Rotation tolerance for convergence check [rad] 45 | double translation_eps = 1e-3; ///< Translation tolerance for convergence check 46 | int num_threads = 4; ///< Number of threads 47 | int max_iterations = 20; ///< Maximum number of iterations 48 | bool verbose = false; ///< Verbose mode 49 | }; 50 | 51 | /// @brief Align point clouds 52 | /// @note This function only accepts Eigen::Vector(3|4)(f|d) as input 53 | /// @see small_gicp::voxelgrid_sampling_omp, small_gicp::estimate_normals_covariances_omp 54 | /// @param target Target points 55 | /// @param source Source points 56 | /// @param init_T Initial guess of T_target_source 57 | /// @param setting Registration setting 58 | /// @return Registration result 59 | template 60 | RegistrationResult align( 61 | const std::vector>& target, 62 | const std::vector>& source, 63 | const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(), 64 | const RegistrationSetting& setting = RegistrationSetting()); 65 | 66 | /// @brief Align preprocessed point clouds 67 | /// @param target Target point cloud 68 | /// @param source Source point cloud 69 | /// @param target_tree Nearest neighbor search for the target point cloud 70 | /// @param init_T Initial guess of T_target_source 71 | /// @param setting Registration setting 72 | /// @return Registration result 73 | RegistrationResult align( 74 | const PointCloud& target, 75 | const PointCloud& source, 76 | const KdTree& target_tree, 77 | const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(), 78 | const RegistrationSetting& setting = RegistrationSetting()); 79 | 80 | /// @brief Align preprocessed point clouds with VGICP 81 | /// @param target Target Gaussian voxelmap 82 | /// @param source Source point cloud 83 | /// @param init_T Initial guess of T_target_source 84 | /// @param setting Registration setting 85 | /// @return Registration result 86 | RegistrationResult align( 87 | const GaussianVoxelMap& target, 88 | const PointCloud& source, 89 | const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity(), 90 | const RegistrationSetting& setting = RegistrationSetting()); 91 | 92 | } // namespace small_gicp 93 | -------------------------------------------------------------------------------- /include/small_gicp/registration/registration_result.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Registration result 11 | struct RegistrationResult { 12 | RegistrationResult(const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) 13 | : T_target_source(T), 14 | converged(false), 15 | iterations(0), 16 | num_inliers(0), 17 | H(Eigen::Matrix::Zero()), 18 | b(Eigen::Matrix::Zero()), 19 | error(0.0) {} 20 | 21 | Eigen::Isometry3d T_target_source; ///< Estimated transformation 22 | 23 | bool converged; ///< If the optimization converged 24 | size_t iterations; ///< Number of optimization iterations 25 | size_t num_inliers; ///< Number of inliear points 26 | 27 | Eigen::Matrix H; ///< Final information matrix 28 | Eigen::Matrix b; ///< Final information vector 29 | double error; ///< Final error 30 | }; 31 | 32 | } // namespace small_gicp 33 | -------------------------------------------------------------------------------- /include/small_gicp/registration/rejector.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Null correspondence rejector. This class accepts all input correspondences. 11 | struct NullRejector { 12 | template 13 | bool operator()(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { 14 | return false; 15 | } 16 | }; 17 | 18 | /// @brief Rejecting correspondences with large distances. 19 | struct DistanceRejector { 20 | DistanceRejector() : max_dist_sq(1.0) {} 21 | 22 | template 23 | bool operator()(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { 24 | return sq_dist > max_dist_sq; 25 | } 26 | 27 | double max_dist_sq; ///< Maximum squared distance between corresponding points 28 | }; 29 | 30 | } // namespace small_gicp 31 | -------------------------------------------------------------------------------- /include/small_gicp/registration/termination_criteria.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace small_gicp { 8 | 9 | /// @brief Registration termination criteria 10 | struct TerminationCriteria { 11 | /// @brief Constructor 12 | TerminationCriteria() : translation_eps(1e-3), rotation_eps(0.1 * M_PI / 180.0) {} 13 | 14 | /// @brief Check the convergence 15 | /// @param delta Transformation update vector 16 | /// @return True if converged 17 | bool converged(const Eigen::Matrix& delta) const { return delta.template head<3>().norm() <= rotation_eps && delta.template tail<3>().norm() <= translation_eps; } 18 | 19 | double translation_eps; ///< Rotation tolerance [rad] 20 | double rotation_eps; ///< Translation tolerance 21 | }; 22 | 23 | } // namespace small_gicp 24 | -------------------------------------------------------------------------------- /include/small_gicp/util/downsampling_omp.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace small_gicp { 15 | 16 | /// @brief Voxel grid downsampling with OpenMP backend. 17 | /// @note This function has minor run-by-run non-deterministic behavior due to parallel data collection that results 18 | /// in a deviation of the number of points in the downsampling results (up to 10% increase from the single-thread version). 19 | /// @note Discretized voxel coords must be in 21bit range [-1048576, 1048575]. 20 | /// For example, if the downsampling resolution is 0.01 m, point coordinates must be in [-10485.76, 10485.75] m. 21 | /// Points outside the valid range will be ignored. 22 | /// @param points Input points 23 | /// @param leaf_size Downsampling resolution 24 | /// @return Downsampled points 25 | template 26 | std::shared_ptr voxelgrid_sampling_omp(const InputPointCloud& points, double leaf_size, int num_threads = 4) { 27 | if (traits::size(points) == 0) { 28 | return std::make_shared(); 29 | } 30 | 31 | const double inv_leaf_size = 1.0 / leaf_size; 32 | 33 | constexpr std::uint64_t invalid_coord = std::numeric_limits::max(); 34 | constexpr int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int) 35 | constexpr size_t coord_bit_mask = (1 << 21) - 1; // Bit mask 36 | constexpr int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive 37 | 38 | std::vector> coord_pt(traits::size(points)); 39 | #pragma omp parallel for num_threads(num_threads) schedule(guided, 32) 40 | for (std::int64_t i = 0; i < traits::size(points); i++) { 41 | const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset; 42 | if ((coord < 0).any() || (coord > coord_bit_mask).any()) { 43 | std::cerr << "warning: voxel coord is out of range!!" << std::endl; 44 | coord_pt[i] = {invalid_coord, i}; 45 | continue; 46 | } 47 | // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit) 48 | const std::uint64_t bits = // 49 | (static_cast(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | // 50 | (static_cast(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | // 51 | (static_cast(coord[2] & coord_bit_mask) << (coord_bit_size * 2)); 52 | coord_pt[i] = {bits, i}; 53 | } 54 | 55 | // Sort by voxel coords 56 | quick_sort_omp(coord_pt.begin(), coord_pt.end(), [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }, num_threads); 57 | 58 | auto downsampled = std::make_shared(); 59 | traits::resize(*downsampled, traits::size(points)); 60 | 61 | // Take block-wise sum 62 | const int block_size = 1024; 63 | std::atomic_uint64_t num_points = 0; 64 | 65 | #pragma omp parallel for num_threads(num_threads) schedule(guided, 4) 66 | for (std::int64_t block_begin = 0; block_begin < traits::size(points); block_begin += block_size) { 67 | std::vector sub_points; 68 | sub_points.reserve(block_size); 69 | 70 | const size_t block_end = std::min(traits::size(points), block_begin + block_size); 71 | 72 | Eigen::Vector4d sum_pt = traits::point(points, coord_pt[block_begin].second); 73 | for (size_t i = block_begin + 1; i != block_end; i++) { 74 | if (coord_pt[i].first == invalid_coord) { 75 | continue; 76 | } 77 | 78 | if (coord_pt[i - 1].first != coord_pt[i].first) { 79 | sub_points.emplace_back(sum_pt / sum_pt.w()); 80 | sum_pt.setZero(); 81 | } 82 | sum_pt += traits::point(points, coord_pt[i].second); 83 | } 84 | sub_points.emplace_back(sum_pt / sum_pt.w()); 85 | 86 | const size_t point_index_begin = num_points.fetch_add(sub_points.size()); 87 | for (size_t i = 0; i < sub_points.size(); i++) { 88 | traits::set_point(*downsampled, point_index_begin + i, sub_points[i]); 89 | } 90 | } 91 | 92 | traits::resize(*downsampled, num_points); 93 | 94 | return downsampled; 95 | } 96 | 97 | } // namespace small_gicp 98 | -------------------------------------------------------------------------------- /include/small_gicp/util/downsampling_tbb.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace small_gicp { 15 | 16 | /// @brief Voxel grid downsampling with TBB backend. 17 | /// @note This function has minor run-by-run non-deterministic behavior due to parallel data collection that results 18 | /// in a deviation of the number of points in the downsampling results (up to 10% increase from the single-thread version). 19 | /// @note Discretized voxel coords must be in 21bit range [-1048576, 1048575]. 20 | /// For example, if the downsampling resolution is 0.01 m, point coordinates must be in [-10485.76, 10485.75] m. 21 | /// Points outside the valid range will be ignored. 22 | /// @param points Input points 23 | /// @param leaf_size Downsampling resolution 24 | /// @return Downsampled points 25 | template 26 | std::shared_ptr voxelgrid_sampling_tbb(const InputPointCloud& points, double leaf_size) { 27 | if (traits::size(points) == 0) { 28 | return std::make_shared(); 29 | } 30 | 31 | const double inv_leaf_size = 1.0 / leaf_size; 32 | 33 | constexpr std::uint64_t invalid_coord = std::numeric_limits::max(); 34 | constexpr int coord_bit_size = 21; // Bits to represent each voxel coordinate (pack 21x3 = 63bits in 64bit int) 35 | constexpr size_t coord_bit_mask = (1 << 21) - 1; // Bit mask 36 | constexpr int coord_offset = 1 << (coord_bit_size - 1); // Coordinate offset to make values positive 37 | 38 | std::vector> coord_pt(traits::size(points)); 39 | tbb::parallel_for(tbb::blocked_range(0, traits::size(points), 64), [&](const tbb::blocked_range& range) { 40 | for (size_t i = range.begin(); i != range.end(); i++) { 41 | const Eigen::Array4i coord = fast_floor(traits::point(points, i) * inv_leaf_size) + coord_offset; 42 | if ((coord < 0).any() || (coord > coord_bit_mask).any()) { 43 | std::cerr << "warning: voxel coord is out of range!!" << std::endl; 44 | coord_pt[i] = {invalid_coord, i}; 45 | continue; 46 | } 47 | 48 | // Compute voxel coord bits (0|1bit, z|21bit, y|21bit, x|21bit) 49 | const std::uint64_t bits = // 50 | (static_cast(coord[0] & coord_bit_mask) << (coord_bit_size * 0)) | // 51 | (static_cast(coord[1] & coord_bit_mask) << (coord_bit_size * 1)) | // 52 | (static_cast(coord[2] & coord_bit_mask) << (coord_bit_size * 2)); 53 | coord_pt[i] = {bits, i}; 54 | } 55 | }); 56 | 57 | // Sort by voxel coords 58 | tbb::parallel_sort(coord_pt, [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); 59 | 60 | auto downsampled = std::make_shared(); 61 | traits::resize(*downsampled, traits::size(points)); 62 | 63 | // Take block-wise sum 64 | const int block_size = 2048; 65 | std::atomic_uint64_t num_points = 0; 66 | tbb::parallel_for(tbb::blocked_range(0, traits::size(points), block_size), [&](const tbb::blocked_range& range) { 67 | std::vector sub_points; 68 | sub_points.reserve(block_size); 69 | 70 | Eigen::Vector4d sum_pt = traits::point(points, coord_pt[range.begin()].second); 71 | for (size_t i = range.begin() + 1; i != range.end(); i++) { 72 | if (coord_pt[i].first == invalid_coord) { 73 | continue; 74 | } 75 | 76 | if (coord_pt[i - 1].first != coord_pt[i].first) { 77 | sub_points.emplace_back(sum_pt / sum_pt.w()); 78 | sum_pt.setZero(); 79 | } 80 | sum_pt += traits::point(points, coord_pt[i].second); 81 | } 82 | sub_points.emplace_back(sum_pt / sum_pt.w()); 83 | 84 | const size_t point_index_begin = num_points.fetch_add(sub_points.size()); 85 | for (size_t i = 0; i < sub_points.size(); i++) { 86 | traits::set_point(*downsampled, point_index_begin + i, sub_points[i]); 87 | } 88 | }); 89 | 90 | traits::resize(*downsampled, num_points); 91 | 92 | return downsampled; 93 | } 94 | 95 | } // namespace small_gicp 96 | -------------------------------------------------------------------------------- /include/small_gicp/util/fast_floor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace small_gicp { 8 | 9 | /// @brief Fast floor (https://stackoverflow.com/questions/824118/why-is-floor-so-slow). 10 | /// @param pt Double vector 11 | /// @return Floored int vector 12 | inline Eigen::Array4i fast_floor(const Eigen::Array4d& pt) { 13 | const Eigen::Array4i ncoord = pt.cast(); 14 | return ncoord - (pt < ncoord.cast()).cast(); 15 | }; 16 | 17 | } // namespace small_gicp 18 | -------------------------------------------------------------------------------- /include/small_gicp/util/lie.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace small_gicp { 9 | 10 | /// @brief Create skew symmetric matrix 11 | /// @param x Rotation vector 12 | /// @return Skew symmetric matrix 13 | inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) { 14 | Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); 15 | skew(0, 1) = -x[2]; 16 | skew(0, 2) = x[1]; 17 | skew(1, 0) = x[2]; 18 | skew(1, 2) = -x[0]; 19 | skew(2, 0) = -x[1]; 20 | skew(2, 1) = x[0]; 21 | 22 | return skew; 23 | } 24 | 25 | /* 26 | * SO3 expmap code taken from Sophus 27 | * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 28 | * 29 | * Copyright 2011-2017 Hauke Strasdat 30 | * 2012-2017 Steven Lovegrove 31 | * 32 | * Permission is hereby granted, free of charge, to any person obtaining a copy 33 | * of this software and associated documentation files (the "Software"), to 34 | * deal in the Software without restriction, including without limitation the 35 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 36 | * sell copies of the Software, and to permit persons to whom the Software is 37 | * furnished to do so, subject to the following conditions: 38 | * 39 | * The above copyright notice and this permission notice shall be included in 40 | * all copies or substantial portions of the Software. 41 | * 42 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 43 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 44 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 45 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 46 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 47 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 48 | * IN THE SOFTWARE. 49 | */ 50 | 51 | /// @brief SO3 expmap. 52 | /// @param omega [rx, ry, rz] 53 | /// @return Quaternion 54 | inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { 55 | double theta_sq = omega.dot(omega); 56 | 57 | double imag_factor; 58 | double real_factor; 59 | if (theta_sq < 1e-10) { 60 | double theta_quad = theta_sq * theta_sq; 61 | imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; 62 | real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; 63 | } else { 64 | double theta = std::sqrt(theta_sq); 65 | double half_theta = 0.5 * theta; 66 | imag_factor = std::sin(half_theta) / theta; 67 | real_factor = std::cos(half_theta); 68 | } 69 | 70 | return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 71 | } 72 | 73 | // Rotation-first 74 | /// @brief SE3 expmap (Rotation-first). 75 | /// @param a Twist vector [rx, ry, rz, tx, ty, tz] 76 | /// @return SE3 matrix 77 | inline Eigen::Isometry3d se3_exp(const Eigen::Matrix& a) { 78 | const Eigen::Vector3d omega = a.head<3>(); 79 | 80 | const double theta_sq = omega.dot(omega); 81 | const double theta = std::sqrt(theta_sq); 82 | 83 | Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity(); 84 | se3.linear() = so3_exp(omega).toRotationMatrix(); 85 | 86 | if (theta < 1e-10) { 87 | se3.translation() = se3.linear() * a.tail<3>(); 88 | /// Note: That is an accurate expansion! 89 | } else { 90 | const Eigen::Matrix3d Omega = skew(omega); 91 | const Eigen::Matrix3d V = (Eigen::Matrix3d::Identity() + (1.0 - std::cos(theta)) / theta_sq * Omega + (theta - std::sin(theta)) / (theta_sq * theta) * Omega * Omega); 92 | se3.translation() = V * a.tail<3>(); 93 | } 94 | 95 | return se3; 96 | } 97 | 98 | } // namespace small_gicp 99 | -------------------------------------------------------------------------------- /include/small_gicp/util/sort_omp.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | #ifdef _MSC_VER 8 | #pragma message("warning: Task-based OpenMP parallelism is not well supported on windows.") 9 | #pragma message("warning: Thus, OpenMP-based downsampling is only partially parallelized on windows.") 10 | #endif 11 | 12 | namespace small_gicp { 13 | 14 | /// @brief Implementation of merge sort with OpenMP parallelism. Do not call this directly. Use merge_sort_omp instead. 15 | /// @param first First iterator 16 | /// @param last Last iterator 17 | /// @param comp Comparison function 18 | template 19 | void merge_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp) { 20 | const size_t n = std::distance(first, last); 21 | if (n < 1024) { 22 | std::sort(first, last, comp); 23 | return; 24 | } 25 | 26 | auto center = first + n / 2; 27 | 28 | #pragma omp task 29 | merge_sort_omp_impl(first, center, comp); 30 | 31 | #pragma omp task 32 | merge_sort_omp_impl(center, last, comp); 33 | 34 | #pragma omp taskwait 35 | std::inplace_merge(first, center, last, comp); 36 | } 37 | 38 | /// @brief Merge sort with OpenMP parallelism. 39 | /// @note This tends to be slower than quick_sort_omp. 40 | /// @param first First iterator 41 | /// @param last Last iterator 42 | /// @param comp Comparison function 43 | /// @param num_threads Number of threads 44 | template 45 | void merge_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp, int num_threads) { 46 | #ifndef _MSC_VER 47 | #pragma omp parallel num_threads(num_threads) 48 | { 49 | #pragma omp single nowait 50 | { merge_sort_omp_impl(first, last, comp); } 51 | } 52 | #else 53 | std::stable_sort(first, last, comp); 54 | #endif 55 | } 56 | 57 | /// @brief Implementation of quick sort with OpenMP parallelism. Do not call this directly. Use quick_sort_omp instead. 58 | /// @param first First iterator 59 | /// @param last Last iterator 60 | /// @param comp Comparison function 61 | template 62 | void quick_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp) { 63 | const std::ptrdiff_t n = std::distance(first, last); 64 | if (n < 1024) { 65 | std::sort(first, last, comp); 66 | return; 67 | } 68 | 69 | const auto median3 = [&](const auto& a, const auto& b, const auto& c, const Compare& comp) { 70 | return comp(a, b) ? (comp(b, c) ? b : (comp(a, c) ? c : a)) : (comp(a, c) ? a : (comp(b, c) ? c : b)); 71 | }; 72 | 73 | const int offset = n / 8; 74 | const auto m1 = median3(*first, *(first + offset), *(first + offset * 2), comp); 75 | const auto m2 = median3(*(first + offset * 3), *(first + offset * 4), *(first + offset * 5), comp); 76 | const auto m3 = median3(*(first + offset * 6), *(first + offset * 7), *(last - 1), comp); 77 | 78 | auto pivot = median3(m1, m2, m3, comp); 79 | auto middle1 = std::partition(first, last, [&](const auto& val) { return comp(val, pivot); }); 80 | auto middle2 = std::partition(middle1, last, [&](const auto& val) { return !comp(pivot, val); }); 81 | 82 | #pragma omp task 83 | quick_sort_omp_impl(first, middle1, comp); 84 | 85 | #pragma omp task 86 | quick_sort_omp_impl(middle2, last, comp); 87 | } 88 | 89 | /// @brief Quick sort with OpenMP parallelism. 90 | /// @param first First iterator 91 | /// @param last Last iterator 92 | /// @param comp Comparison function 93 | /// @param num_threads Number of threads 94 | template 95 | void quick_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp, int num_threads) { 96 | #ifndef _MSC_VER 97 | #pragma omp parallel num_threads(num_threads) 98 | { 99 | #pragma omp single nowait 100 | { quick_sort_omp_impl(first, last, comp); } 101 | } 102 | #else 103 | std::sort(first, last, comp); 104 | #endif 105 | } 106 | 107 | } // namespace small_gicp 108 | -------------------------------------------------------------------------------- /include/small_gicp/util/vector3i_hash.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace small_gicp { 8 | 9 | /** 10 | * @brief Spatial hashing function. 11 | * Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable Objects", VMV2003. 12 | */ 13 | struct XORVector3iHash { 14 | public: 15 | size_t operator()(const Eigen::Vector3i& x) const { 16 | const size_t p1 = 73856093; 17 | const size_t p2 = 19349669; // 19349663 was not a prime number 18 | const size_t p3 = 83492791; 19 | return static_cast((x[0] * p1) ^ (x[1] * p2) ^ (x[2] * p3)); 20 | } 21 | 22 | static size_t hash(const Eigen::Vector3i& x) { return XORVector3iHash()(x); } 23 | static bool equal(const Eigen::Vector3i& x1, const Eigen::Vector3i& x2) { return x1 == x2; } 24 | }; 25 | 26 | } // namespace small_gicp 27 | -------------------------------------------------------------------------------- /mkdocs.yml: -------------------------------------------------------------------------------- 1 | site_name: "small_gicp" 2 | site_author: k.koide 3 | repo_url: https://github.com/koide3/small_gicp 4 | site_url: https://github.com/koide3/small_gicp 5 | site_description: "small_gicp: Efficient and parallel algorithms for point cloud registration" 6 | 7 | theme: 8 | name: material 9 | palette: 10 | primary: indigo 11 | front: 12 | text: Roboto 13 | 14 | use_directory_urls: false 15 | 16 | markdown_extensions: 17 | - meta 18 | - attr_list 19 | - admonition 20 | - pymdownx.highlight: 21 | anchor_linenums: true 22 | - pymdownx.inlinehilite 23 | - pymdownx.snippets 24 | - pymdownx.superfences 25 | 26 | copyright: Copyright © 2024 Kenji Koide 27 | extra: 28 | social: 29 | - icon: material/home 30 | link: https://staff.aist.go.jp/k.koide/ 31 | - icon: fontawesome/brands/github 32 | link: https://github.com/koide3/small_gicp 33 | - icon: fontawesome/brands/twitter 34 | link: https://twitter.com/k_koide3 35 | 36 | extra_css: 37 | - "css/custom.css" 38 | - "https://maxcdn.bootstrapcdn.com/font-awesome/4.6.1/css/font-awesome.min.css" 39 | 40 | nav: 41 | - 'index.md' 42 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | small_gicp 5 | 1.0.0 6 | Efficient and parallelized algorithms for point cloud registration 7 | 8 | Kenji Koide 9 | MIT 10 | 11 | cmake 12 | eigen 13 | libomp-dev 14 | 15 | 16 | 17 | 18 | 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit-build-core >=0.4.3", "pybind11 >2.10.0"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "small_gicp" 7 | version = "1.0.0" 8 | authors = [{name = "Kenji Koide", email = "k.koide@aist.go.jp"}] 9 | description = "Efficient and parallelized algorithms for fine point cloud registration" 10 | readme = "README.md" 11 | license = {text = "MIT"} 12 | requires-python = ">=3.7" 13 | 14 | [project.urls] 15 | Homepage = "https://github.com/koide3/small_gicp" 16 | 17 | [tool.setuptools] 18 | zip_safe = false 19 | 20 | [tool.scikit-build] 21 | wheel.license-files = ["LICENSE"] 22 | install.components = ["python"] 23 | build-dir = "build/{wheel_tag}" 24 | wheel.py-api = "cp312" # Build stable ABI wheels for CPython 3.12+ 25 | 26 | [tool.scikit-build.cmake.define] 27 | BUILD_PYTHON_BINDINGS = true 28 | BUILD_HELPER = false 29 | BUILD_SHARED_LIBS = false 30 | 31 | [tool.cibuildwheel.linux] 32 | before-all = "yum install -y libomp-devel" 33 | 34 | [tool.cibuildwheel.macos] 35 | before-all = "brew install eigen libomp" 36 | environment = { OpenMP_ROOT="/opt/homebrew/opt/libomp" } 37 | 38 | [tool.cibuildwheel.windows] 39 | before-all = "choco install eigen -y" 40 | test-command = "cd /d {project} && pytest {project}/src --color=yes -v" 41 | 42 | [tool.cibuildwheel] 43 | manylinux-x86_64-image = "manylinux_2_28" 44 | manylinux-aarch64-image = "manylinux_2_28" 45 | build = "*" 46 | skip = "pp* *-win32 *_i686 *musllinux*" # Don't build for PyPy, Win32, i686 47 | test-requires = "pytest numpy scipy" 48 | test-command = "cd {project} && pytest {project}/src --color=yes -v" 49 | build-verbosity = 1 50 | -------------------------------------------------------------------------------- /scripts/plot_kdtree.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import re 3 | import os 4 | import numpy 5 | from matplotlib import pyplot 6 | from collections import namedtuple 7 | 8 | def parse_result(filename): 9 | num_points = [] 10 | results = {} 11 | 12 | with open(filename, 'r') as f: 13 | for line in f.readlines(): 14 | matched = re.match(r'num_threads=(\d+)', line) 15 | if matched: 16 | num_threads = int(matched.group(1)) 17 | continue 18 | 19 | matched = re.match(r'num_points=(\d+)', line) 20 | if matched: 21 | num_points.append(int(matched.group(1))) 22 | continue 23 | 24 | matched = re.match(r'(\S+)_times=(\S+) \+\- (\S+) \(median=(\S+)\)', line) 25 | if matched: 26 | method = matched.group(1) 27 | mean = float(matched.group(2)) 28 | std = float(matched.group(3)) 29 | med = float(matched.group(4)) 30 | 31 | if method not in results: 32 | results[method] = [] 33 | 34 | results[method].append(med) 35 | continue 36 | 37 | return (num_threads, num_points, results) 38 | 39 | def main(): 40 | results_path = os.path.dirname(__file__) + '/results' 41 | 42 | results = {} 43 | for filename in os.listdir(results_path): 44 | matched = re.match(r'kdtree_benchmark_(\S+)_(\d+).txt', filename) 45 | if not matched: 46 | continue 47 | 48 | method = '{}_{}'.format(matched.group(1), matched.group(2)) 49 | print(method) 50 | 51 | num_threads, num_points, rets = parse_result(results_path + '/' + filename) 52 | results[method] = rets[list(rets.keys())[0]] 53 | 54 | fig, axes = pyplot.subplots(1, 2, figsize=(12, 3)) 55 | 56 | num_threads = [1, 2, 3, 4, 5, 6, 7, 8, 16, 32, 64, 128] 57 | axes[0].plot(num_points, results['small_1'], label='kdtree (single-thread)', marker='o', linestyle='--') 58 | for idx in [1, 2, 3, 5, 7, 8, 9]: 59 | N = num_threads[idx] 60 | axes[0].plot(num_points, results['omp_{}'.format(N)], label='kdtree_omp (%d threads)' % N, marker='s') 61 | # axes[0].plot(num_points, results['tbb_{}'.format(N)], label='kdtree_tbb (%d threads)' % N, marker='^') 62 | 63 | baseline = numpy.array(results['small_1']) 64 | axes[1].plot([num_threads[0], num_threads[-1]], [1.0, 1.0], label='kdtree (single-thread)', linestyle='--') 65 | for idx in [5]: 66 | threads = num_threads[idx] 67 | N = num_points[idx] 68 | 69 | axes[1].plot(num_threads, baseline[idx] / [results['omp_{}'.format(threads)][idx] for threads in num_threads], label='omp (num_points=%d)' % N, marker='s') 70 | axes[1].plot(num_threads, baseline[idx] / [results['tbb_{}'.format(threads)][idx] for threads in num_threads], label='tbb (num_points=%d)' % N, marker='^') 71 | 72 | axes[1].set_xscale('log') 73 | 74 | axes[0].set_xlabel('Number of points') 75 | axes[0].set_ylabel('KdTree construction time [msec/scan]') 76 | axes[1].set_xlabel('Number of threads = [1, 2, 4, ..., 128]') 77 | axes[1].set_ylabel('Processing speed ratio (single-thread = 1.0)') 78 | 79 | axes[0].grid() 80 | axes[1].grid() 81 | 82 | axes[0].legend() 83 | axes[1].legend() 84 | 85 | fig.savefig('kdtree_time.svg') 86 | 87 | pyplot.show() 88 | 89 | 90 | 91 | if __name__ == '__main__': 92 | main() -------------------------------------------------------------------------------- /scripts/plot_odometry.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import os 3 | import re 4 | import numpy 5 | from collections import namedtuple 6 | from matplotlib import pyplot 7 | 8 | Result = namedtuple('Result', ['reg_mean', 'reg_std', 'tp_mean', 'tp_std']) 9 | 10 | def parse_result(filename): 11 | reg_mean = None 12 | reg_std = None 13 | throughput_mean = None 14 | throughput_std = None 15 | with open(filename, 'r') as f: 16 | for line in f.readlines(): 17 | found = re.findall(r'([^=]+)\s*\+\-\s*(\S+)', line) 18 | if not found or len(found) != 2: 19 | found = re.findall(r'total_throughput=(\S+)', line) 20 | if found: 21 | throughput_mean = float(found[0]) 22 | continue 23 | 24 | reg_mean = float(found[0][0].strip()) 25 | reg_std = float(found[0][1].strip()) 26 | throughput_mean = float(found[1][0].strip()) 27 | throughput_std = float(found[1][1].strip()) 28 | 29 | return Result(reg_mean, reg_std, throughput_mean, throughput_std) 30 | 31 | def main(): 32 | results_path = os.path.dirname(__file__) + '/results' 33 | 34 | results = {} 35 | for filename in os.listdir(results_path): 36 | found = re.findall(r'odometry_benchmark_(\S+)_(\d+).txt', filename) 37 | if not found: 38 | continue 39 | 40 | rets = parse_result(results_path + '/' + filename) 41 | results['{}_{}'.format(found[0][0], found[0][1])] = rets 42 | 43 | fig, axes = pyplot.subplots(2, 2, figsize=(24, 12)) 44 | 45 | num_threads = [1, 2, 4, 8, 16, 32, 64, 128] 46 | 47 | pcl_reg = results['pcl_1'].reg_mean 48 | pcl_tp = results['pcl_1'].tp_mean 49 | axes[0, 0].plot([num_threads[0], num_threads[-1]], [pcl_reg, pcl_reg], label='pcl_gicp', linestyle='--') 50 | axes[0, 1].plot([num_threads[0], num_threads[-1]], [pcl_tp, pcl_tp], label='pcl_gicp', linestyle='--') 51 | axes[1, 0].plot([num_threads[0], num_threads[-1]], [1.0, 1.0], label='pcl_gicp', linestyle='--') 52 | axes[1, 1].plot([num_threads[0], num_threads[-1]], [1.0, 1.0], label='pcl_gicp', linestyle='--') 53 | 54 | methods = ['fast_gicp', 'fast_vgicp', 'small_gicp_omp', 'small_gicp_tbb', 'small_vgicp_tbb', 'small_vgicp_omp'] 55 | markers = ['o', 'o', '^', '^', 's', 's'] 56 | 57 | for method, marker in zip(methods, markers): 58 | reg_means = [results['{}_{}'.format(method, N)].reg_mean for N in num_threads] 59 | axes[0, 0].plot(num_threads, reg_means, label=method, marker=marker) 60 | axes[1, 0].plot(num_threads, pcl_reg / numpy.array(reg_means), label=method, marker=marker) 61 | 62 | for method, marker in zip(methods, markers): 63 | tp_means = [results['{}_{}'.format(method, N)].tp_mean for N in num_threads] 64 | axes[0, 1].plot(num_threads, tp_means, label=method, marker=marker) 65 | axes[1, 1].plot(num_threads, pcl_tp / numpy.array(tp_means), label=method, marker=marker) 66 | flow_tp_means = [results['small_gicp_tbb_flow_{}'.format(N)].tp_mean for N in num_threads] 67 | axes[0, 1].plot(num_threads, flow_tp_means, label='small_gicp_tbb_flow', marker='*') 68 | axes[1, 1].plot(num_threads, pcl_tp / numpy.array(flow_tp_means), label='small_gicp_tbb_flow', marker='*') 69 | 70 | axes[0, 0].set_title('Net registration time (KdTree construction + cov estimation + pose estimation)') 71 | axes[1, 0].set_title('Net registration time (KdTree construction + cov estimation + pose estimation)') 72 | axes[0, 1].set_title('Total throughput (Downsampling + registration)') 73 | axes[1, 1].set_title('Total throughput (Downsampling + registration)') 74 | axes[0, 0].set_ylabel('Time [msec/scan]') 75 | axes[0, 1].set_ylabel('Time [msec/scan]') 76 | axes[1, 0].set_ylabel('Processing speed ratio (pcl_gicp=1.0)') 77 | axes[1, 1].set_ylabel('Processing speed ratio (pcl_gicp=1.0)') 78 | for i in range(2): 79 | for j in range(2): 80 | axes[i, j].set_xlabel('Number of threads = [1, 2, 4, ..., 128]') 81 | axes[i, j].set_xscale('log') 82 | axes[i, j].legend() 83 | axes[i, j].grid() 84 | 85 | fig.savefig('odometry_time.svg') 86 | pyplot.show() 87 | 88 | if __name__ == "__main__": 89 | main() -------------------------------------------------------------------------------- /scripts/plot_odometry_accuracy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import re 3 | import os 4 | import pathos 5 | import subprocess 6 | from collections import namedtuple 7 | from matplotlib import pyplot 8 | 9 | def run_evo(commands): 10 | p = subprocess.Popen(commands, shell=False, stdout=subprocess.PIPE, stderr=subprocess.PIPE) 11 | p.wait() 12 | stdout, stderr = p.communicate() 13 | 14 | if len(stderr): 15 | print(stderr.decode('utf-8')) 16 | 17 | result = stdout.decode('utf-8') 18 | results = {} 19 | for item in re.findall(r'([a-z]+)\s+([0-9]+\.[0-9]+)', result): 20 | results[item[0]] = float(item[1]) 21 | 22 | return results 23 | 24 | 25 | def eval_ape(gt_filename, traj_filename, t_offset=0.0): 26 | ret = run_evo(['evo_ape', 'kitti', gt_filename, traj_filename, '-a']) 27 | return ret 28 | 29 | 30 | def eval_rpe(gt_filename, traj_filename, delta_unit='m', delta=100, all_pairs=True, t_offset=0.0): 31 | commands = ['evo_rpe', 'kitti', gt_filename, traj_filename, '-a', '--delta_unit', str(delta_unit), '--delta', str(delta)] 32 | if all_pairs: 33 | commands += ['--all_pairs'] 34 | 35 | ret = run_evo(commands) 36 | return ret 37 | 38 | 39 | def main(): 40 | gt_path = '/home/koide/datasets/ssd/kitti/poses/00_lidar.txt' 41 | 42 | results_path = os.path.dirname(__file__) + '/results' 43 | 44 | filenames = [] 45 | for filename in os.listdir(results_path): 46 | found = re.findall(r'traj_lidar_(\S+)_(\d+).txt', filename) 47 | if not found: 48 | continue 49 | 50 | method = found[0][0] + '_' + found[0][1] 51 | filenames.append((method, results_path + '/' + filename)) 52 | 53 | methods = ['pcl_1', 'fast_gicp_128', 'fast_vgicp_128', 'small_gicp_1', 'small_gicp_tbb_128', 'small_gicp_omp_128', 'small_vgicp_tbb_128'] 54 | labels = ['pcl_gicp', 'fast_gicp', 'fast_vgicp', 'small_gicp', 'small_gicp (tbb)', 'small_gicp (omp)', 'small_vgicp'] 55 | 56 | Result = namedtuple('Result', ['ape', 'rpe100', 'rpe400', 'rpe800']) 57 | def evaluate(inputs): 58 | method, filename = inputs 59 | print('.', end='', flush=True) 60 | ape = eval_ape(gt_path, filename) 61 | rpe100 = eval_rpe(gt_path, filename, delta=100) 62 | rpe400 = eval_rpe(gt_path, filename, delta=400) 63 | rpe800 = eval_rpe(gt_path, filename, delta=800) 64 | return method, Result(ape, rpe100, rpe400, rpe800) 65 | 66 | print('evaluating') 67 | with pathos.multiprocessing.ProcessingPool() as p: 68 | errors = p.map(evaluate, [(method, filename) for method, filename in filenames if method in methods]) 69 | print() 70 | 71 | results = {} 72 | for method, error in errors: 73 | results[method] = error 74 | 75 | for method, label in zip(methods, labels): 76 | ape, rpe100, rpe400, rpe800 = results[method] 77 | print('{:20s} : APE={:.3f} +- {:.3f} RPE(100)={:.3f} +- {:.3f} RPE(400)={:.3f} +- {:.3f} RPE(800)={:.3f} +- {:.3f}' 78 | .format(label, ape['rmse'], ape['std'], rpe100['rmse'], rpe100['std'], rpe400['rmse'], rpe400['std'], rpe800['rmse'], rpe800['std'])) 79 | 80 | fig, axes = pyplot.subplots(1, 3, figsize=(15, 5)) 81 | apes = [results[method].ape['rmse'] for method in methods] 82 | axes[0].bar(labels, apes) 83 | 84 | pyplot.show() 85 | 86 | 87 | 88 | 89 | 90 | if __name__ == '__main__': 91 | main() 92 | -------------------------------------------------------------------------------- /scripts/plot_odometry_native.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import os 3 | import re 4 | import numpy 5 | from collections import namedtuple 6 | from matplotlib import pyplot 7 | 8 | Result = namedtuple('Result', ['reg_mean', 'reg_std', 'tp_mean', 'tp_std']) 9 | 10 | def parse_result(filename): 11 | reg_mean = None 12 | reg_std = None 13 | throughput_mean = None 14 | throughput_std = None 15 | with open(filename, 'r') as f: 16 | for line in f.readlines(): 17 | found = re.findall(r'([^=]+)\s*\+\-\s*(\S+)', line) 18 | if not found or len(found) != 2: 19 | found = re.findall(r'total_throughput=(\S+)', line) 20 | if found: 21 | throughput_mean = float(found[0]) 22 | continue 23 | 24 | reg_mean = float(found[0][0].strip()) 25 | reg_std = float(found[0][1].strip()) 26 | throughput_mean = float(found[1][0].strip()) 27 | throughput_std = float(found[1][1].strip()) 28 | 29 | return Result(reg_mean, reg_std, throughput_mean, throughput_std) 30 | 31 | def main(): 32 | results_path = os.path.dirname(__file__) + '/results' 33 | 34 | results = {} 35 | for filename in os.listdir(results_path): 36 | found = re.findall(r'odometry_benchmark_(\S+)_(native|nonnative)_(\d+).txt', filename) 37 | if not found: 38 | continue 39 | 40 | rets = parse_result(results_path + '/' + filename) 41 | results['{}_{}_{}'.format(found[0][0], found[0][1], found[0][2])] = rets 42 | 43 | fig, axes = pyplot.subplots(1, 1, figsize=(12, 2)) 44 | axes = [axes] 45 | 46 | num_threads = [1, 2, 4, 8, 16, 32, 64, 128] 47 | 48 | print(results['small_gicp_native_1'], results['small_gicp_tbb_native_1']) 49 | print(results['small_gicp_nonnative_1'], results['small_gicp_tbb_nonnative_1']) 50 | 51 | native = [results['small_gicp_tbb_native_{}'.format(N)].reg_mean for N in num_threads] 52 | nonnative = [results['small_gicp_tbb_nonnative_{}'.format(N)].reg_mean for N in num_threads] 53 | 54 | axes[0].plot(num_threads, native, label='small_gicp_tbb (-march=native)', marker='o') 55 | axes[0].plot(num_threads, nonnative, label='small_gicp_tbb (nonnative)', marker='o') 56 | axes[0].set_xlabel('Number of threads [1, 2, ..., 128]') 57 | axes[0].set_ylabel('Time [msec/scan]') 58 | axes[0].set_xscale('log') 59 | axes[0].grid() 60 | axes[0].legend() 61 | 62 | pyplot.show() 63 | 64 | 65 | if __name__ == "__main__": 66 | main() -------------------------------------------------------------------------------- /scripts/run_downsampling_benchmark.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | dataset_path=$1 3 | exe_path=../build/downsampling_benchmark 4 | 5 | mkdir results 6 | num_threads=(2 3 4 5 6 7 8 16 32 64 128) 7 | 8 | for N in ${num_threads[@]}; do 9 | echo $exe_path $dataset_path --num_threads $N | tee results/downsampling_benchmark_$N.txt 10 | $exe_path $dataset_path --num_threads $N | tee results/downsampling_benchmark_$N.txt 11 | done 12 | -------------------------------------------------------------------------------- /scripts/run_kdtree_benchmark.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | dataset_path=$1 3 | exe_path=../build/kdtree_benchmark 4 | 5 | mkdir results 6 | num_threads=(1 2 3 4 5 6 7 8 16 32 64 92 128) 7 | 8 | $exe_path $dataset_path --num_threads 1 --num_trials 1000 --method small | tee results/kdtree_benchmark_small_$N.txt 9 | 10 | for N in ${num_threads[@]}; do 11 | sleep 1 12 | $exe_path $dataset_path --num_threads $N --num_trials 1000 --method tbb | tee results/kdtree_benchmark_tbb_$N.txt 13 | done 14 | 15 | for N in ${num_threads[@]}; do 16 | sleep 1 17 | $exe_path $dataset_path --num_threads $N --num_trials 1000 --method omp | tee results/kdtree_benchmark_omp_$N.txt 18 | done 19 | -------------------------------------------------------------------------------- /scripts/run_odometry_benchmark.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | dataset_path=$1 3 | exe_path=../build/odometry_benchmark 4 | 5 | mkdir results 6 | 7 | engines=(pcl fast_gicp fast_vgicp small_gicp small_gicp_tbb small_gicp_omp) 8 | for engine in ${engines[@]}; do 9 | N=1 10 | $exe_path $dataset_path $(printf "results/traj_lidar_%s_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_%d.txt" $engine $N) 11 | done 12 | 13 | engines=(fast_gicp fast_vgicp small_gicp_omp small_gicp_tbb small_vgicp_omp small_vgicp_tbb small_gicp_tbb_flow) 14 | num_threads=(128 96 64 32 16 8 4 2) 15 | 16 | for N in ${num_threads[@]}; do 17 | for engine in ${engines[@]}; do 18 | $exe_path $dataset_path $(printf "results/traj_lidar_%s_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_%d.txt" $engine $N) 19 | done 20 | done 21 | -------------------------------------------------------------------------------- /scripts/run_odometry_benchmark_native.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | dataset_path=$1 3 | exe_path=../build/odometry_benchmark 4 | 5 | mkdir results 6 | 7 | engines=(small_gicp small_gicp_tbb) 8 | for engine in ${engines[@]}; do 9 | N=1 10 | $exe_path $dataset_path $(printf "results/traj_lidar_%s_nonnative_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_nonnative_%d.txt" $engine $N) 11 | done 12 | 13 | engines=(small_gicp_tbb) 14 | num_threads=(128 96 64 32 16 8 4 2) 15 | 16 | for N in ${num_threads[@]}; do 17 | for engine in ${engines[@]}; do 18 | $exe_path $dataset_path $(printf "results/traj_lidar_%s_nonnative_%d.txt" $engine $N) --num_threads $N --engine $engine | tee $(printf "results/odometry_benchmark_%s_nonnative_%d.txt" $engine $N) 19 | done 20 | done 21 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main(int argc, char** argv) { 7 | using namespace small_gicp; 8 | 9 | if (argc < 3) { 10 | std::cout << "USAGE: odometry_benchmark [options]" << std::endl; 11 | std::cout << "OPTIONS:" << std::endl; 12 | std::cout << " --visualize" << std::endl; 13 | std::cout << " --num_threads (default: 4)" << std::endl; 14 | std::cout << " --num_neighbors (default: 20)" << std::endl; 15 | std::cout << " --downsampling_resolution (default: 0.25)" << std::endl; 16 | std::cout << " --voxel_resolution (default: 2.0)" << std::endl; 17 | 18 | const auto odom_names = odometry_names(); 19 | std::stringstream sst; 20 | for (size_t i = 0; i < odom_names.size(); i++) { 21 | if (i) { 22 | sst << "|"; 23 | } 24 | sst << odom_names[i]; 25 | } 26 | 27 | std::cout << " --engine <" << sst.str() << "> (default: small_gicp)" << std::endl; 28 | return 0; 29 | } 30 | 31 | const std::string dataset_path = argv[1]; 32 | const std::string output_path = argv[2]; 33 | 34 | OdometryEstimationParams params; 35 | std::string engine = "small_gicp"; 36 | 37 | for (int i = 0; i < argc; i++) { 38 | const std::string arg = argv[i]; 39 | if (arg == "--visualize") { 40 | params.visualize = true; 41 | } else if (arg == "--num_threads") { 42 | params.num_threads = std::stoi(argv[i + 1]); 43 | } else if (arg == "--num_neighbors") { 44 | params.num_neighbors = std::stoi(argv[i + 1]); 45 | } else if (arg == "--downsampling_resolution") { 46 | params.downsampling_resolution = std::stod(argv[i + 1]); 47 | } else if (arg == "--voxel_resolution") { 48 | params.voxel_resolution = std::stod(argv[i + 1]); 49 | } else if (arg == "--engine") { 50 | engine = argv[i + 1]; 51 | } else if (arg.size() >= 2 && arg.substr(0, 2) == "--") { 52 | std::cerr << "unknown option: " << arg << std::endl; 53 | return 1; 54 | } 55 | } 56 | 57 | std::cout << "SIMD in use=" << Eigen::SimdInstructionSetsInUse() << std::endl; 58 | std::cout << "dataset_path=" << dataset_path << std::endl; 59 | std::cout << "output_path=" << output_path << std::endl; 60 | std::cout << "registration_engine=" << engine << std::endl; 61 | std::cout << "num_threads=" << params.num_threads << std::endl; 62 | std::cout << "num_neighbors=" << params.num_neighbors << std::endl; 63 | std::cout << "downsampling_resolution=" << params.downsampling_resolution << std::endl; 64 | std::cout << "voxel_resolution=" << params.voxel_resolution << std::endl; 65 | std::cout << "visualize=" << params.visualize << std::endl; 66 | 67 | std::shared_ptr odom = create_odometry(engine, params); 68 | if (odom == nullptr) { 69 | return 1; 70 | } 71 | 72 | KittiDataset kitti(dataset_path); 73 | std::cout << "num_frames=" << kitti.points.size() << std::endl; 74 | std::cout << fmt::format("num_points={} [points]", summarize(kitti.points, [](const auto& pts) { return pts.size(); })) << std::endl; 75 | 76 | auto raw_points = kitti.convert(true); 77 | std::vector traj = odom->estimate(raw_points); 78 | 79 | std::cout << "done!" << std::endl; 80 | odom->report(); 81 | 82 | std::ofstream ofs(output_path); 83 | for (const auto& T : traj) { 84 | for (int i = 0; i < 3; i++) { 85 | for (int j = 0; j < 4; j++) { 86 | if (i || j) { 87 | ofs << " "; 88 | } 89 | 90 | ofs << fmt::format("{:.6f}", T(i, j)); 91 | } 92 | } 93 | ofs << std::endl; 94 | } 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_fast_gicp.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_FAST_GICP 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace small_gicp { 14 | 15 | class FastGICPOdometryEstimation : public OnlineOdometryEstimation { 16 | public: 17 | explicit FastGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { 18 | gicp.setCorrespondenceRandomness(params.num_neighbors); 19 | gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); 20 | gicp.setNumThreads(params.num_threads); 21 | } 22 | 23 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 24 | auto points_pcl = pcl::make_shared>(); 25 | points_pcl->resize(points->size()); 26 | for (size_t i = 0; i < points->size(); i++) { 27 | points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); 28 | } 29 | 30 | Stopwatch sw; 31 | sw.start(); 32 | 33 | if (!gicp.getInputTarget()) { 34 | gicp.setInputTarget(points_pcl); 35 | return T; 36 | } 37 | 38 | gicp.setInputSource(points_pcl); 39 | pcl::PointCloud aligned; 40 | gicp.align(aligned); 41 | 42 | sw.stop(); 43 | reg_times.push(sw.msec()); 44 | 45 | T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); 46 | gicp.swapSourceAndTarget(); 47 | 48 | return T; 49 | } 50 | 51 | void report() override { // 52 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 53 | } 54 | 55 | private: 56 | Summarizer reg_times; 57 | 58 | fast_gicp::FastGICP gicp; 59 | Eigen::Isometry3d T; 60 | }; 61 | 62 | static auto fast_gicp_registry = register_odometry("fast_gicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 63 | 64 | } // namespace small_gicp 65 | 66 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_fast_vgicp.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_FAST_GICP 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace small_gicp { 16 | 17 | class FastVGICPOdometryEstimation : public OnlineOdometryEstimation { 18 | public: 19 | explicit FastVGICPOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { 20 | vgicp.setCorrespondenceRandomness(params.num_neighbors); 21 | vgicp.setResolution(params.voxel_resolution); 22 | vgicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); 23 | vgicp.setNumThreads(params.num_threads); 24 | } 25 | 26 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 27 | auto points_pcl = pcl::make_shared>(); 28 | points_pcl->resize(points->size()); 29 | for (size_t i = 0; i < points->size(); i++) { 30 | points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); 31 | } 32 | 33 | Stopwatch sw; 34 | sw.start(); 35 | 36 | if (!vgicp.getInputTarget()) { 37 | vgicp.setInputTarget(points_pcl); 38 | return T; 39 | } 40 | 41 | vgicp.setInputSource(points_pcl); 42 | pcl::PointCloud aligned; 43 | vgicp.align(aligned); 44 | 45 | sw.stop(); 46 | reg_times.push(sw.msec()); 47 | 48 | T = T * Eigen::Isometry3d(vgicp.getFinalTransformation().cast()); 49 | vgicp.swapSourceAndTarget(); 50 | 51 | return T; 52 | } 53 | 54 | void report() override { // 55 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 56 | } 57 | 58 | private: 59 | Summarizer reg_times; 60 | 61 | fast_gicp::FastVGICP vgicp; 62 | Eigen::Isometry3d T; 63 | }; 64 | 65 | static auto fast_vgicp_registry = register_odometry("fast_vgicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 66 | 67 | } // namespace small_gicp 68 | 69 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_pcl.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_PCL 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace small_gicp { 10 | 11 | class PCLOnlineOdometryEstimation : public OnlineOdometryEstimation { 12 | public: 13 | explicit PCLOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { 14 | gicp.setCorrespondenceRandomness(params.num_neighbors); 15 | gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); 16 | } 17 | 18 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 19 | auto points_pcl = pcl::make_shared>(); 20 | points_pcl->resize(points->size()); 21 | for (size_t i = 0; i < points->size(); i++) { 22 | points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); 23 | } 24 | 25 | Stopwatch sw; 26 | sw.start(); 27 | 28 | if (!target_points) { 29 | target_points = points_pcl; 30 | return Eigen::Isometry3d::Identity(); 31 | } 32 | 33 | gicp.setInputTarget(target_points); 34 | gicp.setInputSource(points_pcl); 35 | pcl::PointCloud aligned; 36 | gicp.align(aligned); 37 | 38 | sw.stop(); 39 | reg_times.push(sw.msec()); 40 | 41 | T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); 42 | target_points = points_pcl; 43 | 44 | return T; 45 | } 46 | 47 | void report() override { // 48 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 49 | } 50 | 51 | private: 52 | Summarizer reg_times; 53 | 54 | pcl::GeneralizedIterativeClosestPoint gicp; 55 | pcl::PointCloud::Ptr target_points; 56 | Eigen::Isometry3d T; 57 | }; 58 | 59 | static auto pcl_odom_registry = register_odometry("pcl", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 60 | 61 | } // namespace small_gicp 62 | 63 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | class SmallGICPOnlineOdometryEstimation : public OnlineOdometryEstimation { 13 | public: 14 | explicit SmallGICPOnlineOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} 15 | 16 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 17 | Stopwatch sw; 18 | sw.start(); 19 | 20 | auto tree = std::make_shared>(points); 21 | estimate_covariances(*points, *tree, params.num_neighbors); 22 | 23 | if (target_points == nullptr) { 24 | target_points = points; 25 | target_tree = tree; 26 | return T; 27 | } 28 | 29 | Registration registration; 30 | registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; 31 | 32 | auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); 33 | 34 | sw.stop(); 35 | reg_times.push(sw.msec()); 36 | 37 | T = T * result.T_target_source; 38 | target_points = points; 39 | target_tree = tree; 40 | 41 | return T; 42 | } 43 | 44 | void report() override { // 45 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 46 | } 47 | 48 | private: 49 | Summarizer reg_times; 50 | 51 | PointCloud::Ptr target_points; 52 | KdTree::Ptr target_tree; 53 | 54 | Eigen::Isometry3d T; 55 | }; 56 | 57 | static auto small_gicp_registry = 58 | register_odometry("small_gicp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 59 | 60 | } // namespace small_gicp -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp_model_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | class SmallGICPModelOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { 13 | public: 14 | explicit SmallGICPModelOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {} 15 | 16 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 17 | Stopwatch sw; 18 | sw.start(); 19 | 20 | // Note that input points here is already downsampled 21 | // Estimate per-point covariances 22 | estimate_covariances_omp(*points, params.num_neighbors, params.num_threads); 23 | 24 | if (voxelmap == nullptr) { 25 | // This is the very first frame 26 | voxelmap = std::make_shared>(params.voxel_resolution); 27 | voxelmap->insert(*points); 28 | return T_world_lidar; 29 | } 30 | 31 | // Registration using GICP + OMP-based parallel reduction 32 | Registration registration; 33 | registration.reduction.num_threads = params.num_threads; 34 | auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar); 35 | 36 | // Update T_world_lidar with the estimated transformation 37 | T_world_lidar = result.T_target_source; 38 | 39 | // Insert points to the target voxel map 40 | voxelmap->insert(*points, T_world_lidar); 41 | 42 | sw.stop(); 43 | reg_times.push(sw.msec()); 44 | 45 | if (params.visualize) { 46 | update_model_points(T_world_lidar, traits::voxel_points(*voxelmap)); 47 | } 48 | 49 | return T_world_lidar; 50 | } 51 | 52 | void report() override { // 53 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 54 | } 55 | 56 | private: 57 | Summarizer reg_times; 58 | 59 | IncrementalVoxelMap::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds 60 | Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation 61 | }; 62 | 63 | static auto small_gicp_model_omp_registry = 64 | register_odometry("small_gicp_model_omp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 65 | 66 | } // namespace small_gicp 67 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp_model_tbb.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_TBB 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace small_gicp { 14 | 15 | class SmallGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { 16 | public: 17 | explicit SmallGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) 18 | : OnlineOdometryEstimation(params), 19 | control(tbb::global_control::max_allowed_parallelism, params.num_threads), 20 | T_world_lidar(Eigen::Isometry3d::Identity()) {} 21 | 22 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 23 | Stopwatch sw; 24 | sw.start(); 25 | 26 | // Note that input points here is already downsampled 27 | // Estimate per-point covariances 28 | estimate_covariances_tbb(*points, params.num_neighbors); 29 | 30 | if (voxelmap == nullptr) { 31 | // This is the very first frame 32 | voxelmap = std::make_shared>(params.voxel_resolution); 33 | voxelmap->insert(*points); 34 | return T_world_lidar; 35 | } 36 | 37 | // Registration using GICP + TBB-based parallel reduction 38 | Registration registration; 39 | auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar); 40 | 41 | // Update T_world_lidar with the estimated transformation 42 | T_world_lidar = result.T_target_source; 43 | 44 | // Insert points to the target voxel map 45 | voxelmap->insert(*points, T_world_lidar); 46 | 47 | sw.stop(); 48 | reg_times.push(sw.msec()); 49 | 50 | if (params.visualize) { 51 | update_model_points(T_world_lidar, traits::voxel_points(*voxelmap)); 52 | } 53 | 54 | return T_world_lidar; 55 | } 56 | 57 | void report() override { // 58 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 59 | } 60 | 61 | private: 62 | tbb::global_control control; 63 | 64 | Summarizer reg_times; 65 | 66 | IncrementalVoxelMap::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds 67 | Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation 68 | }; 69 | 70 | static auto small_gicp_model_tbb_registry = 71 | register_odometry("small_gicp_model_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 72 | 73 | } // namespace small_gicp 74 | 75 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | class SmallGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { 13 | public: 14 | explicit SmallGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {} 15 | 16 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 17 | Stopwatch sw; 18 | sw.start(); 19 | 20 | // Preprocess input points (kdtree construction & covariance estimation) 21 | // Note that input points here is already downsampled 22 | auto tree = std::make_shared>(points, KdTreeBuilderOMP(params.num_threads)); 23 | estimate_covariances_omp(*points, *tree, params.num_neighbors, params.num_threads); 24 | 25 | if (target_points == nullptr) { 26 | // This is the very first frame 27 | target_points = points; 28 | target_tree = tree; 29 | return T_world_lidar; 30 | } 31 | 32 | // Registration using GICP + OMP-based parallel reduction 33 | Registration registration; 34 | registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; 35 | registration.reduction.num_threads = params.num_threads; 36 | 37 | // Perform registration 38 | auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); 39 | 40 | // Update T_world_lidar and target points 41 | T_world_lidar = T_world_lidar * result.T_target_source; 42 | target_points = points; 43 | target_tree = tree; 44 | 45 | sw.stop(); 46 | reg_times.push(sw.msec()); 47 | 48 | return T_world_lidar; 49 | } 50 | 51 | void report() override { // 52 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 53 | } 54 | 55 | private: 56 | Summarizer reg_times; 57 | 58 | PointCloud::Ptr target_points; // Last point cloud to be registration target 59 | KdTree::Ptr target_tree; // KdTree of the last point cloud 60 | 61 | Eigen::Isometry3d T_world_lidar; // T_world_lidar 62 | }; 63 | 64 | static auto small_gicp_omp_registry = 65 | register_odometry("small_gicp_omp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 66 | 67 | } // namespace small_gicp 68 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp_pcl.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_PCL 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace small_gicp { 12 | 13 | class SmallGICPPCLOdometryEstimation : public OnlineOdometryEstimation { 14 | public: 15 | explicit SmallGICPPCLOdometryEstimation(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) { 16 | gicp.setCorrespondenceRandomness(params.num_neighbors); 17 | gicp.setMaxCorrespondenceDistance(params.max_correspondence_distance); 18 | gicp.setNumThreads(params.num_threads); 19 | } 20 | 21 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 22 | auto points_pcl = pcl::make_shared>(); 23 | points_pcl->resize(points->size()); 24 | for (size_t i = 0; i < points->size(); i++) { 25 | points_pcl->at(i).getVector4fMap() = points->point(i).template cast(); 26 | } 27 | 28 | Stopwatch sw; 29 | sw.start(); 30 | 31 | if (!gicp.getInputTarget()) { 32 | gicp.setInputTarget(points_pcl); 33 | return T; 34 | } 35 | 36 | gicp.setInputSource(points_pcl); 37 | pcl::PointCloud aligned; 38 | gicp.align(aligned); 39 | 40 | sw.stop(); 41 | reg_times.push(sw.msec()); 42 | 43 | T = T * Eigen::Isometry3d(gicp.getFinalTransformation().cast()); 44 | gicp.swapSourceAndTarget(); 45 | 46 | return T; 47 | } 48 | 49 | void report() override { // 50 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 51 | } 52 | 53 | private: 54 | Summarizer reg_times; 55 | 56 | small_gicp::RegistrationPCL gicp; 57 | Eigen::Isometry3d T; 58 | }; 59 | 60 | static auto small_gicp_pcl_registry = 61 | register_odometry("small_gicp_pcl", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 62 | 63 | } // namespace small_gicp 64 | 65 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_gicp_tbb.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_TBB 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace small_gicp { 14 | 15 | class SmallGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { 16 | public: 17 | explicit SmallGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) 18 | : OnlineOdometryEstimation(params), 19 | control(tbb::global_control::max_allowed_parallelism, params.num_threads), 20 | T(Eigen::Isometry3d::Identity()) {} 21 | 22 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 23 | Stopwatch sw; 24 | sw.start(); 25 | 26 | auto tree = std::make_shared>(points, KdTreeBuilderTBB()); 27 | estimate_covariances_tbb(*points, *tree, params.num_neighbors); 28 | 29 | if (target_points == nullptr) { 30 | target_points = points; 31 | target_tree = tree; 32 | return T; 33 | } 34 | 35 | Registration registration; 36 | registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; 37 | 38 | auto result = registration.align(*target_points, *points, *target_tree, Eigen::Isometry3d::Identity()); 39 | 40 | sw.stop(); 41 | reg_times.push(sw.msec()); 42 | 43 | T = T * result.T_target_source; 44 | target_points = points; 45 | target_tree = tree; 46 | 47 | return T; 48 | } 49 | 50 | void report() override { // 51 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 52 | } 53 | 54 | private: 55 | tbb::global_control control; 56 | 57 | Summarizer reg_times; 58 | 59 | PointCloud::Ptr target_points; 60 | KdTree::Ptr target_tree; 61 | 62 | Eigen::Isometry3d T; 63 | }; 64 | 65 | static auto small_gicp_tbb_registry = 66 | register_odometry("small_gicp_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 67 | 68 | } // namespace small_gicp 69 | 70 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_vgicp_model_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace small_gicp { 11 | 12 | class SmallVGICPModelOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { 13 | public: 14 | explicit SmallVGICPModelOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T_world_lidar(Eigen::Isometry3d::Identity()) {} 15 | 16 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 17 | Stopwatch sw; 18 | sw.start(); 19 | 20 | // Note that input points here is already downsampled 21 | // Estimate per-point covariances 22 | estimate_covariances_omp(*points, params.num_neighbors, params.num_threads); 23 | 24 | if (voxelmap == nullptr) { 25 | // This is the very first frame 26 | voxelmap = std::make_shared(params.voxel_resolution); 27 | voxelmap->insert(*points); 28 | return T_world_lidar; 29 | } 30 | 31 | // Registration using GICP + OMP-based parallel reduction 32 | Registration registration; 33 | registration.reduction.num_threads = params.num_threads; 34 | auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar); 35 | 36 | // Update T_world_lidar with the estimated transformation 37 | T_world_lidar = result.T_target_source; 38 | 39 | // Insert points to the target voxel map 40 | voxelmap->insert(*points, T_world_lidar); 41 | 42 | sw.stop(); 43 | reg_times.push(sw.msec()); 44 | 45 | if (params.visualize) { 46 | update_model_points(T_world_lidar, traits::voxel_points(*voxelmap)); 47 | } 48 | 49 | return T_world_lidar; 50 | } 51 | 52 | void report() override { // 53 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 54 | } 55 | 56 | private: 57 | Summarizer reg_times; 58 | 59 | GaussianVoxelMap::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds 60 | Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation 61 | }; 62 | 63 | static auto small_gicp_model_omp_registry = 64 | register_odometry("small_vgicp_model_omp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 65 | 66 | } // namespace small_gicp 67 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_vgicp_model_tbb.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_TBB 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace small_gicp { 14 | 15 | class SmallVGICPModelOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { 16 | public: 17 | explicit SmallVGICPModelOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) 18 | : OnlineOdometryEstimation(params), 19 | control(tbb::global_control::max_allowed_parallelism, params.num_threads), 20 | T_world_lidar(Eigen::Isometry3d::Identity()) {} 21 | 22 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 23 | Stopwatch sw; 24 | sw.start(); 25 | 26 | // Note that input points here is already downsampled 27 | // Estimate per-point covariances 28 | estimate_covariances_tbb(*points, params.num_neighbors); 29 | 30 | if (voxelmap == nullptr) { 31 | // This is the very first frame 32 | voxelmap = std::make_shared(params.voxel_resolution); 33 | voxelmap->insert(*points); 34 | return T_world_lidar; 35 | } 36 | 37 | // Registration using GICP + TBB-based parallel reduction 38 | Registration registration; 39 | auto result = registration.align(*voxelmap, *points, *voxelmap, T_world_lidar); 40 | 41 | // Update T_world_lidar with the estimated transformation 42 | T_world_lidar = result.T_target_source; 43 | 44 | // Insert points to the target voxel map 45 | voxelmap->insert(*points, T_world_lidar); 46 | 47 | sw.stop(); 48 | reg_times.push(sw.msec()); 49 | 50 | if (params.visualize) { 51 | update_model_points(T_world_lidar, traits::voxel_points(*voxelmap)); 52 | } 53 | 54 | return T_world_lidar; 55 | } 56 | 57 | void report() override { // 58 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 59 | } 60 | 61 | private: 62 | tbb::global_control control; 63 | 64 | Summarizer reg_times; 65 | 66 | GaussianVoxelMap::Ptr voxelmap; // Target voxel map that is an accumulation of past point clouds 67 | Eigen::Isometry3d T_world_lidar; // Current world-to-lidar transformation 68 | }; 69 | 70 | static auto small_gicp_model_tbb_registry = 71 | register_odometry("small_vgicp_model_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 72 | 73 | } // namespace small_gicp 74 | 75 | #endif -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_vgicp_omp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace small_gicp { 12 | 13 | class SmallVGICPOnlineOdometryEstimationOMP : public OnlineOdometryEstimation { 14 | public: 15 | explicit SmallVGICPOnlineOdometryEstimationOMP(const OdometryEstimationParams& params) : OnlineOdometryEstimation(params), T(Eigen::Isometry3d::Identity()) {} 16 | 17 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 18 | Stopwatch sw; 19 | sw.start(); 20 | 21 | auto tree = std::make_shared>(points, KdTreeBuilderOMP(params.num_threads)); 22 | estimate_covariances_omp(*points, *tree, params.num_neighbors, params.num_threads); 23 | 24 | auto voxelmap = std::make_shared(params.voxel_resolution); 25 | voxelmap->insert(*points); 26 | 27 | if (target_points == nullptr) { 28 | target_points = points; 29 | target_voxelmap = voxelmap; 30 | return T; 31 | } 32 | 33 | Registration registration; 34 | registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; 35 | 36 | auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity()); 37 | 38 | sw.stop(); 39 | reg_times.push(sw.msec()); 40 | 41 | T = T * result.T_target_source; 42 | 43 | target_points = points; 44 | target_voxelmap = voxelmap; 45 | 46 | return T; 47 | } 48 | 49 | void report() override { // 50 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 51 | } 52 | 53 | private: 54 | Summarizer reg_times; 55 | 56 | PointCloud::Ptr target_points; 57 | GaussianVoxelMap::Ptr target_voxelmap; 58 | 59 | Eigen::Isometry3d T; 60 | }; 61 | 62 | static auto small_vgicp_omp_registry = 63 | register_odometry("small_vgicp_omp", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 64 | 65 | } // namespace small_gicp 66 | -------------------------------------------------------------------------------- /src/benchmark/odometry_benchmark_small_vgicp_tbb.cpp: -------------------------------------------------------------------------------- 1 | #ifdef BUILD_WITH_TBB 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace small_gicp { 14 | 15 | class SmallVGICPOnlineOdometryEstimationTBB : public OnlineOdometryEstimation { 16 | public: 17 | explicit SmallVGICPOnlineOdometryEstimationTBB(const OdometryEstimationParams& params) 18 | : OnlineOdometryEstimation(params), 19 | control(tbb::global_control::max_allowed_parallelism, params.num_threads), 20 | T(Eigen::Isometry3d::Identity()) {} 21 | 22 | Eigen::Isometry3d estimate(const PointCloud::Ptr& points) override { 23 | Stopwatch sw; 24 | sw.start(); 25 | 26 | estimate_covariances_tbb(*points, params.num_neighbors); 27 | 28 | auto voxelmap = std::make_shared(params.voxel_resolution); 29 | voxelmap->insert(*points); 30 | 31 | if (target_points == nullptr) { 32 | target_points = points; 33 | target_voxelmap = voxelmap; 34 | return T; 35 | } 36 | 37 | Registration registration; 38 | registration.rejector.max_dist_sq = params.max_correspondence_distance * params.max_correspondence_distance; 39 | 40 | auto result = registration.align(*target_voxelmap, *points, *target_voxelmap, Eigen::Isometry3d::Identity()); 41 | 42 | sw.stop(); 43 | reg_times.push(sw.msec()); 44 | 45 | T = T * result.T_target_source; 46 | 47 | target_points = points; 48 | target_voxelmap = voxelmap; 49 | 50 | return T; 51 | } 52 | 53 | void report() override { // 54 | std::cout << "registration_time_stats=" << reg_times.str() << " [msec/scan] total_throughput=" << total_times.str() << " [msec/scan]" << std::endl; 55 | } 56 | 57 | private: 58 | tbb::global_control control; 59 | 60 | Summarizer reg_times; 61 | 62 | PointCloud::Ptr target_points; 63 | GaussianVoxelMap::Ptr target_voxelmap; 64 | 65 | Eigen::Isometry3d T; 66 | }; 67 | 68 | static auto small_gicp_tbb_registry = 69 | register_odometry("small_vgicp_tbb", [](const OdometryEstimationParams& params) { return std::make_shared(params); }); 70 | 71 | } // namespace small_gicp 72 | 73 | #endif -------------------------------------------------------------------------------- /src/example/01_basic_registration.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | 4 | /// @brief Basic point cloud registration example with small_gicp::align() 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace small_gicp; 10 | 11 | /// @brief Most basic registration example. 12 | void example1(const std::vector& target_points, const std::vector& source_points) { 13 | RegistrationSetting setting; 14 | setting.num_threads = 4; // Number of threads to be used 15 | setting.downsampling_resolution = 0.25; // Downsampling resolution 16 | setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold) 17 | 18 | Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity(); 19 | RegistrationResult result = align(target_points, source_points, init_T_target_source, setting); 20 | 21 | std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl; 22 | std::cout << "converged:" << result.converged << std::endl; 23 | std::cout << "error:" << result.error << std::endl; 24 | std::cout << "iterations:" << result.iterations << std::endl; 25 | std::cout << "num_inliers:" << result.num_inliers << std::endl; 26 | std::cout << "--- H ---" << std::endl << result.H << std::endl; 27 | std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl; 28 | } 29 | 30 | /// @brief Example to perform preprocessing and registration separately. 31 | void example2(const std::vector& target_points, const std::vector& source_points) { 32 | int num_threads = 4; // Number of threads to be used 33 | double downsampling_resolution = 0.25; // Downsampling resolution 34 | int num_neighbors = 10; // Number of neighbor points used for normal and covariance estimation 35 | 36 | // std::pair::Ptr> 37 | auto [target, target_tree] = preprocess_points(target_points, downsampling_resolution, num_neighbors, num_threads); 38 | auto [source, source_tree] = preprocess_points(source_points, downsampling_resolution, num_neighbors, num_threads); 39 | 40 | RegistrationSetting setting; 41 | setting.num_threads = num_threads; 42 | setting.max_correspondence_distance = 1.0; // Maximum correspondence distance between points (e.g., triming threshold) 43 | 44 | Eigen::Isometry3d init_T_target_source = Eigen::Isometry3d::Identity(); 45 | RegistrationResult result = align(*target, *source, *target_tree, init_T_target_source, setting); 46 | 47 | std::cout << "--- T_target_source ---" << std::endl << result.T_target_source.matrix() << std::endl; 48 | std::cout << "converged:" << result.converged << std::endl; 49 | std::cout << "error:" << result.error << std::endl; 50 | std::cout << "iterations:" << result.iterations << std::endl; 51 | std::cout << "num_inliers:" << result.num_inliers << std::endl; 52 | std::cout << "--- H ---" << std::endl << result.H << std::endl; 53 | std::cout << "--- b ---" << std::endl << result.b.transpose() << std::endl; 54 | 55 | // Preprocessed points and trees can be reused for the next registration for efficiency 56 | RegistrationResult result2 = align(*source, *target, *source_tree, Eigen::Isometry3d::Identity(), setting); 57 | } 58 | 59 | int main(int argc, char** argv) { 60 | std::vector target_points = read_ply("data/target.ply"); 61 | std::vector source_points = read_ply("data/source.ply"); 62 | if (target_points.empty() || source_points.empty()) { 63 | std::cerr << "error: failed to read points from data/(target|source).ply" << std::endl; 64 | return 1; 65 | } 66 | 67 | example1(target_points, source_points); 68 | example2(target_points, source_points); 69 | 70 | return 0; 71 | } -------------------------------------------------------------------------------- /src/example/kitti_odometry.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import os 3 | import time 4 | import argparse 5 | import collections 6 | import numpy 7 | import small_gicp 8 | from pyridescence import * 9 | 10 | # Odometry estimation based on scan-to-scan matching 11 | class ScanToScanMatchingOdometry(object): 12 | def __init__(self, num_threads): 13 | self.num_threads = num_threads 14 | self.T_last_current = numpy.identity(4) 15 | self.T_world_lidar = numpy.identity(4) 16 | self.target = None 17 | 18 | def estimate(self, raw_points): 19 | downsampled, tree = small_gicp.preprocess_points(raw_points, 0.25, num_threads=self.num_threads) 20 | 21 | if self.target is None: 22 | self.target = (downsampled, tree) 23 | return self.T_world_lidar 24 | 25 | result = small_gicp.align(self.target[0], downsampled, self.target[1], self.T_last_current, num_threads=self.num_threads) 26 | 27 | self.T_last_current = result.T_target_source 28 | self.T_world_lidar = self.T_world_lidar @ result.T_target_source 29 | self.target = (downsampled, tree) 30 | 31 | return self.T_world_lidar 32 | 33 | # Odometry estimation based on scan-to-model matching 34 | class ScanToModelMatchingOdometry(object): 35 | def __init__(self, num_threads): 36 | self.num_threads = num_threads 37 | self.T_last_current = numpy.identity(4) 38 | self.T_world_lidar = numpy.identity(4) 39 | self.target = small_gicp.GaussianVoxelMap(1.0) 40 | self.target.set_lru(horizon=100, clear_cycle=10) 41 | 42 | def estimate(self, raw_points): 43 | downsampled, tree = small_gicp.preprocess_points(raw_points, 0.25, num_threads=self.num_threads) 44 | 45 | if self.target.size() == 0: 46 | self.target.insert(downsampled) 47 | return self.T_world_lidar 48 | 49 | result = small_gicp.align(self.target, downsampled, self.T_world_lidar @ self.T_last_current, num_threads=self.num_threads) 50 | 51 | self.T_last_current = numpy.linalg.inv(self.T_world_lidar) @ result.T_target_source 52 | self.T_world_lidar = result.T_target_source 53 | self.target.insert(downsampled, self.T_world_lidar) 54 | 55 | guik.viewer().update_drawable('target', glk.create_pointcloud_buffer(self.target.voxel_points()[:, :3]), guik.Rainbow()) 56 | 57 | return self.T_world_lidar 58 | 59 | 60 | def main(): 61 | parser = argparse.ArgumentParser() 62 | parser.add_argument('dataset_path', help='/path/to/kitti/velodyne') 63 | parser.add_argument('--num_threads', help='Number of threads', type=int, default=4) 64 | parser.add_argument('-m', '--model', help='Use scan-to-model matching odometry', action='store_true') 65 | args = parser.parse_args() 66 | 67 | dataset_path = args.dataset_path 68 | filenames = sorted([dataset_path + '/' + x for x in os.listdir(dataset_path) if x.endswith('.bin')]) 69 | 70 | if not args.model: 71 | odom = ScanToScanMatchingOdometry(args.num_threads) 72 | else: 73 | odom = ScanToModelMatchingOdometry(args.num_threads) 74 | 75 | viewer = guik.viewer() 76 | viewer.disable_vsync() 77 | time_queue = collections.deque(maxlen=500) 78 | 79 | for i, filename in enumerate(filenames): 80 | raw_points = numpy.fromfile(filename, dtype=numpy.float32).reshape(-1, 4)[:, :3] 81 | 82 | t1 = time.time() 83 | T = odom.estimate(raw_points) 84 | t2 = time.time() 85 | 86 | time_queue.append(t2 - t1) 87 | viewer.lookat(T[:3, 3]) 88 | viewer.update_drawable('points', glk.create_pointcloud_buffer(raw_points), guik.FlatOrange(T).add('point_scale', 2.0)) 89 | 90 | if i % 10 == 0: 91 | viewer.update_drawable('pos_{}'.format(i), glk.primitives.coordinate_system(), guik.VertexColor(T)) 92 | viewer.append_text('avg={:.3f} msec/scan last={:.3f} msec/scan'.format(1000 * numpy.mean(time_queue), 1000 * time_queue[-1])) 93 | 94 | if not viewer.spin_once(): 95 | break 96 | 97 | if __name__ == '__main__': 98 | main() 99 | -------------------------------------------------------------------------------- /src/python/misc.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace py = pybind11; 15 | using namespace small_gicp; 16 | 17 | void define_misc(py::module& m) { 18 | // read_ply 19 | m.def( 20 | "read_ply", 21 | [](const std::string& filename) { 22 | const auto points = read_ply(filename); 23 | return std::make_shared(points); 24 | }, 25 | "Read PLY file. This function can only read simple point clouds with XYZ properties for testing purposes. Do not use this for general PLY IO.", 26 | py::arg("filename")); 27 | } -------------------------------------------------------------------------------- /src/python/pointcloud.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace py = pybind11; 14 | using namespace small_gicp; 15 | 16 | void define_pointcloud(py::module& m) { 17 | // PointCloud 18 | py::class_>(m, "PointCloud") // 19 | .def( 20 | py::init([](const Eigen::MatrixXd& points) { 21 | if (points.cols() != 3 && points.cols() != 4) { 22 | std::cerr << "points must be Nx3 or Nx4" << std::endl; 23 | return std::make_shared(); 24 | } 25 | 26 | auto pc = std::make_shared(); 27 | pc->resize(points.rows()); 28 | if (points.cols() == 3) { 29 | for (size_t i = 0; i < points.rows(); i++) { 30 | pc->point(i) << points.row(i).transpose(), 1.0; 31 | } 32 | } else { 33 | for (size_t i = 0; i < points.rows(); i++) { 34 | pc->point(i) << points.row(i).transpose(); 35 | } 36 | } 37 | 38 | return pc; 39 | }), 40 | py::arg("points"), 41 | R"""( 42 | PointCloud(points: numpy.ndarray) 43 | 44 | Construct a PointCloud from a numpy array. 45 | 46 | Parameters 47 | ---------- 48 | points : numpy.ndarray, shape (n, 3) or (n, 4) 49 | The input point cloud. 50 | )""") 51 | .def("__repr__", [](const PointCloud& points) { return "small_gicp.PointCloud (" + std::to_string(points.size()) + " points)"; }) 52 | .def("__len__", [](const PointCloud& points) { return points.size(); }) 53 | .def( 54 | "empty", 55 | &PointCloud::empty, 56 | R"pbdoc( 57 | Check if the point cloud is empty 58 | 59 | Returns 60 | ------- 61 | empty : bool 62 | True if the point cloud is empty. 63 | )pbdoc") 64 | .def( 65 | "size", 66 | &PointCloud::size, 67 | R"pbdoc( 68 | Get the number of points. 69 | 70 | Returns 71 | ------- 72 | num_points : int 73 | Number of points. 74 | )pbdoc") 75 | .def( 76 | "points", 77 | [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.points[0].data(), points.size(), 4); }, 78 | R"pbdoc( 79 | Get the points as a Nx4 matrix. 80 | 81 | Returns 82 | ------- 83 | points : numpy.ndarray 84 | Points. 85 | )pbdoc") 86 | .def( 87 | "normals", 88 | [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.normals[0].data(), points.size(), 4); }, 89 | R"pbdoc( 90 | Get the normals as a Nx4 matrix. 91 | 92 | Returns 93 | ------- 94 | normals : numpy.ndarray 95 | Normals. 96 | )pbdoc") 97 | .def( 98 | "covs", 99 | [](const PointCloud& points) { return points.covs; }, 100 | R"pbdoc( 101 | Get the covariance matrices as a list of 4x4 matrices. 102 | 103 | Returns 104 | ------- 105 | covs : list of numpy.ndarray 106 | Covariance matrices. 107 | )pbdoc") 108 | .def( 109 | "point", 110 | [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); }, 111 | py::arg("i"), 112 | R"pbdoc( 113 | Get the i-th point. 114 | 115 | Parameters 116 | ---------- 117 | i : int 118 | Index of the point. 119 | 120 | Returns 121 | ------- 122 | point : numpy.ndarray, shape (4,) 123 | Point. 124 | )pbdoc") 125 | .def( 126 | "normal", 127 | [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); }, 128 | py::arg("i"), 129 | R"pbdoc( 130 | Get the i-th normal. 131 | 132 | Parameters 133 | ---------- 134 | i : int 135 | Index of the point. 136 | 137 | Returns 138 | ------- 139 | normal : numpy.ndarray, shape (4,) 140 | Normal. 141 | )pbdoc") 142 | .def( 143 | "cov", 144 | [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); }, 145 | py::arg("i"), 146 | R"pbdoc( 147 | Get the i-th covariance matrix. 148 | 149 | Parameters 150 | ---------- 151 | i : int 152 | Index of the point. 153 | 154 | Returns 155 | ------- 156 | cov : numpy.ndarray, shape (4, 4) 157 | Covariance matrix. 158 | )pbdoc"); 159 | } -------------------------------------------------------------------------------- /src/python/python.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | namespace py = pybind11; 6 | 7 | void define_pointcloud(py::module& m); 8 | void define_kdtree(py::module& m); 9 | void define_voxelmap(py::module& m); 10 | void define_preprocess(py::module& m); 11 | void define_result(py::module& m); 12 | void define_align(py::module& m); 13 | void define_factors(py::module& m); 14 | void define_misc(py::module& m); 15 | 16 | PYBIND11_MODULE(small_gicp, m) { 17 | m.doc() = "Efficient and parallel algorithms for point cloud registration"; 18 | 19 | define_pointcloud(m); 20 | define_kdtree(m); 21 | define_voxelmap(m); 22 | define_preprocess(m); 23 | define_result(m); 24 | define_align(m); 25 | define_factors(m); 26 | define_misc(m); 27 | } -------------------------------------------------------------------------------- /src/python/result.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace py = pybind11; 14 | using namespace small_gicp; 15 | 16 | void define_result(py::module& m) { 17 | // RegistrationResult 18 | py::class_(m, "RegistrationResult") // 19 | .def( 20 | "__repr__", 21 | [](const RegistrationResult& result) { 22 | std::stringstream sst; 23 | sst << "small_gicp.RegistrationResult\n"; 24 | sst << "converted=" << result.converged << "\n"; 25 | sst << "iterations=" << result.iterations << "\n"; 26 | sst << "num_inliers=" << result.num_inliers << "\n"; 27 | sst << "T_target_source=\n" << result.T_target_source.matrix() << "\n"; 28 | sst << "H=\n" << result.H << "\n"; 29 | sst << "b=\n" << result.b.transpose() << "\n"; 30 | sst << "error= " << result.error << "\n"; 31 | return sst.str(); 32 | }) 33 | .def_property_readonly( 34 | "T_target_source", 35 | [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); }, 36 | "NDArray[np.float64] : Final transformation matrix (4x4). This transformation brings a point in the source cloud frame to the target cloud frame.") 37 | .def_readonly("converged", &RegistrationResult::converged, "bool : Convergence flag") 38 | .def_readonly("iterations", &RegistrationResult::iterations, "int : Number of iterations") 39 | .def_readonly("num_inliers", &RegistrationResult::num_inliers, "int : Number of inliers") 40 | .def_readonly("H", &RegistrationResult::H, "NDArray[np.float64] : Final Hessian matrix (6x6)") 41 | .def_readonly("b", &RegistrationResult::b, "NDArray[np.float64] : Final information vector (6,)") 42 | .def_readonly("error", &RegistrationResult::error, "float : Final error"); 43 | } -------------------------------------------------------------------------------- /src/small_gicp/benchmark/benchmark_odom.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | namespace small_gicp { 6 | 7 | std::vector>> odometry_registry; 8 | 9 | size_t register_odometry(const std::string& name, std::function factory) { 10 | odometry_registry.emplace_back(name, factory); 11 | return odometry_registry.size() - 1; 12 | } 13 | 14 | std::vector odometry_names() { 15 | std::vector names(odometry_registry.size()); 16 | std::transform(odometry_registry.begin(), odometry_registry.end(), names.begin(), [](const auto& p) { return p.first; }); 17 | return names; 18 | } 19 | 20 | OdometryEstimation::Ptr create_odometry(const std::string& name, const OdometryEstimationParams& params) { 21 | auto found = std::find_if(odometry_registry.begin(), odometry_registry.end(), [&](const auto& p) { return p.first == name; }); 22 | if (found == odometry_registry.end()) { 23 | std::cerr << "error: unknown odometry engine: " << name << std::endl; 24 | return nullptr; 25 | } 26 | return found->second(params); 27 | } 28 | 29 | } // namespace small_gicp 30 | -------------------------------------------------------------------------------- /src/small_gicp/registration/registration.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #include 4 | 5 | namespace small_gicp {} -------------------------------------------------------------------------------- /src/test/points_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace small_gicp; 9 | 10 | template 11 | void test_points(const std::vector& src_points, PointCloud& points, std::mt19937& mt) { 12 | EXPECT_EQ(traits::size(points), src_points.size()); 13 | EXPECT_TRUE(traits::has_points(points)); 14 | EXPECT_TRUE(traits::has_normals(points)); 15 | EXPECT_TRUE(traits::has_covs(points)); 16 | 17 | std::uniform_real_distribution<> dist(-100.0, 100.0); 18 | std::vector normals(src_points.size()); 19 | std::vector covs(src_points.size()); 20 | for (size_t i = 0; i < traits::size(points); i++) { 21 | normals[i] = Eigen::Vector4d(dist(mt), dist(mt), dist(mt), dist(mt)); 22 | covs[i] = normals[i] * normals[i].transpose(); 23 | } 24 | 25 | for (size_t i = 0; i < traits::size(points); i++) { 26 | traits::set_normal(points, i, normals[i]); 27 | traits::set_cov(points, i, covs[i]); 28 | 29 | EXPECT_NEAR((traits::point(points, i) - src_points[i]).norm(), 0.0, 1e-3); 30 | EXPECT_NEAR((traits::normal(points, i) - normals[i]).norm(), 0.0, 1e-3); 31 | EXPECT_NEAR((traits::cov(points, i) - covs[i]).norm(), 0.0, 1e-3); 32 | } 33 | 34 | traits::resize(points, src_points.size() / 2); 35 | EXPECT_EQ(traits::size(points), src_points.size() / 2); 36 | 37 | for (size_t i = 0; i < traits::size(points); i++) { 38 | EXPECT_NEAR((traits::point(points, i) - src_points[i]).norm(), 0.0, 1e-3); 39 | EXPECT_NEAR((traits::normal(points, i) - normals[i]).norm(), 0.0, 1e-3); 40 | EXPECT_NEAR((traits::cov(points, i) - covs[i]).norm(), 0.0, 1e-3); 41 | } 42 | } 43 | 44 | TEST(PointsTest, PointsTest) { 45 | std::mt19937 mt; 46 | std::uniform_real_distribution<> dist(-100.0, 100.0); 47 | 48 | std::vector src_points(100); 49 | std::generate(src_points.begin(), src_points.end(), [&] { return Eigen::Vector4d(dist(mt), dist(mt), dist(mt), 1.0); }); 50 | 51 | auto points = std::make_shared(src_points); 52 | test_points(src_points, *points, mt); 53 | 54 | auto points_pcl = pcl::make_shared>(); 55 | points_pcl->resize(src_points.size()); 56 | for (size_t i = 0; i < src_points.size(); i++) { 57 | points_pcl->at(i).getVector4fMap() = src_points[i].cast(); 58 | } 59 | test_points(src_points, *points_pcl, mt); 60 | } -------------------------------------------------------------------------------- /src/test/sort_omp_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | using namespace small_gicp; 9 | 10 | // Check if two vectors are identical 11 | template 12 | bool identical(const std::vector& arr1, const std::vector& arr2) { 13 | if (arr1.size() != arr2.size()) { 14 | return false; 15 | } 16 | 17 | for (size_t i = 0; i < arr1.size(); i++) { 18 | if (arr1[i] != arr2[i]) { 19 | return false; 20 | } 21 | } 22 | return true; 23 | } 24 | 25 | // Test merge_sort_omp 26 | TEST(SortOMP, MergeSortTest) { 27 | std::mt19937 mt; 28 | 29 | std::uniform_int_distribution<> size_dist(0, 8192); 30 | 31 | // int 32 | for (int i = 0; i < 100; i++) { 33 | std::uniform_int_distribution<> data_dist(-100, 100); 34 | std::vector data(size_dist(mt)); 35 | std::generate(data.begin(), data.end(), [&] { return data_dist(mt); }); 36 | 37 | std::vector sorted = data; 38 | std::sort(sorted.begin(), sorted.end()); 39 | 40 | std::vector sorted_omp = data; 41 | merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); 42 | 43 | EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); 44 | } 45 | 46 | // double 47 | for (int i = 0; i < 100; i++) { 48 | std::uniform_real_distribution<> data_dist(-100, 100); 49 | std::vector data(size_dist(mt)); 50 | std::generate(data.begin(), data.end(), [&] { return data_dist(mt); }); 51 | 52 | std::vector sorted = data; 53 | std::sort(sorted.begin(), sorted.end()); 54 | 55 | std::vector sorted_omp = data; 56 | merge_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); 57 | 58 | EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); 59 | } 60 | 61 | // empty 62 | std::vector empty_vector; 63 | merge_sort_omp(empty_vector.begin(), empty_vector.end(), std::less(), 4); 64 | EXPECT_TRUE(empty_vector.empty()) << "Empty vector check"; 65 | } 66 | 67 | // Test quick_sort_omp 68 | TEST(SortOMP, QuickSortTest) { 69 | std::mt19937 mt; 70 | 71 | std::uniform_int_distribution<> size_dist(0, 8192); 72 | 73 | // int 74 | for (int i = 0; i < 100; i++) { 75 | std::uniform_int_distribution<> data_dist(-100, 100); 76 | std::vector data(size_dist(mt)); 77 | std::generate(data.begin(), data.end(), [&] { return data_dist(mt); }); 78 | 79 | std::vector sorted = data; 80 | std::sort(sorted.begin(), sorted.end()); 81 | 82 | std::vector sorted_omp = data; 83 | quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); 84 | 85 | EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); 86 | } 87 | 88 | // double 89 | for (int i = 0; i < 100; i++) { 90 | std::uniform_real_distribution<> data_dist(-100, 100); 91 | std::vector data(size_dist(mt)); 92 | std::generate(data.begin(), data.end(), [&] { return data_dist(mt); }); 93 | 94 | std::vector sorted = data; 95 | std::sort(sorted.begin(), sorted.end()); 96 | 97 | std::vector sorted_omp = data; 98 | quick_sort_omp(sorted_omp.begin(), sorted_omp.end(), std::less(), 4); 99 | 100 | EXPECT_TRUE(identical(sorted, sorted_omp)) << fmt::format("i={} N={}", i, data.size()); 101 | } 102 | 103 | // empty 104 | std::vector empty_vector; 105 | quick_sort_omp(empty_vector.begin(), empty_vector.end(), std::less(), 4); 106 | EXPECT_TRUE(empty_vector.empty()) << "Empty vector check"; 107 | } 108 | -------------------------------------------------------------------------------- /src/test/sort_tbb_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using namespace small_gicp; 11 | 12 | // Check if two vectors are identical 13 | template 14 | bool identical(const std::vector& arr1, const std::vector& arr2) { 15 | if (arr1.size() != arr2.size()) { 16 | return false; 17 | } 18 | 19 | for (size_t i = 0; i < arr1.size(); i++) { 20 | if (arr1[i] != arr2[i]) { 21 | return false; 22 | } 23 | } 24 | return true; 25 | } 26 | 27 | template 28 | void test_radix_sort(std::mt19937& mt) { 29 | std::uniform_int_distribution<> size_dist(0, 8192); 30 | 31 | for (int i = 0; i < 20; i++) { 32 | std::vector data(size_dist(mt)); 33 | std::generate(data.begin(), data.end(), [&] { return mt(); }); 34 | 35 | std::vector sorted = data; 36 | std::stable_sort(sorted.begin(), sorted.end()); 37 | 38 | std::vector sorted_tbb = data; 39 | radix_sort_tbb(sorted_tbb.data(), sorted_tbb.data() + sorted_tbb.size(), [](const T x) { return x; }); 40 | 41 | EXPECT_TRUE(identical(sorted, sorted_tbb)) << fmt::format("i={} N={}", i, data.size()); 42 | } 43 | 44 | for (int i = 0; i < 20; i++) { 45 | std::vector> data(size_dist(mt)); 46 | std::generate(data.begin(), data.end(), [&] { return std::make_pair(mt(), mt()); }); 47 | 48 | std::vector> sorted = data; 49 | std::stable_sort(sorted.begin(), sorted.end(), [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); 50 | 51 | std::vector> sorted_tbb = data; 52 | radix_sort_tbb(sorted_tbb.data(), sorted_tbb.data() + sorted_tbb.size(), [](const auto& x) -> T { return x.first; }); 53 | 54 | EXPECT_TRUE(identical(sorted, sorted_tbb)) << fmt::format("i={} N={}", i, data.size()); 55 | } 56 | } 57 | 58 | // Test radix_sort_tbb 59 | TEST(SortTBB, RadixSortTest) { 60 | std::mt19937 mt; 61 | 62 | test_radix_sort(mt); 63 | test_radix_sort(mt); 64 | test_radix_sort(mt); 65 | test_radix_sort(mt); 66 | 67 | // empty 68 | std::vector empty_vector; 69 | radix_sort_tbb(empty_vector.data(), empty_vector.data() + empty_vector.size(), [](const std::uint64_t x) { return x; }); 70 | EXPECT_TRUE(empty_vector.empty()) << "Empty vector check"; 71 | } 72 | -------------------------------------------------------------------------------- /src/test/vector_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | using namespace small_gicp; 8 | 9 | TEST(VectorTest, HashTest) { 10 | std::mt19937 mt; 11 | 12 | for (int i = 0; i < 1000; i++) { 13 | std::uniform_int_distribution<> dist(-1000, 1000); 14 | const Eigen::Vector3i v(dist(mt), dist(mt), dist(mt)); 15 | EXPECT_EQ(XORVector3iHash::hash(v), XORVector3iHash()(v)); 16 | EXPECT_TRUE(XORVector3iHash::equal(v, v)); 17 | } 18 | } 19 | 20 | TEST(VectorTest, FloorTest) { 21 | std::mt19937 mt; 22 | for (int i = 0; i < 1000; i++) { 23 | std::uniform_real_distribution<> dist(-1000.0, 1000.0); 24 | const Eigen::Vector4d v(dist(mt), dist(mt), dist(mt), 1.0); 25 | const Eigen::Array4i floor1 = fast_floor(v); 26 | EXPECT_EQ(floor1[0], std::floor(v[0])); 27 | EXPECT_EQ(floor1[1], std::floor(v[1])); 28 | EXPECT_EQ(floor1[2], std::floor(v[2])); 29 | EXPECT_EQ(floor1[3], std::floor(v[3])); 30 | } 31 | } --------------------------------------------------------------------------------