├── cpp ├── COLCON_IGNORE └── kiss_icp │ ├── README.md │ ├── 3rdparty │ ├── eigen │ │ ├── LICENSE │ │ ├── eigen.cmake │ │ └── eigen.patch │ ├── sophus │ │ ├── LICENSE │ │ └── sophus.cmake │ ├── tsl_robin │ │ ├── LICENSE │ │ └── tsl_robin.cmake │ ├── tbb │ │ └── tbb.cmake │ └── find_dependencies.cmake │ ├── core │ ├── VoxelUtils.cpp │ ├── CMakeLists.txt │ ├── Preprocessing.hpp │ ├── Registration.hpp │ ├── Threshold.hpp │ ├── VoxelUtils.hpp │ ├── Threshold.cpp │ ├── VoxelHashMap.hpp │ └── Preprocessing.cpp │ ├── LICENSE │ ├── metrics │ ├── CMakeLists.txt │ └── Metrics.hpp │ ├── pipeline │ ├── CMakeLists.txt │ ├── KissICP.hpp │ └── KissICP.cpp │ ├── CMakeLists.txt │ └── cmake │ └── CompilerOptions.cmake ├── python ├── COLCON_IGNORE ├── kiss_icp │ ├── pybind │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ └── stl_vector_eigen.h │ ├── tools │ │ ├── __init__.py │ │ ├── progress_bar.py │ │ └── pipeline_results.py │ ├── __init__.py │ ├── config │ │ ├── __init__.py │ │ ├── config.py │ │ └── parser.py │ ├── voxelization.py │ ├── metrics.py │ ├── preprocess.py │ ├── threshold.py │ ├── registration.py │ ├── mapping.py │ ├── datasets │ │ ├── apollo.py │ │ ├── __init__.py │ │ ├── tum.py │ │ ├── boreas.py │ │ ├── ncd.py │ │ ├── mulran.py │ │ ├── kitti.py │ │ ├── helipr.py │ │ ├── ouster.py │ │ ├── mcap.py │ │ ├── rosbag.py │ │ └── nclt.py │ └── kiss_icp.py ├── tests │ └── test_kiss_icp.py ├── MANIFEST.in ├── LICENSE ├── CMakeLists.txt ├── pyproject.toml └── README.md ├── eval ├── .gitignore ├── README.md ├── kiss_icp_eval.py ├── kitti_raw.ipynb └── kitti.ipynb ├── .github ├── CODEOWNERS └── workflows │ ├── pre-commit.yml │ ├── ros.yml │ ├── python.yml │ ├── pypi.yml │ └── cpp.yml ├── .cmake-format.yaml ├── .gitattributes ├── config ├── basic.yaml ├── advanced.yaml └── README.md ├── .clang-format ├── Makefile ├── CITATION.cff ├── .pre-commit-config.yaml ├── LICENSE ├── ros ├── LICENSE ├── config │ └── config.yaml ├── package.xml ├── README.md ├── CMakeLists.txt ├── src │ └── OdometryServer.hpp └── launch │ └── odometry.launch.py └── README.md /cpp/COLCON_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /python/COLCON_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /python/kiss_icp/pybind/.gitignore: -------------------------------------------------------------------------------- 1 | include/ 2 | share/ 3 | -------------------------------------------------------------------------------- /eval/.gitignore: -------------------------------------------------------------------------------- 1 | results/ 2 | kitti-odometry/ 3 | .ipynb_checkpoints/ 4 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @nachovizzo @benemer @tizianoGuadagnino @saurabh1002 @mehermvr 2 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | enable_markup: false 2 | line_width: 120 3 | format: 4 | max_subgroups_hwrap: 5 5 | -------------------------------------------------------------------------------- /python/tests/test_kiss_icp.py: -------------------------------------------------------------------------------- 1 | def test_import(): 2 | from kiss_icp.kiss_icp import KissICP 3 | 4 | assert KissICP is not None 5 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.cpp linguist-language=C++ 2 | *.hpp linguist-language=C++ 3 | *.md linguist-documentation 4 | *.py linguist-vendored 5 | *.ipynb linguist-vendored 6 | -------------------------------------------------------------------------------- /python/MANIFEST.in: -------------------------------------------------------------------------------- 1 | recursive-include kiss_icp *.py 2 | recursive-include kiss_icp *.cpp 3 | recursive-include kiss_icp *.h 4 | global-include CMakeLists.txt 5 | global-include LICENSE 6 | global-include *.cmake 7 | -------------------------------------------------------------------------------- /config/basic.yaml: -------------------------------------------------------------------------------- 1 | out_dir: "results" 2 | 3 | data: 4 | deskew: True 5 | max_range: 100.0 6 | min_range: 0.0 7 | 8 | mapping: 9 | max_points_per_voxel: 20 10 | 11 | adaptive_threshold: 12 | initial_threshold: 2.0 13 | min_motion_th: 0.1 14 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | UseTab: Never 3 | IndentWidth: 4 4 | AccessModifierOffset: -4 5 | ColumnLimit: 100 6 | BinPackParameters: false 7 | SortIncludes: true 8 | Standard: c++17 9 | DerivePointerAlignment: false 10 | PointerAlignment: Right 11 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: cpp 2 | 3 | install: 4 | @pip install --verbose ./python/ 5 | 6 | uninstall: 7 | @pip -v uninstall kiss_icp 8 | 9 | editable: 10 | @pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake 11 | @pip install --no-build-isolation -ve ./python/ 12 | 13 | test: 14 | @pytest -rA --verbose ./python/ 15 | 16 | cpp: 17 | @cmake -Bbuild cpp/kiss_icp/ 18 | @cmake --build build -j$(nproc --all) 19 | -------------------------------------------------------------------------------- /cpp/kiss_icp/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP C++ Library 2 | 3 | ## How to build 4 | 5 | ```sh 6 | cmake -Bbuild 7 | cmake --build build -j$(nproc --all) 8 | ``` 9 | 10 | ## Dependencies 11 | 12 | The cmake build system should handle all dependencies for you. In case you have some particular 13 | requirements for the library dependencies, by default the build system will attempt to use the 14 | ones you have installed on your local system. 15 | -------------------------------------------------------------------------------- /.github/workflows/pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Style Check 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | jobs: 10 | pre-commit: 11 | name: Pre-commit checks 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: actions/setup-python@v4 16 | with: 17 | python-version: "3.10" 18 | - uses: pre-commit/action@v3.0.0 19 | -------------------------------------------------------------------------------- /.github/workflows/ros.yml: -------------------------------------------------------------------------------- 1 | name: ROS nodes 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | jobs: 10 | ros2_node: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | release: [humble, jazzy, kilted, rolling] 15 | container: osrf/ros:${{ matrix.release }}-desktop 16 | steps: 17 | - name: Setup cmake 18 | uses: jwlawson/actions-setup-cmake@v1.13 19 | with: 20 | cmake-version: "3.25.x" 21 | - uses: actions/checkout@v3 22 | - name: Run colcon 23 | run: source /opt/ros/${{ matrix.release }}/setup.bash && colcon build --event-handlers console_direct+ 24 | shell: bash 25 | -------------------------------------------------------------------------------- /config/advanced.yaml: -------------------------------------------------------------------------------- 1 | # NOTE: Please note that this is just an example of the advanced configuration for the KISS-ICP 2 | # pipeline and it's not really meant to use for any particular dataset, is just to expose other 3 | # options that are disabled by default 4 | out_dir: "results" 5 | 6 | data: 7 | deskew: True 8 | max_range: 100.0 9 | min_range: 0.0 10 | 11 | mapping: 12 | voxel_size: 1.0 # <- optional 13 | max_points_per_voxel: 20 14 | 15 | adaptive_threshold: 16 | fixed_threshold: 0.3 # <- optional, disables adaptive threshold 17 | # initial_threshold: 2.0 18 | # min_motion_th: 0.1 19 | 20 | registration: 21 | max_num_iterations: 500 # <- optional 22 | convergence_criterion: 0.0001 # <- optional 23 | max_num_threads: 0 # <- optional, 0 means automatic 24 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.1 2 | preferred-citation: 3 | title: "KISS-ICP: In Defense of Point-to-Point ICP - Simple, Accurate, and Robust Registration If Done the Right Way" 4 | doi: "10.1109/LRA.2023.3236571" 5 | year: "2023" 6 | type: article 7 | journal: "IEEE Robotics and Automation Letters (RA-L)" 8 | url: https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf 9 | codeurl: https://github.com/PRBonn/kiss-icp 10 | authors: 11 | - family-names: Vizzo 12 | given-names: Ignacio 13 | - family-names: Guadagnino 14 | given-names: Tiziano 15 | - family-names: Mersch 16 | given-names: Benedikt 17 | - family-names: Wiesmann 18 | given-names: Louis 19 | - family-names: Behley 20 | given-names: Jens 21 | - family-names: Stachniss 22 | given-names: Cyrill 23 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/eigen/LICENSE: -------------------------------------------------------------------------------- 1 | Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: 2 | http://www.mozilla.org/MPL/2.0/ 3 | http://www.mozilla.org/MPL/2.0/FAQ.html 4 | 5 | Some files contain third-party code under BSD or LGPL licenses, whence the other 6 | COPYING.* files here. 7 | 8 | All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. 9 | For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. 10 | 11 | If you want to guarantee that the Eigen code that you are #including is licensed 12 | under the MPL2 and possibly more permissive licenses (like BSD), #define this 13 | preprocessor symbol: 14 | EIGEN_MPL2_ONLY 15 | For example, with most compilers, you could add this to your project CXXFLAGS: 16 | -DEIGEN_MPL2_ONLY 17 | This will cause a compilation error to be generated if you #include any code that is 18 | LGPL licensed. 19 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: trailing-whitespace 6 | exclude: \.patch$ 7 | - id: end-of-file-fixer 8 | exclude: \.patch$ 9 | - id: check-yaml 10 | - id: check-added-large-files 11 | - repo: https://github.com/pre-commit/mirrors-clang-format 12 | rev: v14.0.0 13 | hooks: 14 | - id: clang-format 15 | - repo: https://github.com/psf/black 16 | rev: 23.1.0 17 | hooks: 18 | - id: black 19 | args: [--config=python/pyproject.toml] 20 | - repo: https://github.com/ahans/cmake-format-precommit 21 | rev: 8e52fb6506f169dddfaa87f88600d765fca48386 22 | hooks: 23 | - id: cmake-format 24 | - repo: https://github.com/pycqa/isort 25 | rev: 5.12.0 26 | hooks: 27 | - id: isort 28 | args: ["--settings-path=python/pyproject.toml"] 29 | -------------------------------------------------------------------------------- /.github/workflows/python.yml: -------------------------------------------------------------------------------- 1 | name: Python API 2 | on: 3 | push: 4 | branches: ["main"] 5 | pull_request: 6 | branches: ["main"] 7 | 8 | jobs: 9 | python_package: 10 | runs-on: ${{ matrix.os }} 11 | strategy: 12 | matrix: 13 | os: [ubuntu-24.04, ubuntu-22.04, windows-2022, macos-14, macos-15] 14 | 15 | steps: 16 | - uses: actions/checkout@v3 17 | - name: Set up Python3 18 | uses: actions/setup-python@v4 19 | - name: Install dependencies 20 | run: | 21 | python -m pip install --upgrade pip 22 | - name: Build pip package 23 | run: | 24 | python -m pip install --verbose ./python/ 25 | - name: Test installation 26 | run: | 27 | kiss_icp_pipeline --version 28 | - name: Run unittests 29 | run: | 30 | python -m pip install --verbose './python[test]' 31 | pytest -rA --verbose ./python/ 32 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/VoxelUtils.cpp: -------------------------------------------------------------------------------- 1 | #include "VoxelUtils.hpp" 2 | 3 | #include 4 | 5 | namespace kiss_icp { 6 | 7 | std::vector VoxelDownsample(const std::vector &frame, 8 | const double voxel_size) { 9 | tsl::robin_map grid; 10 | grid.reserve(frame.size()); 11 | std::for_each(frame.cbegin(), frame.cend(), [&](const auto &point) { 12 | const auto voxel = PointToVoxel(point, voxel_size); 13 | if (!grid.contains(voxel)) grid.insert({voxel, point}); 14 | }); 15 | std::vector frame_dowsampled; 16 | frame_dowsampled.reserve(grid.size()); 17 | std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) { 18 | frame_dowsampled.emplace_back(voxel_and_point.second); 19 | }); 20 | return frame_dowsampled; 21 | } 22 | 23 | } // namespace kiss_icp 24 | -------------------------------------------------------------------------------- /config/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP Config 2 | 3 | **NOTE:** You **DO NOT NEED** to use these configuration files to run the system. 4 | 5 | The configuration of our system is a sanitzed selection of parameters. In most of the cases it 6 | should be enough to use the default parameters "as is". 7 | 8 | ## Additional configuration for KISS-ICP 9 | 10 | You will need first need to install all the dependencies of the kiss_icp: 11 | 12 | ```sh 13 | pip install "kiss-icp[all]" 14 | ``` 15 | If you need extra tunning then you can use the optionally provided yaml files from this folder. You 16 | can override just 1 or all of the default parameters. To load this configuration you only need to 17 | instruct the command line interface: 18 | 19 | ```sh 20 | $ kiss_icp_pipeline --config /my_config.yaml 21 | ``` 22 | 23 | Big thanks to Markus Pielmeier who contributed PR #63 making this configuration module as flexible 24 | as possible. 25 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/sophus/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2015 Jesse Beder. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWAR 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/tsl_robin/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Thibaut Goetghebuer-Planchon 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 | -------------------------------------------------------------------------------- /ros/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /python/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /ros/config/config.yaml: -------------------------------------------------------------------------------- 1 | # NOTE: Please note that this is just an example of the advanced configuration for the KISS-ICP 2 | # pipeline and it's not really meant to use for any particular dataset, is just to expose other 3 | # options that are disabled by default 4 | 5 | kiss_icp_node: 6 | ros__parameters: 7 | #### /!\ If you set these ROS-related parameters in the file, they will overwrite the corresponding launch arguments #### 8 | 9 | # base_frame: "" 10 | # lidar_odom_frame: "odom_lidar" 11 | # publish_odom_tf: True 12 | # invert_odom_tf: True 13 | # position_covariance: 0.1 14 | # orientation_covariance: 0.1 15 | 16 | #### Core KISS-ICP parameters #### 17 | 18 | data: 19 | deskew: True 20 | max_range: 100.0 21 | min_range: 0.0 22 | 23 | mapping: 24 | # voxel_size: 1.0 # <- optional, default = max_range / 100.0 25 | max_points_per_voxel: 20 26 | 27 | adaptive_threshold: 28 | initial_threshold: 2.0 29 | min_motion_th: 0.1 30 | 31 | registration: 32 | max_num_iterations: 500 # <- optional 33 | convergence_criterion: 0.0001 # <- optional 34 | max_num_threads: 0 # <- optional, 0 means automatic 35 | -------------------------------------------------------------------------------- /cpp/kiss_icp/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /python/kiss_icp/tools/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | -------------------------------------------------------------------------------- /python/kiss_icp/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | __version__ = "1.2.3" 24 | -------------------------------------------------------------------------------- /python/kiss_icp/config/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | from .parser import KISSConfig, load_config, write_config 25 | -------------------------------------------------------------------------------- /python/kiss_icp/tools/progress_bar.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from tqdm.auto import trange 24 | 25 | 26 | def get_progress_bar(first_scan, last_scan): 27 | return trange(first_scan, last_scan, unit=" frames", dynamic_ncols=True) 28 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/tsl_robin/tsl_robin.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | include(FetchContent) 24 | FetchContent_Declare(tessil SYSTEM URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.4.0.tar.gz) 25 | FetchContent_MakeAvailable(tessil) 26 | -------------------------------------------------------------------------------- /python/kiss_icp/pybind/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | pybind11_add_module(kiss_icp_pybind MODULE kiss_icp_pybind.cpp) 24 | target_link_libraries(kiss_icp_pybind PRIVATE kiss_icp_core kiss_icp_metrics) 25 | install(TARGETS kiss_icp_pybind DESTINATION .) 26 | -------------------------------------------------------------------------------- /cpp/kiss_icp/metrics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(kiss_icp_metrics STATIC) 24 | target_sources(kiss_icp_metrics PRIVATE Metrics.cpp) 25 | target_link_libraries(kiss_icp_metrics PUBLIC Eigen3::Eigen) 26 | set_global_target_properties(kiss_icp_metrics) 27 | -------------------------------------------------------------------------------- /cpp/kiss_icp/pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | add_library(kiss_icp_pipeline STATIC) 25 | target_sources(kiss_icp_pipeline PRIVATE KissICP.cpp) 26 | target_link_libraries(kiss_icp_pipeline PUBLIC kiss_icp_core) 27 | set_global_target_properties(kiss_icp_pipeline) 28 | -------------------------------------------------------------------------------- /python/kiss_icp/voxelization.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.pybind import kiss_icp_pybind 26 | 27 | 28 | def voxel_down_sample(points: np.ndarray, voxel_size: float): 29 | _points = kiss_icp_pybind._Vector3dVector(points) 30 | return np.asarray(kiss_icp_pybind._voxel_down_sample(_points, voxel_size)) 31 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(kiss_icp_core STATIC) 24 | target_sources(kiss_icp_core PRIVATE Registration.cpp VoxelHashMap.cpp VoxelUtils.cpp Preprocessing.cpp Threshold.cpp) 25 | target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) 26 | set_global_target_properties(kiss_icp_core) 27 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/sophus/sophus.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of Bonn 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 | include(FetchContent) 23 | 24 | set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") 25 | set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") 26 | set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") 27 | 28 | FetchContent_Declare(sophus SYSTEM URL https://github.com/strasdat/Sophus/archive/refs/tags/1.24.6.tar.gz) 29 | FetchContent_MakeAvailable(sophus) 30 | -------------------------------------------------------------------------------- /cpp/kiss_icp/metrics/Metrics.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | 24 | #ifndef KITTI_UTILS_H_ 25 | #define KITTI_UTILS_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace kiss_icp::metrics { 32 | 33 | std::tuple SeqError(const std::vector &poses_gt, 34 | const std::vector &poses_result); 35 | 36 | std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, 37 | const std::vector &poses_result); 38 | } // namespace kiss_icp::metrics 39 | 40 | #endif // KITTI_UTILS_H_ 41 | -------------------------------------------------------------------------------- /python/kiss_icp/metrics.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from typing import Tuple 24 | 25 | import numpy as np 26 | 27 | from kiss_icp.pybind import kiss_icp_pybind 28 | 29 | 30 | def sequence_error(gt_poses: np.ndarray, results_poses: np.ndarray) -> Tuple[float, float]: 31 | """Sptis the sequence error for a given trajectory in camera coordinate frames.""" 32 | return kiss_icp_pybind._kitti_seq_error(gt_poses, results_poses) 33 | 34 | 35 | def absolute_trajectory_error( 36 | gt_poses: np.ndarray, results_poses: np.ndarray 37 | ) -> Tuple[float, float]: 38 | """Sptis the sequence error for a given trajectory in camera coordinate frames.""" 39 | return kiss_icp_pybind._absolute_trajectory_error(gt_poses, results_poses) 40 | -------------------------------------------------------------------------------- /python/kiss_icp/config/config.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from typing import Optional 24 | 25 | from pydantic import BaseModel 26 | 27 | 28 | class DataConfig(BaseModel): 29 | max_range: float = 100.0 30 | min_range: float = 0.0 31 | deskew: bool = True 32 | 33 | 34 | class MappingConfig(BaseModel): 35 | voxel_size: Optional[float] = None # default: take it from data 36 | max_points_per_voxel: int = 20 37 | 38 | 39 | class RegistrationConfig(BaseModel): 40 | max_num_iterations: Optional[int] = 500 41 | convergence_criterion: Optional[float] = 0.0001 42 | max_num_threads: Optional[int] = 0 # 0 means automatic 43 | 44 | 45 | class AdaptiveThresholdConfig(BaseModel): 46 | fixed_threshold: Optional[float] = None 47 | initial_threshold: float = 2.0 48 | min_motion_th: float = 0.1 49 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/Preprocessing.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace kiss_icp { 31 | 32 | struct Preprocessor { 33 | Preprocessor(const double max_range, 34 | const double min_range, 35 | const bool deskew, 36 | const int max_num_threads); 37 | 38 | std::vector Preprocess(const std::vector &frame, 39 | const std::vector ×tamps, 40 | const Sophus::SE3d &relative_motion) const; 41 | double max_range_; 42 | double min_range_; 43 | bool deskew_; 44 | int max_num_threads_; 45 | }; 46 | } // namespace kiss_icp 47 | -------------------------------------------------------------------------------- /.github/workflows/pypi.yml: -------------------------------------------------------------------------------- 1 | name: Publish to PyPI.org 2 | on: 3 | release: 4 | types: [published] 5 | push: 6 | branches: ["main"] 7 | pull_request: 8 | branches: ["main"] 9 | 10 | jobs: 11 | build_sdist: 12 | name: Build source distribution 13 | runs-on: ubuntu-latest 14 | steps: 15 | - uses: actions/checkout@v3 16 | 17 | - name: Build sdist 18 | run: pipx run build --sdist ${{github.workspace}}/python/ 19 | - name: Move sdist to dist 20 | run: mkdir -p dist && mv ${{github.workspace}}/python/dist/*.tar.gz dist/ 21 | 22 | - uses: actions/upload-artifact@v4 23 | with: 24 | path: dist/*.tar.gz 25 | 26 | cibuildwheel: 27 | name: Build wheels on ${{ matrix.os }} 28 | runs-on: ${{ matrix.os }} 29 | strategy: 30 | matrix: 31 | os: [ubuntu-24.04, windows-2022, macos-15] 32 | 33 | steps: 34 | - uses: actions/checkout@v3 35 | 36 | - name: Build test wheels (only PRs) 37 | if: github.event_name != 'release' 38 | uses: pypa/cibuildwheel@v2.22.0 39 | env: # build 1 build per platform just to make sure we can do it later when releasing 40 | CIBW_BUILD: "cp310-*" 41 | with: 42 | package-dir: ${{github.workspace}}/python/ 43 | 44 | - name: Build all wheels 45 | if: github.event_name == 'release' 46 | uses: pypa/cibuildwheel@v2.22.0 47 | with: 48 | package-dir: ${{github.workspace}}/python/ 49 | 50 | - uses: actions/upload-artifact@v4 51 | with: 52 | name: artifact-${{ matrix.os }} 53 | path: ./wheelhouse/*.whl 54 | 55 | pypi: 56 | if: github.event_name == 'release' 57 | needs: [cibuildwheel, build_sdist] 58 | runs-on: ubuntu-latest 59 | steps: 60 | - uses: actions/download-artifact@v4 61 | with: 62 | pattern: artifact* 63 | path: dist 64 | merge-multiple: true 65 | 66 | - uses: pypa/gh-action-pypi-publish@release/v1 67 | with: 68 | password: ${{ secrets.PYPI_API_TOKEN }} 69 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/Registration.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "VoxelHashMap.hpp" 30 | 31 | namespace kiss_icp { 32 | 33 | struct Registration { 34 | explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads); 35 | 36 | Sophus::SE3d AlignPointsToMap(const std::vector &frame, 37 | const VoxelHashMap &voxel_map, 38 | const Sophus::SE3d &initial_guess, 39 | const double max_correspondence_distance, 40 | const double kernel_scale); 41 | 42 | int max_num_iterations_; 43 | double convergence_criterion_; 44 | int max_num_threads_; 45 | }; 46 | } // namespace kiss_icp 47 | -------------------------------------------------------------------------------- /.github/workflows/cpp.yml: -------------------------------------------------------------------------------- 1 | name: C++ API 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | 12 | jobs: 13 | cpp_api: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-24.04, ubuntu-22.04, windows-2022, macos-14, macos-15] 18 | steps: 19 | - uses: actions/checkout@v3 20 | - name: Setup cmake 21 | uses: jwlawson/actions-setup-cmake@v1.13 22 | with: 23 | cmake-version: "3.25.x" 24 | - name: Configure CMake 25 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp 26 | - name: Build 27 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 28 | 29 | # As the previous job will always install the dependencies from cmake, and this is guaranteed to 30 | # work, we also want to support dev sandboxes where the main dependencies are already 31 | # pre-installed in the system. For now, we only support dev machines under a GNU/Linux 32 | # environmnets. If you are reading this and need the same functionallity in Windows/macOS please 33 | # open a ticket. 34 | cpp_api_dev: 35 | runs-on: ${{ matrix.os }} 36 | strategy: 37 | matrix: 38 | os: [ubuntu-24.04, ubuntu-22.04] 39 | 40 | steps: 41 | - uses: actions/checkout@v3 42 | - name: Cache dependencies 43 | uses: actions/cache@v4 44 | with: 45 | path: ~/.apt/cache 46 | key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} 47 | restore-keys: | 48 | ${{ runner.os }}-apt- 49 | - name: Install dependencies 50 | run: | 51 | sudo apt-get update 52 | sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev 53 | - name: Configure CMake 54 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kiss_icp 55 | - name: Build 56 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 57 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/Threshold.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace kiss_icp { 28 | 29 | struct AdaptiveThreshold { 30 | explicit AdaptiveThreshold(double initial_threshold, 31 | double min_motion_threshold, 32 | double max_range); 33 | 34 | /// Update the current belief of the deviation from the prediction model 35 | void UpdateModelDeviation(const Sophus::SE3d ¤t_deviation); 36 | 37 | /// Returns the KISS-ICP adaptive threshold used in registration 38 | inline double ComputeThreshold() const { return std::sqrt(model_sse_ / num_samples_); } 39 | 40 | // configurable parameters 41 | double min_motion_threshold_; 42 | double max_range_; 43 | 44 | // Local cache for ccomputation 45 | double model_sse_; 46 | int num_samples_; 47 | }; 48 | 49 | } // namespace kiss_icp 50 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/tbb/tbb.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | option(BUILD_SHARED_LIBS OFF) 24 | option(TBBMALLOC_BUILD OFF) 25 | option(TBB_EXAMPLES OFF) 26 | option(TBB_STRICT OFF) 27 | option(TBB_TEST OFF) 28 | 29 | include(FetchContent) 30 | FetchContent_Declare(tbb URL https://github.com/uxlfoundation/oneTBB/archive/refs/tags/v2022.1.0.tar.gz) 31 | FetchContent_GetProperties(tbb) 32 | if(NOT tbb_POPULATED) 33 | FetchContent_Populate(tbb) 34 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 35 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 36 | else() 37 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 38 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 39 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) 40 | get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) 41 | set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") 42 | endif() 43 | endif() 44 | -------------------------------------------------------------------------------- /eval/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP Evaluation 2 | 3 | In this folder you can find a few Python notebooks to run a complete evaluation of KISS-ICP without 4 | much effort. 5 | 6 | ## Datasets evaluated 7 | 8 | **NOTE:** In this repository we add the empty notebooks to use it for running experiments on your 9 | local machine. If you want to see the results of these experiments you can click on any click below 10 | or directly access the already runned notebooks [here](https://nbviewer.org/github/nachovizzo/kiss-icp/tree/main/evaluation/) 11 | 12 | - [KITTI Odometry](https://nbviewer.org/github/nachovizzo/kiss-icp/blob/main/evaluation/kitti.ipynb) 13 | - [KITTI RAW Odometry (no motion compensation applied)](https://nbviewer.org/github/nachovizzo/kiss-icp/blob/main/evaluation/kitti_raw.ipynb) 14 | - [Newer College Dataset](https://nbviewer.org/github/nachovizzo/kiss-icp/blob/main/evaluation/newer_college.ipynb) 15 | - [MulRan Dataset](https://nbviewer.org/github/nachovizzo/kiss-icp/blob/main/evaluation/mulran.ipynb) 16 | 17 | ## Where is the NCLT dataset? 18 | 19 | We discourage using NCLT to evaluate odometry systems: misalignments in the ground truth poses, 20 | missing frames, and inconsistencies in the data make the evaluation of odometry systems on such a 21 | dataset not a good evaluation tool from our perspective. However, we provide the results here for 22 | the sake of completeness in the manuscript. 23 | 24 | ## What about all the other datasets? 25 | 26 | Here we just want to add an easy evaluation bench for the KISS-ICP pipeline. Much more datasets are 27 | currently supported by our system and more are to come, an in-complete list of such datasets 28 | includes: 29 | 30 | - apollo 31 | - boreas 32 | - kitti 33 | - kitti_raw 34 | - mulran 35 | - ncd 36 | - nclt 37 | - nuscenes 38 | - rosbag 39 | - rosbag2 40 | - generic folders containing files with extensions: 41 | - bin 42 | - pcd 43 | - ply 44 | - xyz 45 | - obj 46 | - ctm 47 | - off 48 | - st 49 | 50 | ## What about all the other experiments from the paper? 51 | 52 | The experiment missing from this evaluation set are those whe require to modify the C++ core 53 | library, therefore we could not add Python notebooks for those experiments, but should be easy to 54 | run them once the full source code is available. 55 | -------------------------------------------------------------------------------- /python/kiss_icp/preprocess.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_preprocessor(config: KISSConfig): 30 | return Preprocessor( 31 | max_range=config.data.max_range, 32 | min_range=config.data.min_range, 33 | deskew=config.data.deskew, 34 | max_num_threads=config.registration.max_num_threads, 35 | ) 36 | 37 | 38 | class Preprocessor: 39 | def __init__(self, max_range, min_range, deskew, max_num_threads): 40 | self._preprocessor = kiss_icp_pybind._Preprocessor( 41 | max_range, min_range, deskew, max_num_threads 42 | ) 43 | 44 | def preprocess(self, frame: np.ndarray, timestamps: np.ndarray, relative_motion: np.ndarray): 45 | return np.asarray( 46 | self._preprocessor._preprocess( 47 | kiss_icp_pybind._Vector3dVector(frame), 48 | timestamps.ravel(), 49 | relative_motion, 50 | ) 51 | ) 52 | -------------------------------------------------------------------------------- /ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | kiss_icp 28 | 1.2.3 29 | KISS-ICP ROS 2 Wrapper 30 | ivizzo 31 | frevo 32 | benedikt 33 | MIT 34 | 35 | ament_cmake 36 | 37 | geometry_msgs 38 | nav_msgs 39 | rclcpp 40 | rclcpp_components 41 | rcutils 42 | sensor_msgs 43 | std_msgs 44 | tf2_ros 45 | std_srvs 46 | 47 | eigen 48 | sophus 49 | tbb 50 | robin-map-dev 51 | 52 | ros2launch 53 | 54 | 55 | ament_cmake 56 | 57 | 58 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # Silence timestamp warning 24 | if(CMAKE_VERSION VERSION_GREATER 3.24) 25 | cmake_policy(SET CMP0135 OLD) 26 | endif() 27 | 28 | function(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) 29 | string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) 30 | set(USE_FROM_SYSTEM_OPTION "USE_SYSTEM_${PACKAGE_NAME_UP}") 31 | if(${${USE_FROM_SYSTEM_OPTION}}) 32 | find_package(${PACKAGE_NAME} QUIET NO_MODULE) 33 | endif() 34 | if(NOT ${${USE_FROM_SYSTEM_OPTION}} OR NOT TARGET ${TARGET_NAME}) 35 | set(${USE_FROM_SYSTEM_OPTION} OFF PARENT_SCOPE) 36 | include(${INCLUDED_CMAKE_PATH}) 37 | endif() 38 | endfunction() 39 | 40 | find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") 41 | find_external_dependency("Sophus" "Sophus::Sophus" "${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake") 42 | find_external_dependency("TBB" "TBB::tbb" "${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake") 43 | find_external_dependency("tsl-robin-map" "tsl::robin_map" "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") 44 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp_pybind VERSION 1.2.3 LANGUAGES CXX) 25 | 26 | # Set build type 27 | set(CMAKE_BUILD_TYPE Release) 28 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 29 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 30 | 31 | set(PYBIND11_NEWPYTHON ON) 32 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 33 | find_package(pybind11 CONFIG REQUIRED) 34 | 35 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/) 36 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp) 37 | else() 38 | cmake_minimum_required(VERSION 3.18) 39 | message(STATUS "Performing out-of-tree build, fetching KISS-ICP v${CMAKE_PROJECT_VERSION} Release from Github") 40 | include(FetchContent) 41 | FetchContent_Declare( 42 | ext_kiss_icp_core PREFIX kiss_icp_core 43 | URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR 44 | cpp/kiss_icp) 45 | FetchContent_MakeAvailable(ext_kiss_icp_core) 46 | endif() 47 | 48 | add_subdirectory(kiss_icp/pybind) 49 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/VoxelUtils.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace kiss_icp { 31 | 32 | using Voxel = Eigen::Vector3i; 33 | inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) { 34 | return Voxel(static_cast(std::floor(point.x() / voxel_size)), 35 | static_cast(std::floor(point.y() / voxel_size)), 36 | static_cast(std::floor(point.z() / voxel_size))); 37 | } 38 | 39 | /// Voxelize a point cloud keeping the original coordinates 40 | std::vector VoxelDownsample(const std::vector &frame, 41 | const double voxel_size); 42 | 43 | } // namespace kiss_icp 44 | 45 | template <> 46 | struct std::hash { 47 | std::size_t operator()(const kiss_icp::Voxel &voxel) const { 48 | const uint32_t *vec = reinterpret_cast(voxel.data()); 49 | return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); 50 | } 51 | }; 52 | -------------------------------------------------------------------------------- /cpp/kiss_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp_cpp VERSION 1.2.3 LANGUAGES CXX) 25 | 26 | # Setup build options 27 | option(USE_CCACHE "Build using Ccache if found on the path" ON) 28 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 29 | option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) 30 | option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) 31 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 32 | 33 | # ccache setup 34 | if(USE_CCACHE) 35 | find_program(CCACHE_PATH ccache) 36 | if(CCACHE_PATH) 37 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 38 | set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 39 | message(STATUS "Using ccache: ${CCACHE_PATH}") 40 | endif() 41 | endif() 42 | 43 | # Set build type (repeat here for C++ only consumers) 44 | set(CMAKE_BUILD_TYPE Release) 45 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 46 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 47 | 48 | include(3rdparty/find_dependencies.cmake) 49 | include(cmake/CompilerOptions.cmake) 50 | 51 | add_subdirectory(core) 52 | add_subdirectory(metrics) 53 | add_subdirectory(pipeline) 54 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/Threshold.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Threshold.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace kiss_icp { 30 | AdaptiveThreshold::AdaptiveThreshold(double initial_threshold, 31 | double min_motion_threshold, 32 | double max_range) 33 | : min_motion_threshold_(min_motion_threshold), 34 | max_range_(max_range), 35 | model_sse_(initial_threshold * initial_threshold), 36 | num_samples_(1) {} 37 | 38 | void AdaptiveThreshold::UpdateModelDeviation(const Sophus::SE3d ¤t_deviation) { 39 | const double model_error = [&]() { 40 | const double theta = Eigen::AngleAxisd(current_deviation.rotationMatrix()).angle(); 41 | const double delta_rot = 2.0 * max_range_ * std::sin(theta / 2.0); 42 | const double delta_trans = current_deviation.translation().norm(); 43 | return delta_trans + delta_rot; 44 | }(); 45 | if (model_error > min_motion_threshold_) { 46 | model_sse_ += model_error * model_error; 47 | num_samples_++; 48 | } 49 | } 50 | 51 | } // namespace kiss_icp 52 | -------------------------------------------------------------------------------- /python/kiss_icp/threshold.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_threshold_estimator(config: KISSConfig): 30 | if config.adaptive_threshold.fixed_threshold is not None: 31 | return FixedThreshold(config.adaptive_threshold.fixed_threshold) 32 | return AdaptiveThreshold(config) 33 | 34 | 35 | class FixedThreshold: 36 | def __init__(self, fixed_threshold: float): 37 | self.fixed_threshold = fixed_threshold 38 | 39 | def get_threshold(self): 40 | return self.fixed_threshold 41 | 42 | def update_model_deviation(self, model_deviation): 43 | pass 44 | 45 | 46 | class AdaptiveThreshold: 47 | def __init__(self, config: KISSConfig): 48 | self._estimator = kiss_icp_pybind._AdaptiveThreshold( 49 | initial_threshold=config.adaptive_threshold.initial_threshold, 50 | min_motion_th=config.adaptive_threshold.min_motion_th, 51 | max_range=config.data.max_range, 52 | ) 53 | 54 | def get_threshold(self): 55 | return self._estimator._compute_threshold() 56 | 57 | def update_model_deviation(self, model_deviation: np.ndarray): 58 | self._estimator._update_model_deviation(model_deviation=model_deviation) 59 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") 25 | set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") 26 | set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") 27 | set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") 28 | set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") 29 | 30 | include(FetchContent) 31 | FetchContent_Declare(eigen URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz 32 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/eigen.patch UPDATE_DISCONNECTED 1) 33 | FetchContent_GetProperties(eigen) 34 | if(NOT eigen_POPULATED) 35 | FetchContent_Populate(eigen) 36 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 37 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 38 | else() 39 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 40 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 41 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) 42 | get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) 43 | set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") 44 | endif() 45 | endif() 46 | -------------------------------------------------------------------------------- /cpp/kiss_icp/3rdparty/eigen/eigen.patch: -------------------------------------------------------------------------------- 1 | commit cf82186416d04ea5df2a397d8fe09dc78d40ca65 2 | Author: Antonio Sánchez 3 | Date: Sat Mar 5 05:49:45 2022 +0000 4 | 5 | Adds new CMake Options for controlling build components. 6 | 7 | diff --git a/CMakeLists.txt b/CMakeLists.txt 8 | index de1c23e91..0af36a53a 100644 9 | --- a/CMakeLists.txt 10 | +++ b/CMakeLists.txt 11 | @@ -477,6 +477,9 @@ if(EIGEN_BUILD_TESTING) 12 | add_subdirectory(failtest) 13 | endif() 14 | 15 | +include(CMakeDetermineFortranCompiler) 16 | +option(EIGEN_BUILD_BLAS "Toggles the building of the Eigen Blas library" ${CMAKE_Fortran_COMPILER}) 17 | +option(EIGEN_BUILD_LAPACK "Toggles the building of the included Eigen LAPACK library" ${CMAKE_Fortran_COMPILER}) 18 | if(EIGEN_LEAVE_TEST_IN_ALL_TARGET) 19 | add_subdirectory(blas) 20 | add_subdirectory(lapack) 21 | @@ -611,6 +614,8 @@ set_target_properties (eigen PROPERTIES EXPORT_NAME Eigen) 22 | 23 | install (TARGETS eigen EXPORT Eigen3Targets) 24 | 25 | +option(EIGEN_BUILD_CMAKE_PACKAGE "Enables the creation of EigenConfig.cmake and related files" ON) 26 | +if(EIGEN_BUILD_CMAKE_PACKAGE) 27 | configure_package_config_file ( 28 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3Config.cmake.in 29 | ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 30 | @@ -655,6 +660,7 @@ install (FILES ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 31 | # Add uninstall target 32 | add_custom_target ( uninstall 33 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake) 34 | +endif() 35 | 36 | if (EIGEN_SPLIT_TESTSUITE) 37 | ei_split_testsuite("${EIGEN_SPLIT_TESTSUITE}") 38 | diff --git a/blas/CMakeLists.txt b/blas/CMakeLists.txt 39 | index 8d3cb86dc..c530957fb 100644 40 | --- a/blas/CMakeLists.txt 41 | +++ b/blas/CMakeLists.txt 42 | @@ -1,6 +1,7 @@ 43 | 44 | project(EigenBlas CXX) 45 | 46 | +if(EIGEN_BUILD_BLAS) 47 | include(CheckLanguage) 48 | check_language(Fortran) 49 | if(CMAKE_Fortran_COMPILER) 50 | @@ -59,4 +60,4 @@ if(EIGEN_BUILD_TESTING) 51 | endif() 52 | 53 | endif() 54 | - 55 | +endif() 56 | diff --git a/lapack/CMakeLists.txt b/lapack/CMakeLists.txt 57 | index c8ca64001..8d6d75401 100644 58 | --- a/lapack/CMakeLists.txt 59 | +++ b/lapack/CMakeLists.txt 60 | @@ -1,5 +1,7 @@ 61 | project(EigenLapack CXX) 62 | 63 | +if(EIGEN_BUILD_LAPACK AND EIGEN_BUILD_BLAS) 64 | + 65 | include(CheckLanguage) 66 | check_language(Fortran) 67 | if(CMAKE_Fortran_COMPILER) 68 | @@ -457,3 +459,6 @@ if(EXISTS ${eigen_full_path_to_testing_lapack}) 69 | 70 | endif() 71 | 72 | +elseif(EIGEN_BUILD_LAPACK AND NOT EIGEN_BUILD_BLAS) 73 | + message(FATAL_ERROR "EIGEN_BUILD_LAPACK requires EIGEN_BUILD_BLAS") 74 | +endif() #EIGEN_BUILD_LAPACK 75 | -------------------------------------------------------------------------------- /python/kiss_icp/registration.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.config.parser import KISSConfig 26 | from kiss_icp.mapping import VoxelHashMap 27 | from kiss_icp.pybind import kiss_icp_pybind 28 | 29 | 30 | def get_registration(config: KISSConfig): 31 | return Registration( 32 | max_num_iterations=config.registration.max_num_iterations, 33 | convergence_criterion=config.registration.convergence_criterion, 34 | max_num_threads=config.registration.max_num_threads, 35 | ) 36 | 37 | 38 | class Registration: 39 | def __init__( 40 | self, 41 | max_num_iterations: int, 42 | convergence_criterion: float, 43 | max_num_threads: int = 0, 44 | ): 45 | self._registration = kiss_icp_pybind._Registration( 46 | max_num_iterations=max_num_iterations, 47 | convergence_criterion=convergence_criterion, 48 | max_num_threads=max_num_threads, 49 | ) 50 | 51 | def align_points_to_map( 52 | self, 53 | points: np.ndarray, 54 | voxel_map: VoxelHashMap, 55 | initial_guess: np.ndarray, 56 | max_correspondance_distance: float, 57 | kernel: float, 58 | ) -> np.ndarray: 59 | return self._registration._align_points_to_map( 60 | points=kiss_icp_pybind._Vector3dVector(points), 61 | voxel_map=voxel_map._internal_map, 62 | initial_guess=initial_guess, 63 | max_correspondance_distance=max_correspondance_distance, 64 | kernel=kernel, 65 | ) 66 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/VoxelHashMap.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | // NOTE: This implementation is heavily inspired in the original CT-ICP VoxelHashMap implementation, 25 | // although it was heavily modifed and drastically simplified, but if you are using this module you 26 | // should at least acknoowledge the work from CT-ICP by giving a star on GitHub 27 | #pragma once 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "VoxelUtils.hpp" 36 | 37 | namespace kiss_icp { 38 | struct VoxelHashMap { 39 | explicit VoxelHashMap(double voxel_size, double max_distance, unsigned int max_points_per_voxel) 40 | : voxel_size_(voxel_size), 41 | max_distance_(max_distance), 42 | max_points_per_voxel_(max_points_per_voxel) {} 43 | 44 | inline void Clear() { map_.clear(); } 45 | inline bool Empty() const { return map_.empty(); } 46 | void Update(const std::vector &points, const Eigen::Vector3d &origin); 47 | void Update(const std::vector &points, const Sophus::SE3d &pose); 48 | void AddPoints(const std::vector &points); 49 | void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); 50 | std::vector Pointcloud() const; 51 | std::tuple GetClosestNeighbor(const Eigen::Vector3d &query) const; 52 | 53 | double voxel_size_; 54 | double max_distance_; 55 | unsigned int max_points_per_voxel_; 56 | tsl::robin_map> map_; 57 | }; 58 | } // namespace kiss_icp 59 | -------------------------------------------------------------------------------- /ros/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP ROS 2 Wrapper 2 | 3 | ### How to build 4 | 5 | You should not need any extra dependency, just clone and build: 6 | 7 | ```sh 8 | git clone https://github.com/PRBonn/kiss-icp 9 | colcon build 10 | source ./install/setup.bash 11 | ``` 12 | 13 | ### How to run 14 | 15 | The only required argument to provide is the **topic name** so KISS-ICP knows which PointCloud2 to process: 16 | 17 | ```sh 18 | ros2 launch kiss_icp odometry.launch.py bagfile:= topic:= 19 | ``` 20 | 21 | You can optionally launch the node with any bagfile, and play the bagfiles on a different shell: 22 | 23 | ```sh 24 | ros2 launch kiss_icp odometry.launch.py topic:= 25 | ``` 26 | 27 | and then, 28 | 29 | ```sh 30 | ros2 bag play *.bag 31 | ``` 32 | 33 | A preconfigured Rviz2 window is launched by default. To disable it, you can set the `visualize` launch argument to `false`: 34 | ```sh 35 | ros2 launch kiss_icp odometry.launch.py topic:= visualize:=false 36 | ``` 37 | 38 | ### Configuration 39 | 40 | The parameters for the KISS-ICP algorithm itself are written in a yaml configuration file. They are meant to be set before launching the node. An **example** yaml file is given in config/config.yaml. The file parsed by default is located in the share directory of the kiss_icp package (in your workspace that would be `install/kiss_icp/share/kiss_icp/config/config.yaml`), but any file path can be provided with the `config_file` launch argument, e.g.: `ros2 launch kiss_icp odometry.launch.py config_file:=/path/to/my/config_file.yaml`. 41 | 42 | The ROS-related parameters can be set via command-line when launching the node: 43 | * `base_frame` 44 | * `lidar_odom_frame` 45 | * `publish_odom_tf` 46 | * `invert_odom_tf` 47 | * `position_covariance` 48 | * `orientation_covariance` 49 | 50 | You can set them like so: 51 | ```sh 52 | ros2 launch kiss_icp odometry.launch.py topic:= := 53 | ``` 54 | For example: 55 | ```sh 56 | ros2 launch kiss_icp odometry.launch.py topic:=/lidar/points base_frame:=lidar_cloud 57 | ``` 58 | Note that if you set the ROS-related parameters in the yaml configuration file and via command-line, the values in the yaml file will be selected in the end. 59 | 60 | ## Out of source builds 61 | 62 | Good news! If you don't have git or you don't need to change the core KISS-ICP library, you can just 63 | copy paste this folder into your ROS2 workspace and build as usual. The build system will fetch 64 | the latest stable release for you. 65 | 66 | ## Looking how to run KITTI on ROS along with KISS-ICP? 67 | 68 | I believe you could use the python API instead, to avoid converting all the data to a format that is 69 | not the original one. If you still insist in doing it, I've created a separate repository for this, 70 | just clone and build [this package](https://github.com/nachovizzo/kiss_icp_kitti) 71 | -------------------------------------------------------------------------------- /python/kiss_icp/mapping.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.pybind import kiss_icp_pybind 27 | 28 | 29 | def get_voxel_hash_map(config: KISSConfig): 30 | return VoxelHashMap( 31 | voxel_size=config.mapping.voxel_size, 32 | max_distance=config.data.max_range, 33 | max_points_per_voxel=config.mapping.max_points_per_voxel, 34 | ) 35 | 36 | 37 | class VoxelHashMap: 38 | def __init__(self, voxel_size: float, max_distance: float, max_points_per_voxel: int): 39 | self._internal_map = kiss_icp_pybind._VoxelHashMap( 40 | voxel_size=voxel_size, 41 | max_distance=max_distance, 42 | max_points_per_voxel=max_points_per_voxel, 43 | ) 44 | 45 | def clear(self): 46 | return self._internal_map._clear() 47 | 48 | def empty(self): 49 | return self._internal_map._empty() 50 | 51 | def update(self, points: np.ndarray, pose: np.ndarray = np.eye(4)): 52 | """Add points to the inernal map representaion. 53 | 54 | The origin is needed to remove the far away poitns 55 | 56 | TODO(NACHO): Use similar overload API as we did for VDBFusion 57 | """ 58 | self._internal_map._update(kiss_icp_pybind._Vector3dVector(points), pose) 59 | 60 | def add_points(self, points): 61 | self._internal_map._add_points(kiss_icp_pybind._Vector3dVector(points)) 62 | 63 | def remove_far_away_points(self, origin): 64 | self._internal_map._remove_far_away_points(origin) 65 | 66 | def point_cloud(self) -> np.ndarray: 67 | """Return the internal representaion as a np.array (pointcloud).""" 68 | return np.asarray(self._internal_map._point_cloud()) 69 | -------------------------------------------------------------------------------- /python/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit_build_core", "pybind11"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "kiss-icp" 7 | version = "1.2.3" 8 | description = "Simple yet effective 3D LiDAR-Odometry registration pipeline" 9 | readme = "README.md" 10 | authors = [ 11 | { name = "Ignacio Vizzo", email = "ignaciovizzo@gmail.com" }, 12 | { name = "Tiziano Guadagnino", email = "frevo93@gmail.com" }, 13 | { name = "Benedikt Mersch", email = "benedikt.mersch@gmail.com" }, 14 | ] 15 | requires-python = ">=3.8" 16 | keywords = [ 17 | "LiDAR", 18 | "Localization", 19 | "Odometry", 20 | "SLAM", 21 | ] 22 | classifiers = [ 23 | "Intended Audience :: Developers", 24 | "Intended Audience :: Education", 25 | "Intended Audience :: Other Audience", 26 | "Intended Audience :: Science/Research", 27 | "License :: OSI Approved :: MIT License", 28 | "Operating System :: MacOS", 29 | "Operating System :: Microsoft :: Windows", 30 | "Operating System :: Unix", 31 | "Programming Language :: C++", 32 | "Programming Language :: Python :: 3", 33 | "Programming Language :: Python :: 3.8", 34 | "Programming Language :: Python :: 3.9", 35 | "Programming Language :: Python :: 3.10", 36 | "Programming Language :: Python :: 3.11", 37 | "Programming Language :: Python :: 3.12", 38 | "Programming Language :: Python :: 3.13", 39 | ] 40 | dependencies = [ 41 | "natsort", 42 | "numpy", 43 | "pydantic>=2", 44 | "pydantic-settings", 45 | "pyquaternion", 46 | "rich", 47 | "tqdm", 48 | "typer[all]>=0.6.0", 49 | ] 50 | 51 | [project.optional-dependencies] 52 | all = [ 53 | "open3d>=0.16", 54 | "polyscope>=2.4.0", 55 | "ouster-sdk>=0.11", 56 | "pyntcloud", 57 | "PyYAML", 58 | "trimesh", 59 | "rosbags>=0.10,<0.12", 60 | ] 61 | test = [ 62 | "pytest", 63 | ] 64 | visualizer = [ 65 | "open3d>=0.13", 66 | ] 67 | 68 | [project.scripts] 69 | kiss_icp_pipeline = "kiss_icp.tools.cmd:run" 70 | kiss_icp_dump_config = "kiss_icp.config.parser:write_config" 71 | 72 | [project.urls] 73 | Homepage = "https://github.com/PRBonn/kiss-icp" 74 | 75 | [tool.scikit-build] 76 | build-dir = "build/{wheel_tag}" 77 | cmake.verbose = false 78 | cmake.minimum-version = "3.16" 79 | editable.mode = "redirect" 80 | editable.rebuild = true 81 | editable.verbose = true 82 | sdist.exclude = ["pybind/"] 83 | wheel.install-dir = "kiss_icp/pybind/" 84 | 85 | [tool.black] 86 | line-length = 100 87 | 88 | [tool.isort] 89 | profile = "black" 90 | 91 | [tool.pylint.format] 92 | max-line-length = "100" 93 | 94 | [tool.cibuildwheel] 95 | archs = ["auto64"] 96 | skip = ["*-musllinux*", "pp*", "cp36-*"] 97 | 98 | [tool.cibuildwheel.macos] 99 | environment = "MACOSX_DEPLOYMENT_TARGET=11.0" 100 | archs = ["auto64", "arm64"] 101 | 102 | [tool.pytest.ini_options] 103 | testpaths = ['tests'] 104 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/apollo.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import importlib 25 | import os 26 | import sys 27 | from pathlib import Path 28 | 29 | import natsort 30 | import numpy as np 31 | from pyquaternion import Quaternion 32 | 33 | 34 | class ApolloDataset: 35 | def __init__(self, data_dir: Path, *_, **__): 36 | try: 37 | self.o3d = importlib.import_module("open3d") 38 | except ModuleNotFoundError: 39 | print( 40 | 'pcd files requires open3d and is not installed on your system run "pip install open3d"' 41 | ) 42 | sys.exit(1) 43 | 44 | self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/pcds/*.pcd")) 45 | self.gt_poses = self.read_poses(f"{data_dir}/poses/gt_poses.txt") 46 | self.sequence_id = os.path.basename(data_dir) 47 | 48 | def __len__(self): 49 | return len(self.scan_files) 50 | 51 | def __getitem__(self, idx): 52 | return self.get_scan(self.scan_files[idx]), np.array([]) 53 | 54 | def get_scan(self, scan_file: str): 55 | points = np.asarray(self.o3d.io.read_point_cloud(scan_file).points, dtype=np.float64) 56 | return points.astype(np.float64) 57 | 58 | @staticmethod 59 | def read_poses(file): 60 | data = np.loadtxt(file) 61 | _, _, translations, qxyzw = np.split(data, [1, 2, 5], axis=1) 62 | rotations = np.array( 63 | [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in qxyzw] 64 | ) 65 | poses = np.zeros([rotations.shape[0], 4, 4]) 66 | poses[:, :3, -1] = translations 67 | poses[:, :3, :3] = rotations 68 | poses[:, -1, -1] = 1 69 | # Convert from global coordinate poses to local poses 70 | first_pose = poses[0, :, :] 71 | return np.linalg.inv(first_pose) @ poses 72 | -------------------------------------------------------------------------------- /cpp/kiss_icp/cmake/CompilerOptions.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | function(set_global_target_properties target) 24 | target_compile_features(${target} PUBLIC cxx_std_17) 25 | target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) 26 | target_compile_options( 27 | ${target} 28 | PRIVATE # MSVC 29 | $<$:/W4> 30 | $<$:/WX> 31 | # Clang/AppleClang 32 | $<$:-fcolor-diagnostics> 33 | $<$:-Werror> 34 | $<$:-Wall> 35 | $<$:-Wextra> 36 | $<$:-Wconversion> 37 | $<$:-Wno-sign-conversion> 38 | # GNU 39 | $<$:-fdiagnostics-color=always> 40 | $<$:-Werror> 41 | $<$:-Wall> 42 | $<$:-Wextra> 43 | $<$:-pedantic> 44 | $<$:-Wcast-align> 45 | $<$:-Wcast-qual> 46 | $<$:-Wconversion> 47 | $<$:-Wdisabled-optimization> 48 | $<$:-Woverloaded-virtual>) 49 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 50 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 51 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 52 | PUBLIC $ $) 53 | endfunction() 54 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from pathlib import Path 24 | from typing import Dict, List 25 | 26 | 27 | def supported_file_extensions(): 28 | return [ 29 | "bin", 30 | "pcd", 31 | "ply", 32 | "xyz", 33 | "obj", 34 | "ctm", 35 | "off", 36 | "stl", 37 | ] 38 | 39 | 40 | def sequence_dataloaders(): 41 | # TODO: automatically infer this 42 | return ["kitti", "kitti_raw", "nuscenes", "helipr"] 43 | 44 | 45 | def available_dataloaders() -> List: 46 | import os.path 47 | import pkgutil 48 | 49 | pkgpath = os.path.dirname(__file__) 50 | return [name for _, name, _ in pkgutil.iter_modules([pkgpath])] 51 | 52 | 53 | def jumpable_dataloaders(): 54 | _jumpable_dataloaders = available_dataloaders() 55 | _jumpable_dataloaders.remove("mcap") 56 | _jumpable_dataloaders.remove("ouster") 57 | _jumpable_dataloaders.remove("rosbag") 58 | return _jumpable_dataloaders 59 | 60 | 61 | def dataloader_types() -> Dict: 62 | import ast 63 | import importlib 64 | 65 | dataloaders = available_dataloaders() 66 | _types = {} 67 | for dataloader in dataloaders: 68 | script = importlib.util.find_spec(f".{dataloader}", __name__).origin 69 | with open(script) as f: 70 | tree = ast.parse(f.read(), script) 71 | classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] 72 | _types[dataloader] = classes[0].name # assuming there is only 1 class 73 | return _types 74 | 75 | 76 | def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): 77 | import importlib 78 | 79 | dataloader_type = dataloader_types()[dataloader] 80 | module = importlib.import_module(f".{dataloader}", __name__) 81 | assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" 82 | dataset = getattr(module, dataloader_type) 83 | return dataset(data_dir=data_dir, *args, **kwargs) 84 | -------------------------------------------------------------------------------- /ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kiss_icp VERSION 1.2.3 LANGUAGES CXX) 25 | 26 | set(CMAKE_BUILD_TYPE Release) 27 | 28 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp/ AND NOT IS_SYMLINK ${CMAKE_CURRENT_SOURCE_DIR}) 29 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_icp ${CMAKE_CURRENT_BINARY_DIR}/kiss_icp) 30 | else() 31 | cmake_minimum_required(VERSION 3.18) 32 | message(STATUS "Performing out-of-tree build, fetching KISS-ICP v${CMAKE_PROJECT_VERSION} Release from Github") 33 | include(FetchContent) 34 | FetchContent_Declare( 35 | ext_kiss_icp_core PREFIX kiss_icp_core 36 | URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR 37 | cpp/kiss_icp) 38 | FetchContent_MakeAvailable(ext_kiss_icp_core) 39 | endif() 40 | 41 | find_package(ament_cmake REQUIRED) 42 | find_package(geometry_msgs REQUIRED) 43 | find_package(nav_msgs REQUIRED) 44 | find_package(rclcpp REQUIRED) 45 | find_package(rclcpp_components REQUIRED) 46 | find_package(rcutils REQUIRED) 47 | find_package(sensor_msgs REQUIRED) 48 | find_package(std_msgs REQUIRED) 49 | find_package(tf2_ros REQUIRED) 50 | find_package(std_srvs REQUIRED) 51 | 52 | # ROS 2 node 53 | add_library(odometry_component SHARED src/OdometryServer.cpp) 54 | target_compile_features(odometry_component PUBLIC cxx_std_20) 55 | target_include_directories(odometry_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) 56 | target_link_libraries(odometry_component kiss_icp_pipeline) 57 | ament_target_dependencies( 58 | odometry_component 59 | geometry_msgs 60 | nav_msgs 61 | rclcpp 62 | rclcpp_components 63 | rcutils 64 | sensor_msgs 65 | std_msgs 66 | std_srvs 67 | tf2_ros) 68 | 69 | rclcpp_components_register_node(odometry_component PLUGIN "kiss_icp_ros::OdometryServer" EXECUTABLE kiss_icp_node) 70 | 71 | install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) 72 | install(DIRECTORY launch rviz config DESTINATION share/${PROJECT_NAME}/) 73 | 74 | ament_package() 75 | -------------------------------------------------------------------------------- /python/kiss_icp/tools/pipeline_results.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from dataclasses import dataclass 24 | from typing import Optional 25 | 26 | from rich import box 27 | from rich.console import Console 28 | from rich.table import Table 29 | 30 | 31 | class PipelineResults: 32 | def __init__(self) -> None: 33 | self._results = [] 34 | 35 | def empty(self) -> bool: 36 | return len(self._results) == 0 37 | 38 | def print(self) -> None: 39 | # TODO(Nacho): Add mechanism to print ASCII if rich is not available 40 | self.log_to_console() 41 | 42 | def append(self, desc: str, units: str, value: float, trunc: bool = False): 43 | @dataclass 44 | class Metric: 45 | desc: str 46 | units: str 47 | value: float 48 | 49 | self._results.append(Metric(desc, units, int(value) if trunc else value)) 50 | 51 | def log_to_file(self, filename: str, title: Optional[str]) -> None: 52 | with open(filename, "wt") as logfile: 53 | console = Console(file=logfile, width=100, force_jupyter=False) 54 | if title: 55 | console.rule(title) 56 | console.print(self._rich_table(table_format=box.ASCII_DOUBLE_HEAD)) 57 | 58 | def log_to_console(self) -> None: 59 | if self.empty(): 60 | return 61 | console = Console() 62 | console.print(self._rich_table()) 63 | 64 | def _rich_table(self, table_format: box.Box = box.HORIZONTALS) -> Table: 65 | """Takes a metrics dictionary and sptis a Rich Table with the experiment results""" 66 | table = Table(box=table_format) 67 | table.add_column("Metric", justify="right", style="cyan") 68 | table.add_column("Value", justify="center", style="magenta") 69 | table.add_column("Units", justify="left", style="green") 70 | for result in self._results: 71 | table.add_row( 72 | result.desc, 73 | f"{result.value:{'.3f' if isinstance(result.value, float) else ''}}", 74 | result.units, 75 | ) 76 | return table 77 | 78 | def __iter__(self): 79 | return self._results.__iter__() 80 | -------------------------------------------------------------------------------- /python/kiss_icp/kiss_icp.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | 25 | from kiss_icp.config import KISSConfig 26 | from kiss_icp.mapping import get_voxel_hash_map 27 | from kiss_icp.preprocess import get_preprocessor 28 | from kiss_icp.registration import get_registration 29 | from kiss_icp.threshold import get_threshold_estimator 30 | from kiss_icp.voxelization import voxel_down_sample 31 | 32 | 33 | class KissICP: 34 | def __init__(self, config: KISSConfig): 35 | self.last_pose = np.eye(4) 36 | self.last_delta = np.eye(4) 37 | self.config = config 38 | self.adaptive_threshold = get_threshold_estimator(self.config) 39 | self.preprocessor = get_preprocessor(self.config) 40 | self.registration = get_registration(self.config) 41 | self.local_map = get_voxel_hash_map(self.config) 42 | 43 | def register_frame(self, frame, timestamps): 44 | # Apply motion compensation 45 | frame = self.preprocessor.preprocess(frame, timestamps, self.last_delta) 46 | 47 | # Voxelize 48 | source, frame_downsample = self.voxelize(frame) 49 | 50 | # Get adaptive_threshold 51 | sigma = self.adaptive_threshold.get_threshold() 52 | 53 | # Compute initial_guess for ICP 54 | initial_guess = self.last_pose @ self.last_delta 55 | 56 | # Run ICP 57 | new_pose = self.registration.align_points_to_map( 58 | points=source, 59 | voxel_map=self.local_map, 60 | initial_guess=initial_guess, 61 | max_correspondance_distance=3 * sigma, 62 | kernel=sigma, 63 | ) 64 | 65 | # Compute the difference between the prediction and the actual estimate 66 | model_deviation = np.linalg.inv(initial_guess) @ new_pose 67 | 68 | # Update step: threshold, local map, delta, and the last pose 69 | self.adaptive_threshold.update_model_deviation(model_deviation) 70 | self.local_map.update(frame_downsample, new_pose) 71 | self.last_delta = np.linalg.inv(self.last_pose) @ new_pose 72 | self.last_pose = new_pose 73 | 74 | # Return the (deskew) input raw scan (frame) and the points used for registration (source) 75 | return frame, source 76 | 77 | def voxelize(self, iframe): 78 | frame_downsample = voxel_down_sample(iframe, self.config.mapping.voxel_size * 0.5) 79 | source = voxel_down_sample(frame_downsample, self.config.mapping.voxel_size * 1.5) 80 | return source, frame_downsample 81 | -------------------------------------------------------------------------------- /python/kiss_icp/config/parser.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # NOTE: This module was contributed by Markus Pielmeier on PR #63 24 | from __future__ import annotations 25 | 26 | import importlib 27 | import sys 28 | from pathlib import Path 29 | from typing import Any, Dict, Optional 30 | 31 | from pydantic_settings import BaseSettings, SettingsConfigDict 32 | 33 | from kiss_icp.config.config import ( 34 | AdaptiveThresholdConfig, 35 | DataConfig, 36 | MappingConfig, 37 | RegistrationConfig, 38 | ) 39 | 40 | 41 | class KISSConfig(BaseSettings): 42 | model_config = SettingsConfigDict(env_prefix="kiss_icp_") 43 | out_dir: str = "results" 44 | data: DataConfig = DataConfig() 45 | registration: RegistrationConfig = RegistrationConfig() 46 | mapping: MappingConfig = MappingConfig() 47 | adaptive_threshold: AdaptiveThresholdConfig = AdaptiveThresholdConfig() 48 | 49 | 50 | def _yaml_source(config_file: Optional[Path]) -> Dict[str, Any]: 51 | data = None 52 | if config_file is not None: 53 | try: 54 | yaml = importlib.import_module("yaml") 55 | except ModuleNotFoundError: 56 | print( 57 | "Custom configuration file specified but PyYAML is not installed on your system," 58 | ' run `pip install "kiss-icp[all]"`. You can also modify the config.py if your ' 59 | "system does not support PyYaml " 60 | ) 61 | sys.exit(1) 62 | with open(config_file) as cfg_file: 63 | data = yaml.safe_load(cfg_file) 64 | return data or {} 65 | 66 | 67 | def load_config(config_file: Optional[Path]) -> KISSConfig: 68 | """Load configuration from an optional yaml file.""" 69 | 70 | config = KISSConfig(**_yaml_source(config_file)) 71 | 72 | # Check if there is a possible mistake 73 | if config.data.max_range < config.data.min_range: 74 | print("[WARNING] max_range is smaller than min_range, settng min_range to 0.0") 75 | config.data.min_range = 0.0 76 | 77 | # Use specified voxel size or compute one using the max range 78 | if config.mapping.voxel_size is None: 79 | config.mapping.voxel_size = float(config.data.max_range / 100.0) 80 | 81 | return config 82 | 83 | 84 | def write_config(config: KISSConfig = KISSConfig(), filename: str = "kiss_icp.yaml"): 85 | with open(filename, "w") as outfile: 86 | try: 87 | yaml = importlib.import_module("yaml") 88 | yaml.dump(config.model_dump(), outfile, default_flow_style=False) 89 | except ModuleNotFoundError: 90 | outfile.write(str(config.model_dump())) 91 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/tum.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | from pathlib import Path 26 | 27 | import numpy as np 28 | from pyquaternion import Quaternion 29 | 30 | 31 | class TUMDataset: 32 | def __init__(self, data_dir: Path, *_, **__): 33 | try: 34 | self.o3d = importlib.import_module("open3d") 35 | except ModuleNotFoundError as err: 36 | print(f'open3d is not installed on your system, run "pip install open3d"') 37 | exit(1) 38 | 39 | self.data_dir = Path(data_dir) 40 | self.sequence_id = os.path.basename(data_dir) 41 | 42 | # Load depth frames 43 | self.depth_frames = np.loadtxt(fname=self.data_dir / "depth.txt", dtype=str) 44 | 45 | # rgb single frame 46 | rgb_path = os.path.join(self.data_dir, "rgb", os.listdir(self.data_dir / "rgb")[0]) 47 | self.rgb_default_frame = self.o3d.io.read_image(rgb_path) 48 | 49 | # Load GT poses 50 | gt_list = np.loadtxt(fname=self.data_dir / "groundtruth.txt", dtype=str) 51 | self.gt_poses = self.load_poses(gt_list) 52 | 53 | def __len__(self): 54 | return len(self.depth_frames) 55 | 56 | def load_poses(self, gt_list): 57 | indices = np.unique( 58 | np.abs( 59 | ( 60 | np.subtract.outer( 61 | gt_list[:, 0].astype(np.float64), 62 | self.depth_frames[:, 0].astype(np.float64), 63 | ) 64 | ) 65 | ).argmin(0) 66 | ) 67 | xyz = gt_list[indices][:, 1:4] 68 | 69 | rotations = np.array( 70 | [ 71 | Quaternion(x=x, y=y, z=z, w=w).rotation_matrix 72 | for x, y, z, w in gt_list[indices][:, 4:] 73 | ] 74 | ) 75 | num_poses = rotations.shape[0] 76 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 77 | poses[:, :3, :3] = rotations 78 | poses[:, :3, 3] = xyz 79 | return poses 80 | 81 | def get_frames_timestamps(self): 82 | return self.depth_frames[:, 0] 83 | 84 | def __getitem__(self, idx): 85 | depth_id = self.depth_frames[idx][-1] 86 | depth_raw = self.o3d.io.read_image(str(self.data_dir / depth_id)) 87 | rgbd_image = self.o3d.geometry.RGBDImage.create_from_tum_format( 88 | self.rgb_default_frame, depth_raw 89 | ) 90 | pcd = self.o3d.geometry.PointCloud.create_from_rgbd_image( 91 | rgbd_image, 92 | self.o3d.camera.PinholeCameraIntrinsic( 93 | self.o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault 94 | ), 95 | ) 96 | return np.array(pcd.points, dtype=np.float64), np.array([]) 97 | -------------------------------------------------------------------------------- /cpp/kiss_icp/pipeline/KissICP.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 3 | // Stachniss. 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 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "kiss_icp/core/Preprocessing.hpp" 30 | #include "kiss_icp/core/Registration.hpp" 31 | #include "kiss_icp/core/Threshold.hpp" 32 | #include "kiss_icp/core/VoxelHashMap.hpp" 33 | 34 | namespace kiss_icp::pipeline { 35 | 36 | struct KISSConfig { 37 | // map params 38 | double voxel_size = 1.0; 39 | double max_range = 100.0; 40 | double min_range = 0.0; 41 | int max_points_per_voxel = 20; 42 | 43 | // th parms 44 | double min_motion_th = 0.1; 45 | double initial_threshold = 2.0; 46 | 47 | // registration params 48 | int max_num_iterations = 500; 49 | double convergence_criterion = 0.0001; 50 | int max_num_threads = 0; 51 | 52 | // Motion compensation 53 | bool deskew = true; 54 | }; 55 | 56 | class KissICP { 57 | public: 58 | using Vector3dVector = std::vector; 59 | using Vector3dVectorTuple = std::tuple; 60 | 61 | public: 62 | explicit KissICP(const KISSConfig &config) 63 | : config_(config), 64 | preprocessor_(config.max_range, config.min_range, config.deskew, config.max_num_threads), 65 | registration_( 66 | config.max_num_iterations, config.convergence_criterion, config.max_num_threads), 67 | local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel), 68 | adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} 69 | 70 | public: 71 | Vector3dVectorTuple RegisterFrame(const std::vector &frame, 72 | const std::vector ×tamps); 73 | Vector3dVectorTuple Voxelize(const std::vector &frame) const; 74 | 75 | std::vector LocalMap() const { return local_map_.Pointcloud(); }; 76 | 77 | const VoxelHashMap &VoxelMap() const { return local_map_; }; 78 | VoxelHashMap &VoxelMap() { return local_map_; }; 79 | 80 | const Sophus::SE3d &pose() const { return last_pose_; } 81 | Sophus::SE3d &pose() { return last_pose_; } 82 | 83 | const Sophus::SE3d &delta() const { return last_delta_; } 84 | Sophus::SE3d &delta() { return last_delta_; } 85 | void Reset(); 86 | 87 | private: 88 | Sophus::SE3d last_pose_; 89 | Sophus::SE3d last_delta_; 90 | 91 | // KISS-ICP pipeline modules 92 | KISSConfig config_; 93 | Preprocessor preprocessor_; 94 | Registration registration_; 95 | VoxelHashMap local_map_; 96 | AdaptiveThreshold adaptive_threshold_; 97 | }; 98 | 99 | } // namespace kiss_icp::pipeline 100 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/boreas.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | from pathlib import Path 26 | 27 | import natsort 28 | import numpy as np 29 | 30 | 31 | class BoreasDataset: 32 | def __init__(self, data_dir: Path, *_, **__): 33 | self.root_dir = os.path.realpath(data_dir) 34 | self.scan_files = natsort.natsorted(glob.glob(f"{data_dir}/lidar/*.bin")) 35 | self.gt_poses = self.load_poses(f"{data_dir}/applanix/lidar_poses.csv") 36 | self.sequence_id = os.path.basename(data_dir) 37 | assert len(self.scan_files) == self.gt_poses.shape[0] 38 | 39 | def __len__(self): 40 | return len(self.scan_files) 41 | 42 | def __getitem__(self, idx): 43 | return self.read_point_cloud(self.scan_files[idx]) 44 | 45 | def read_point_cloud(self, scan_file: str): 46 | points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 6))[:, :3] 47 | return points.astype(np.float64), self.get_timestamps(points) 48 | 49 | def load_poses(self, poses_file): 50 | data = np.loadtxt(poses_file, delimiter=",", skiprows=1) 51 | n, m = data.shape 52 | t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[0, :] 53 | first_pose = self.get_transformation_matrix(x, y, z, ya, p, r) 54 | poses = np.empty((n, 4, 4), dtype=np.float32) 55 | poses[0, :, :] = np.identity(4, dtype=np.float32) 56 | for i in range(n): 57 | t, x, y, z, vx, vy, vz, r, p, ya, wz, wy, wx = data[i, :] 58 | current_pose = self.get_transformation_matrix(x, y, z, ya, p, r) 59 | poses[i, :, :] = np.linalg.inv(first_pose) @ current_pose 60 | return poses 61 | 62 | @staticmethod 63 | def get_timestamps(points): 64 | x = points[:, 0] 65 | y = points[:, 1] 66 | yaw = -np.arctan2(y, x) 67 | timestamps = 0.5 * (yaw / np.pi + 1.0) 68 | return timestamps 69 | 70 | @staticmethod 71 | def get_transformation_matrix(x, y, z, yaw, pitch, roll): 72 | T_enu_sensor = np.identity(4, dtype=np.float64) 73 | R_yaw = np.array( 74 | [[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]], 75 | dtype=np.float64, 76 | ) 77 | R_pitch = np.array( 78 | [[np.cos(pitch), 0, -np.sin(pitch)], [0, 1, 0], [np.sin(pitch), 0, np.cos(pitch)]], 79 | dtype=np.float64, 80 | ) 81 | R_roll = np.array( 82 | [[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]], 83 | dtype=np.float64, 84 | ) 85 | C_enu_sensor = R_roll @ R_pitch @ R_yaw 86 | T_enu_sensor[:3, :3] = C_enu_sensor 87 | r_sensor_enu_in_enu = np.array([x, y, z]).reshape(3, 1) 88 | T_enu_sensor[:3, 3:] = r_sensor_enu_in_enu 89 | return T_enu_sensor 90 | -------------------------------------------------------------------------------- /cpp/kiss_icp/pipeline/KissICP.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | 24 | #include "KissICP.hpp" 25 | 26 | #include 27 | #include 28 | 29 | #include "kiss_icp/core/Preprocessing.hpp" 30 | #include "kiss_icp/core/Registration.hpp" 31 | #include "kiss_icp/core/VoxelHashMap.hpp" 32 | 33 | namespace kiss_icp::pipeline { 34 | 35 | KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector &frame, 36 | const std::vector ×tamps) { 37 | // Preprocess the input cloud 38 | const auto &preprocessed_frame = preprocessor_.Preprocess(frame, timestamps, last_delta_); 39 | 40 | // Voxelize 41 | const auto &[source, frame_downsample] = Voxelize(preprocessed_frame); 42 | 43 | // Get adaptive_threshold 44 | const double sigma = adaptive_threshold_.ComputeThreshold(); 45 | 46 | // Compute initial_guess for ICP 47 | const auto initial_guess = last_pose_ * last_delta_; 48 | 49 | // Run ICP 50 | const auto new_pose = registration_.AlignPointsToMap(source, // frame 51 | local_map_, // voxel_map 52 | initial_guess, // initial_guess 53 | 3.0 * sigma, // max_correspondence_dist 54 | sigma); // kernel 55 | 56 | // Compute the difference between the prediction and the actual estimate 57 | const auto model_deviation = initial_guess.inverse() * new_pose; 58 | 59 | // Update step: threshold, local map, delta, and the last pose 60 | adaptive_threshold_.UpdateModelDeviation(model_deviation); 61 | local_map_.Update(frame_downsample, new_pose); 62 | last_delta_ = last_pose_.inverse() * new_pose; 63 | last_pose_ = new_pose; 64 | 65 | // Return the (deskew) input raw scan (preprocessed_frame) and the points used for registration 66 | // (source) 67 | return {preprocessed_frame, source}; 68 | } 69 | 70 | KissICP::Vector3dVectorTuple KissICP::Voxelize(const std::vector &frame) const { 71 | const auto voxel_size = config_.voxel_size; 72 | const auto frame_downsample = kiss_icp::VoxelDownsample(frame, voxel_size * 0.5); 73 | const auto source = kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5); 74 | return {source, frame_downsample}; 75 | } 76 | void KissICP::Reset() { 77 | last_pose_ = Sophus::SE3d(); 78 | last_delta_ = Sophus::SE3d(); 79 | 80 | // Clear the local map 81 | local_map_.Clear(); 82 | 83 | // Reset adaptive threshold (it will start fresh) 84 | adaptive_threshold_ = 85 | AdaptiveThreshold(config_.initial_threshold, config_.min_motion_th, config_.max_range); 86 | } 87 | 88 | } // namespace kiss_icp::pipeline 89 | -------------------------------------------------------------------------------- /ros/src/OdometryServer.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | // KISS-ICP 26 | #include "kiss_icp/pipeline/KissICP.hpp" 27 | 28 | // ROS 2 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace kiss_icp_ros { 40 | 41 | class OdometryServer : public rclcpp::Node { 42 | public: 43 | /// OdometryServer constructor 44 | OdometryServer() = delete; 45 | explicit OdometryServer(const rclcpp::NodeOptions &options); 46 | 47 | private: 48 | /// Declare ROS parameters and set the associated variables (in this class and in the provided 49 | /// config object) 50 | void initializeParameters(kiss_icp::pipeline::KISSConfig &config); 51 | 52 | /// Register new frame 53 | void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 54 | 55 | /// Stream the estimated pose to ROS 56 | void PublishOdometry(const Sophus::SE3d &kiss_pose, const std_msgs::msg::Header &header); 57 | 58 | /// Stream the debugging point clouds for visualization (if required) 59 | void PublishClouds(const std::vector &frame, 60 | const std::vector &keypoints, 61 | const std_msgs::msg::Header &header); 62 | void ResetService(const std::shared_ptr request, 63 | std::shared_ptr response); 64 | 65 | private: 66 | /// Tools for broadcasting TFs. 67 | std::unique_ptr tf_broadcaster_; 68 | std::unique_ptr tf2_buffer_; 69 | std::unique_ptr tf2_listener_; 70 | bool invert_odom_tf_; 71 | bool publish_odom_tf_; 72 | bool publish_debug_clouds_; 73 | 74 | /// Data subscribers. 75 | rclcpp::Subscription::SharedPtr pointcloud_sub_; 76 | 77 | /// Data publishers. 78 | rclcpp::Publisher::SharedPtr odom_publisher_; 79 | rclcpp::Publisher::SharedPtr frame_publisher_; 80 | rclcpp::Publisher::SharedPtr kpoints_publisher_; 81 | rclcpp::Publisher::SharedPtr map_publisher_; 82 | 83 | /// Service servers. 84 | rclcpp::Service::SharedPtr reset_service_; 85 | 86 | /// KISS-ICP 87 | std::unique_ptr kiss_icp_; 88 | 89 | /// Global/map coordinate frame. 90 | std::string lidar_odom_frame_{"odom_lidar"}; 91 | std::string base_frame_{}; 92 | 93 | /// Covariance diagonal 94 | double position_covariance_; 95 | double orientation_covariance_; 96 | }; 97 | 98 | } // namespace kiss_icp_ros 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

KISS-ICP

3 | 4 | 5 | 6 | 7 | 8 |
9 |
10 | Demo 11 |   •   12 | Install 13 |   •   14 | ROS 2 15 |   •   16 | Paper 17 |   •   18 | Contact Us 19 |
20 |
21 | 22 | [KISS-ICP](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf) is a LiDAR Odometry pipeline that **just works** on most of the cases without tunning any parameter. 23 | 24 |

25 | KISS-ICP Demo 26 |

27 |
28 | 29 |
30 | 31 | ## Install 32 | 33 | ```sh 34 | pip install kiss-icp 35 | ``` 36 | 37 | Next, follow the instructions on how to run the system by typing: 38 | 39 | ```sh 40 | kiss_icp_pipeline --help 41 | ``` 42 | 43 |
44 | This should print the following help message: 45 | 46 | ![out](https://github.com/user-attachments/assets/7dea767f-d0e4-4f6b-a523-ba3be25bbfae) 47 | 48 |
49 | 50 | For advanced instructions on the Python package please see [this README](python/README.md) 51 | 52 | ## ROS support 53 | 54 |
55 | ROS 2 56 | 57 | ```sh 58 | cd ~/ros2_ws/src/ && git clone https://github.com/PRBonn/kiss-icp && cd ~/ros2_ws/ && colcon build --packages-select kiss_icp 59 | ``` 60 | For more detailed instructions on the ROS wrapper, please visit this [README](ros/README.md) 61 | 62 |
63 | 64 |
65 | ROS 1 66 | 67 | ⚠️ ⚠️ **ROS 1 is deprecated in KISS-ICP and is not officially supported anymore. Upgrade now to ROS 2!** ⚠️ ⚠️ 68 | 69 | The last release that supports ROS 1 is [v0.3.0](https://github.com/PRBonn/kiss-icp/tree/v0.3.0), if you still need ROS 1 support please check that version. 70 | 71 |
72 | 73 | 74 | ## Citation 75 | 76 | If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf). 77 | 78 | ```bibtex 79 | @article{vizzo2023ral, 80 | author = {Vizzo, Ignacio and Guadagnino, Tiziano and Mersch, Benedikt and Wiesmann, Louis and Behley, Jens and Stachniss, Cyrill}, 81 | title = {{KISS-ICP: In Defense of Point-to-Point ICP -- Simple, Accurate, and Robust Registration If Done the Right Way}}, 82 | journal = {IEEE Robotics and Automation Letters (RA-L)}, 83 | pages = {1029--1036}, 84 | doi = {10.1109/LRA.2023.3236571}, 85 | volume = {8}, 86 | number = {2}, 87 | year = {2023}, 88 | codeurl = {https://github.com/PRBonn/kiss-icp}, 89 | } 90 | ``` 91 | 92 | ## Contributing 93 | 94 | We envision KISS-ICP as a community-driven project, we love to see how the project is growing thanks to the contributions from the community. We would love to see your face in the list below, just open a Pull Request! 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /eval/kiss_icp_eval.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Cyrill Stachniss, University of Bonn 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 | from dataclasses import dataclass 23 | from typing import Callable, Dict, List 24 | 25 | import matplotlib.pyplot as plt 26 | import numpy as np 27 | from evo.core.trajectory import PosePath3D 28 | from evo.tools import plot 29 | from evo.tools.settings import SETTINGS 30 | from IPython.display import display_markdown 31 | 32 | from kiss_icp.pipeline import OdometryPipeline 33 | 34 | 35 | @dataclass 36 | class Metric: 37 | units: str 38 | values: List 39 | 40 | 41 | def run_sequence(kiss_pipeline: Callable, results: Dict, **kwargs): 42 | # Create pipeline object 43 | pipeline: OdometryPipeline = kiss_pipeline(kwargs.pop("sequence")) 44 | 45 | # New entry to the results dictionary 46 | results.setdefault("dataset_name", pipeline.dataset_name) 47 | 48 | # Run pipeline 49 | print(f"Now evaluating sequence {pipeline.dataset_sequence}") 50 | seq_res = pipeline.run() 51 | seq_res.print() 52 | 53 | # Update the metrics dictionary 54 | for result in seq_res: 55 | results.setdefault("metrics", {}).setdefault( 56 | result.desc, Metric(result.units, []) 57 | ).values.append(result.value) 58 | 59 | # Update the trajectories results 60 | results.setdefault("trajectories", {}).update( 61 | { 62 | pipeline.dataset_sequence: { 63 | "gt_poses": pipeline.gt_poses, 64 | "poses": np.asarray(pipeline.poses).reshape(len(pipeline.poses), 4, 4), 65 | } 66 | } 67 | ) 68 | 69 | 70 | def print_metrics_table(results: Dict, title: str = "") -> None: 71 | """Takes a results dictionary and spits a Markdwon table into the notebook""" 72 | table_results = f"# Experiment Results {title}\n|Metric|Value|Units|\n|-:|:-:|:-|\n" 73 | for metric, result in results["metrics"].items(): 74 | table_results += f"{metric}| {np.mean(result.values):.2f}|{result.units} |\n" 75 | display_markdown(table_results, raw=True) 76 | 77 | 78 | def plot_trajectories(results: Dict, close_all: bool = True) -> None: 79 | if close_all: 80 | plt.close("all") 81 | for sequence, trajectory in results["trajectories"].items(): 82 | poses = PosePath3D(poses_se3=trajectory["poses"]) 83 | gt_poses = PosePath3D(poses_se3=trajectory["gt_poses"]) 84 | 85 | plot_mode = plot.PlotMode.xyz 86 | fig = plt.figure(f"Trajectory results for {results['dataset_name']} {sequence}") 87 | ax = plot.prepare_axis(fig, plot_mode) 88 | plot.traj( 89 | ax=ax, 90 | plot_mode=plot_mode, 91 | traj=gt_poses, 92 | label="ground truth", 93 | style=SETTINGS.plot_reference_linestyle, 94 | color=SETTINGS.plot_reference_color, 95 | alpha=SETTINGS.plot_reference_alpha, 96 | ) 97 | plot.traj( 98 | ax=ax, 99 | plot_mode=plot_mode, 100 | traj=poses, 101 | label="KISS-ICP", 102 | style=SETTINGS.plot_trajectory_linestyle, 103 | color="#4c72b0bf", 104 | alpha=SETTINGS.plot_trajectory_alpha, 105 | ) 106 | 107 | ax.legend(frameon=True) 108 | ax.set_title(f"Sequence {sequence}") 109 | plt.show() 110 | -------------------------------------------------------------------------------- /cpp/kiss_icp/core/Preprocessing.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Preprocessing.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace kiss_icp { 39 | 40 | Preprocessor::Preprocessor(const double max_range, 41 | const double min_range, 42 | const bool deskew, 43 | const int max_num_threads) 44 | : max_range_(max_range), 45 | min_range_(min_range), 46 | deskew_(deskew), 47 | max_num_threads_(max_num_threads > 0 ? max_num_threads 48 | : tbb::this_task_arena::max_concurrency()) { 49 | // This global variable requires static duration storage to be able to manipulate the max 50 | // concurrency from TBB across the entire class 51 | static const auto tbb_control_settings = tbb::global_control( 52 | tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); 53 | } 54 | 55 | std::vector Preprocessor::Preprocess(const std::vector &frame, 56 | const std::vector ×tamps, 57 | const Sophus::SE3d &relative_motion) const { 58 | const std::vector &deskewed_frame = [&]() { 59 | if (!deskew_ || timestamps.empty()) { 60 | return frame; 61 | } else { 62 | const auto &[min, max] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); 63 | const double min_time = *min; 64 | const double max_time = *max; 65 | const auto normalize = [&](const double t) { 66 | return (t - min_time) / (max_time - min_time); 67 | }; 68 | const auto &omega = relative_motion.log(); 69 | std::vector deskewed_frame(frame.size()); 70 | tbb::parallel_for( 71 | // Index Range 72 | tbb::blocked_range{0, deskewed_frame.size()}, 73 | // Parallel Compute 74 | [&](const tbb::blocked_range &r) { 75 | for (size_t idx = r.begin(); idx < r.end(); ++idx) { 76 | const auto &point = frame.at(idx); 77 | const auto &stamp = normalize(timestamps.at(idx)); 78 | const auto pose = Sophus::SE3d::exp((stamp - 1.0) * omega); 79 | deskewed_frame.at(idx) = pose * point; 80 | }; 81 | }); 82 | return deskewed_frame; 83 | } 84 | }(); 85 | std::vector preprocessed_frame; 86 | preprocessed_frame.reserve(deskewed_frame.size()); 87 | std::for_each(deskewed_frame.cbegin(), deskewed_frame.cend(), [&](const auto &point) { 88 | const double point_range = point.norm(); 89 | if (point_range < max_range_ && point_range > min_range_) { 90 | preprocessed_frame.emplace_back(point); 91 | } 92 | }); 93 | preprocessed_frame.shrink_to_fit(); 94 | return preprocessed_frame; 95 | } 96 | } // namespace kiss_icp 97 | -------------------------------------------------------------------------------- /eval/kitti_raw.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "id": "cdbdaad6", 6 | "metadata": {}, 7 | "source": [ 8 | "# KISS-ICP running on the KITTI-Raw benchmark dataset\n", 9 | "\n", 10 | "The whole purpose of this notebook is to have a reproducable entry point for the experiments of the paper. You can also modify the system and just run this notebook to inspect the overall results.\n", 11 | "\n", 12 | "The main ingredient of this experiment is to test the \"deskew\" algoirthm we use.\n", 13 | "\n", 14 | "## Expected dataset layout\n", 15 | "\n", 16 | "```sh\n", 17 | "\n", 18 | "├── 2011_09_26\n", 19 | "│   ├── calib_cam_to_cam.txt\n", 20 | "│   ├── calib_imu_to_velo.txt\n", 21 | "│   └── calib_velo_to_cam.txt\n", 22 | "├── 2011_09_30\n", 23 | "│   ├── 2011_09_30_drive_0016_sync\n", 24 | "│   │   ├── image_00\n", 25 | "│   │   ├── image_01\n", 26 | "│   │   ├── image_02\n", 27 | "│   │   ├── image_03\n", 28 | "│   │   ├── oxts\n", 29 | "│   │   └── velodyne_points\n", 30 | "```" 31 | ] 32 | }, 33 | { 34 | "cell_type": "code", 35 | "execution_count": null, 36 | "id": "584cfb31", 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "# Install KISS-ICP and Plotting tools\n", 41 | "%pip install kiss-icp ipympl evo >/dev/null\n", 42 | "\n", 43 | "import os\n", 44 | "from pathlib import Path\n", 45 | "\n", 46 | "import kiss_icp\n", 47 | "import matplotlib.pyplot as plt\n", 48 | "import numpy as np\n", 49 | "from evo.tools import plot\n", 50 | "from kiss_icp.datasets import dataset_factory\n", 51 | "from kiss_icp.pipeline import OdometryPipeline\n", 52 | "\n", 53 | "%autoreload 2\n", 54 | "%matplotlib inline\n", 55 | "%matplotlib widget" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "id": "676a6a26", 62 | "metadata": {}, 63 | "outputs": [], 64 | "source": [ 65 | "data_root = os.environ.get(\"DATASETS\")\n", 66 | "kitti_root = Path(os.path.join(data_root, \"kitti-odometry/raw\"))\n", 67 | "print(f\"Reading datasets from : {data_root}\")" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "id": "a7795f59", 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "from kiss_icp_eval import run_sequence\n", 78 | "\n", 79 | "\n", 80 | "def kitti_raw_sequence(sequence: int):\n", 81 | " return OdometryPipeline(\n", 82 | " dataset=dataset_factory(\n", 83 | " dataloader=\"kitti_raw\",\n", 84 | " data_dir=kitti_root,\n", 85 | " sequence=sequence,\n", 86 | " ),\n", 87 | " deskew=True,\n", 88 | " )\n", 89 | "\n", 90 | "\n", 91 | "results = {}\n", 92 | "for sequence in range(0, 11):\n", 93 | " if sequence == 3:\n", 94 | " continue\n", 95 | " run_sequence(kitti_raw_sequence, sequence=sequence, results=results)" 96 | ] 97 | }, 98 | { 99 | "cell_type": "code", 100 | "execution_count": null, 101 | "id": "bb9e4e09", 102 | "metadata": {}, 103 | "outputs": [], 104 | "source": [ 105 | "from kiss_icp_eval import print_metrics_table\n", 106 | "\n", 107 | "print_metrics_table(results)" 108 | ] 109 | }, 110 | { 111 | "cell_type": "markdown", 112 | "id": "1de1786d", 113 | "metadata": {}, 114 | "source": [ 115 | "## Trajectories Results" 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": null, 121 | "id": "1747d471", 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "from kiss_icp_eval import plot_trajectories\n", 126 | "\n", 127 | "plot_trajectories(results)" 128 | ] 129 | } 130 | ], 131 | "metadata": { 132 | "kernelspec": { 133 | "display_name": "Python 3 (ipykernel)", 134 | "language": "python", 135 | "name": "python3" 136 | }, 137 | "language_info": { 138 | "codemirror_mode": { 139 | "name": "ipython", 140 | "version": 3 141 | }, 142 | "file_extension": ".py", 143 | "mimetype": "text/x-python", 144 | "name": "python", 145 | "nbconvert_exporter": "python", 146 | "pygments_lexer": "ipython3", 147 | "version": "3.10.6" 148 | }, 149 | "vscode": { 150 | "interpreter": { 151 | "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6" 152 | } 153 | } 154 | }, 155 | "nbformat": 4, 156 | "nbformat_minor": 5 157 | } 158 | -------------------------------------------------------------------------------- /ros/launch/odometry.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import ExecuteProcess 28 | from launch.conditions import IfCondition 29 | from launch.substitutions import ( 30 | LaunchConfiguration, 31 | PathJoinSubstitution, 32 | PythonExpression, 33 | ) 34 | from launch_ros.actions import Node 35 | from launch_ros.substitutions import FindPackageShare 36 | 37 | PACKAGE_NAME = "kiss_icp" 38 | 39 | default_config_file = os.path.join( 40 | get_package_share_directory(PACKAGE_NAME), "config", "config.yaml" 41 | ) 42 | 43 | 44 | def generate_launch_description(): 45 | use_sim_time = LaunchConfiguration("use_sim_time", default="true") 46 | 47 | # ROS configuration 48 | pointcloud_topic = LaunchConfiguration("topic") 49 | visualize = LaunchConfiguration("visualize", default="true") 50 | 51 | # Optional ros bag play 52 | bagfile = LaunchConfiguration("bagfile", default="") 53 | 54 | # tf tree configuration, these are the likely parameters to change and nothing else 55 | base_frame = LaunchConfiguration("base_frame", default="") # (base_link/base_footprint) 56 | lidar_odom_frame = LaunchConfiguration("lidar_odom_frame", default="odom_lidar") 57 | publish_odom_tf = LaunchConfiguration("publish_odom_tf", default=True) 58 | invert_odom_tf = LaunchConfiguration("invert_odom_tf", default=True) 59 | 60 | position_covariance = LaunchConfiguration("position_covariance", default=0.1) 61 | orientation_covariance = LaunchConfiguration("orientation_covariance", default=0.1) 62 | 63 | config_file = LaunchConfiguration("config_file", default=default_config_file) 64 | 65 | # KISS-ICP node 66 | kiss_icp_node = Node( 67 | package=PACKAGE_NAME, 68 | executable="kiss_icp_node", 69 | name="kiss_icp_node", 70 | output="screen", 71 | remappings=[ 72 | ("pointcloud_topic", pointcloud_topic), 73 | ], 74 | parameters=[ 75 | { 76 | # ROS node configuration 77 | "base_frame": base_frame, 78 | "lidar_odom_frame": lidar_odom_frame, 79 | "publish_odom_tf": publish_odom_tf, 80 | "invert_odom_tf": invert_odom_tf, 81 | # ROS CLI arguments 82 | "publish_debug_clouds": visualize, 83 | "use_sim_time": use_sim_time, 84 | "position_covariance": position_covariance, 85 | "orientation_covariance": orientation_covariance, 86 | }, 87 | config_file, 88 | ], 89 | ) 90 | rviz_node = Node( 91 | package="rviz2", 92 | executable="rviz2", 93 | output="screen", 94 | arguments=[ 95 | "-d", 96 | PathJoinSubstitution([FindPackageShare(PACKAGE_NAME), "rviz", "kiss_icp.rviz"]), 97 | ], 98 | condition=IfCondition(visualize), 99 | ) 100 | 101 | bagfile_play = ExecuteProcess( 102 | cmd=["ros2", "bag", "play", "--rate", "1", bagfile, "--clock", "1000.0"], 103 | output="screen", 104 | condition=IfCondition(PythonExpression(["'", bagfile, "' != ''"])), 105 | ) 106 | 107 | return LaunchDescription( 108 | [ 109 | kiss_icp_node, 110 | rviz_node, 111 | bagfile_play, 112 | ] 113 | ) 114 | -------------------------------------------------------------------------------- /eval/kitti.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "id": "4262e2f2", 6 | "metadata": {}, 7 | "source": [ 8 | "# KISS-ICP running on the KITTI-Odometry benchmark dataset\n", 9 | "\n", 10 | "The whole purpose of this notebook is to have a reproducable entry point for the experiments of the paper. You can also modify the system and just run this notebook to inspect the overall results\n", 11 | "\n", 12 | "## Expected dataset layout\n", 13 | "\n", 14 | "```\n", 15 | "\n", 16 | "├── poses\n", 17 | "│   ├── 00.txt\n", 18 | "│   ├── 01.txt\n", 19 | "│   ├── 02.txt\n", 20 | "│   ├── 03.txt\n", 21 | "│   ├── 04.txt\n", 22 | "│   ├── 05.txt\n", 23 | "│   ├── 06.txt\n", 24 | "│   ├── 07.txt\n", 25 | "│   ├── 08.txt\n", 26 | "│   ├── 09.txt\n", 27 | "│   ├── 10.txt\n", 28 | "└── sequences\n", 29 | " ├── 00\n", 30 | " │   ├── calib.txt\n", 31 | " │   ├── poses.txt\n", 32 | " │   ├── times.txt\n", 33 | " │   └── velodyne\n", 34 | " ├── 01\n", 35 | " │   ├── ....\n", 36 | "\n", 37 | "```" 38 | ] 39 | }, 40 | { 41 | "cell_type": "code", 42 | "execution_count": null, 43 | "id": "50ef1421", 44 | "metadata": {}, 45 | "outputs": [], 46 | "source": [ 47 | "# Install KISS-ICP and Plotting tools\n", 48 | "%pip install kiss-icp ipympl evo >/dev/null\n", 49 | "\n", 50 | "import os\n", 51 | "\n", 52 | "import kiss_icp\n", 53 | "import matplotlib.pyplot as plt\n", 54 | "import numpy as np\n", 55 | "from evo.tools import plot\n", 56 | "from kiss_icp.datasets import dataset_factory\n", 57 | "from kiss_icp.pipeline import OdometryPipeline\n", 58 | "\n", 59 | "%autoreload 2\n", 60 | "%matplotlib inline\n", 61 | "%matplotlib widget" 62 | ] 63 | }, 64 | { 65 | "cell_type": "code", 66 | "execution_count": null, 67 | "id": "0e7c52d1", 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "data_root = os.environ.get(\"DATASETS\")\n", 72 | "kitti_root = os.path.join(data_root, \"kitti-odometry/dataset\")\n", 73 | "print(f\"Reading datasets from : {data_root}\")" 74 | ] 75 | }, 76 | { 77 | "cell_type": "markdown", 78 | "id": "882e36f3", 79 | "metadata": {}, 80 | "source": [ 81 | "## Run all sequences on the dataset" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "id": "fd62b33e", 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "from kiss_icp_eval import run_sequence\n", 92 | "\n", 93 | "\n", 94 | "def kitti_sequence(sequence: int):\n", 95 | " return OdometryPipeline(\n", 96 | " dataset=dataset_factory(\n", 97 | " dataloader=\"kitti\",\n", 98 | " data_dir=kitti_root,\n", 99 | " sequence=sequence,\n", 100 | " )\n", 101 | " )\n", 102 | "\n", 103 | "\n", 104 | "results = {}\n", 105 | "for sequence in range(0, 11):\n", 106 | " run_sequence(kitti_sequence, sequence=sequence, results=results)" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": null, 112 | "id": "d67cb048", 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "from kiss_icp_eval import print_metrics_table\n", 117 | "\n", 118 | "print_metrics_table(results)" 119 | ] 120 | }, 121 | { 122 | "cell_type": "markdown", 123 | "id": "bbc9fc28", 124 | "metadata": {}, 125 | "source": [ 126 | "## Trajectories Results" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": null, 132 | "id": "1d0d4a21", 133 | "metadata": {}, 134 | "outputs": [], 135 | "source": [ 136 | "from kiss_icp_eval import plot_trajectories\n", 137 | "\n", 138 | "plot_trajectories(results)" 139 | ] 140 | } 141 | ], 142 | "metadata": { 143 | "kernelspec": { 144 | "display_name": "Python 3 (ipykernel)", 145 | "language": "python", 146 | "name": "python3" 147 | }, 148 | "language_info": { 149 | "codemirror_mode": { 150 | "name": "ipython", 151 | "version": 3 152 | }, 153 | "file_extension": ".py", 154 | "mimetype": "text/x-python", 155 | "name": "python", 156 | "nbconvert_exporter": "python", 157 | "pygments_lexer": "ipython3", 158 | "version": "3.10.6" 159 | }, 160 | "vscode": { 161 | "interpreter": { 162 | "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6" 163 | } 164 | } 165 | }, 166 | "nbformat": 4, 167 | "nbformat_minor": 5 168 | } 169 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/ncd.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | import re 26 | from pathlib import Path 27 | 28 | import numpy as np 29 | from pyquaternion import Quaternion 30 | 31 | 32 | class NewerCollegeDataset: 33 | def __init__(self, data_dir: Path, *_, **__): 34 | try: 35 | self.PyntCloud = importlib.import_module("pyntcloud").PyntCloud 36 | except ModuleNotFoundError: 37 | print(f'Newer College requires pnytccloud: "pip install pyntcloud"') 38 | 39 | self.data_source = os.path.join(data_dir, "") 40 | self.scan_folder = os.path.join(self.data_source, "raw_format/ouster_scan") 41 | self.pose_file = os.path.join(self.data_source, "ground_truth/registered_poses.csv") 42 | self.sequence_id = os.path.basename(data_dir) 43 | 44 | # Load scan files and poses 45 | self.scan_files = self.get_pcd_filenames(self.scan_folder) 46 | self.gt_poses = self.load_gt_poses(self.pose_file) 47 | 48 | # Visualization Options 49 | self.use_global_visualizer = True 50 | 51 | def __len__(self): 52 | return len(self.scan_files) 53 | 54 | def __getitem__(self, idx): 55 | file_path = os.path.join(self.scan_folder, self.scan_files[idx]) 56 | return self.getitem(file_path) 57 | 58 | def getitem(self, scan_file: str): 59 | points = self.PyntCloud.from_file(scan_file).points[["x", "y", "z"]].to_numpy() 60 | timestamps = self.get_timestamps() 61 | if points.shape[0] != timestamps.shape[0]: 62 | # Newer College has some broken point clouds, just fallback to no timestamps 63 | return points.astype(np.float64), np.array([]) 64 | return points.astype(np.float64), timestamps 65 | 66 | @staticmethod 67 | def get_timestamps(): 68 | H = 64 69 | W = 1024 70 | return np.floor(np.arange(H * W) / H) / W 71 | 72 | @staticmethod 73 | def get_pcd_filenames(scans_folder): 74 | # cloud_1583836591_182590976.pcd 75 | regex = re.compile("^cloud_(\d*_\d*)") 76 | 77 | def get_cloud_timestamp(pcd_filename): 78 | m = regex.search(pcd_filename) 79 | secs, nsecs = m.groups()[0].split("_") 80 | return int(secs) * int(1e9) + int(nsecs) 81 | 82 | return sorted(os.listdir(scans_folder), key=get_cloud_timestamp) 83 | 84 | @staticmethod 85 | def load_gt_poses(file_path: str): 86 | """Taken from pyLiDAR-SLAM/blob/master/slam/dataset/nhcd_dataset.py""" 87 | ground_truth_df = np.genfromtxt(str(file_path), delimiter=",", dtype=np.float64) 88 | xyz = ground_truth_df[:, 2:5] 89 | rotations = np.array( 90 | [ 91 | Quaternion(x=x, y=y, z=z, w=w).rotation_matrix 92 | for x, y, z, w in ground_truth_df[:, 5:] 93 | ] 94 | ) 95 | 96 | num_poses = rotations.shape[0] 97 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(num_poses, axis=0) 98 | poses[:, :3, :3] = rotations 99 | poses[:, :3, 3] = xyz 100 | 101 | T_CL = np.eye(4, dtype=np.float32) 102 | T_CL[:3, :3] = Quaternion(x=0, y=0, z=0.924, w=0.383).rotation_matrix 103 | T_CL[:3, 3] = np.array([-0.084, -0.025, 0.050], dtype=np.float32) 104 | poses = np.einsum("nij,jk->nik", poses, T_CL) 105 | poses = np.einsum("ij,njk->nik", np.linalg.inv(poses[0]), poses) 106 | return poses 107 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/mulran.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | from pathlib import Path 26 | 27 | import numpy as np 28 | 29 | 30 | class MulranDataset: 31 | def __init__(self, data_dir: Path, *_, **__): 32 | self.sequence_id = os.path.basename(data_dir) 33 | self.sequence_dir = os.path.realpath(data_dir) 34 | self.velodyne_dir = os.path.join(self.sequence_dir, "Ouster/") 35 | 36 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) 37 | self.scan_timestamps = [int(os.path.basename(t).split(".")[0]) for t in self.scan_files] 38 | self.gt_poses = self.load_gt_poses(os.path.join(self.sequence_dir, "global_pose.csv")) 39 | 40 | def __len__(self): 41 | return len(self.scan_files) 42 | 43 | def __getitem__(self, idx): 44 | return self.read_point_cloud(self.scan_files[idx]) 45 | 46 | def read_point_cloud(self, file_path: str): 47 | points = np.fromfile(file_path, dtype=np.float32).reshape((-1, 4))[:, :3] 48 | timestamps = self.get_timestamps() 49 | if points.shape[0] != timestamps.shape[0]: 50 | # MuRan has some broken point clouds, just fallback to no timestamps 51 | return points.astype(np.float64), np.array([]) 52 | return points.astype(np.float64), timestamps 53 | 54 | @staticmethod 55 | def get_timestamps(): 56 | H = 64 57 | W = 1024 58 | return np.floor(np.arange(H * W) / H) / W 59 | 60 | def load_gt_poses(self, poses_file: str): 61 | """MuRan has more poses than scans, therefore we need to match 1-1 timestamp with pose""" 62 | 63 | def read_csv(poses_file: str): 64 | poses = np.loadtxt(poses_file, delimiter=",") 65 | timestamps = poses[:, 0] 66 | poses = poses[:, 1:] 67 | n = poses.shape[0] 68 | poses = np.concatenate( 69 | (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), 70 | axis=1, 71 | ) 72 | poses = poses.reshape((n, 4, 4)) # [N, 4, 4] 73 | return poses, timestamps 74 | 75 | # Read the csv file 76 | poses, timestamps = read_csv(poses_file) 77 | # Extract only the poses that has a matching Ouster scan 78 | poses = poses[[np.argmin(abs(timestamps - t)) for t in self.scan_timestamps]] 79 | 80 | # Convert from global coordinate poses to local poses 81 | first_pose = poses[0, :, :] 82 | poses = np.linalg.inv(first_pose) @ poses 83 | 84 | T_lidar_to_base, T_base_to_lidar = self._get_calibration() 85 | return T_lidar_to_base @ poses @ T_base_to_lidar 86 | 87 | def _get_calibration(self): 88 | # Apply calibration obtainbed from calib_base2ouster.txt 89 | # T_lidar_to_base[:3, 3] = np.array([1.7042, -0.021, 1.8047]) 90 | # T_lidar_to_base[:3, :3] = tu_vieja.from_euler( 91 | # "xyz", [0.0001, 0.0003, 179.6654], degrees=True 92 | # ) 93 | T_lidar_to_base = np.array( 94 | [ 95 | [-9.9998295e-01, -5.8398386e-03, -5.2257060e-06, 1.7042000e00], 96 | [5.8398386e-03, -9.9998295e-01, 1.7758769e-06, -2.1000000e-02], 97 | [-5.2359878e-06, 1.7453292e-06, 1.0000000e00, 1.8047000e00], 98 | [0.0000000e00, 0.0000000e00, 0.0000000e00, 1.0000000e00], 99 | ] 100 | ) 101 | T_base_to_lidar = np.linalg.inv(T_lidar_to_base) 102 | return T_lidar_to_base, T_base_to_lidar 103 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/kitti.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to mse, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE msE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | 26 | import numpy as np 27 | 28 | 29 | class KITTIOdometryDataset: 30 | def __init__(self, data_dir, sequence: str, *_, **__): 31 | self.sequence_id = str(sequence).zfill(2) 32 | self.kitti_sequence_dir = os.path.join(data_dir, "sequences", self.sequence_id) 33 | self.velodyne_dir = os.path.join(self.kitti_sequence_dir, "velodyne/") 34 | 35 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) 36 | self.calibration = self.read_calib_file(os.path.join(self.kitti_sequence_dir, "calib.txt")) 37 | 38 | # Load GT Poses (if available) 39 | if int(sequence) < 11: 40 | self.poses_fn = os.path.join(data_dir, f"poses/{self.sequence_id}.txt") 41 | self.gt_poses = self.load_poses(self.poses_fn) 42 | 43 | # Add correction for KITTI datasets, can be easilty removed if unwanted 44 | from kiss_icp.pybind import kiss_icp_pybind 45 | 46 | self.correct_kitti_scan = lambda frame: np.asarray( 47 | kiss_icp_pybind._correct_kitti_scan(kiss_icp_pybind._Vector3dVector(frame)) 48 | ) 49 | 50 | def __getitem__(self, idx): 51 | return self.scans(idx) 52 | 53 | def __len__(self): 54 | return len(self.scan_files) 55 | 56 | def scans(self, idx): 57 | return self.read_point_cloud(self.scan_files[idx]), np.array([]) 58 | 59 | def apply_calibration(self, poses: np.ndarray) -> np.ndarray: 60 | """Converts from Velodyne to Camera Frame""" 61 | Tr = np.eye(4, dtype=np.float64) 62 | Tr[:3, :4] = self.calibration["Tr"].reshape(3, 4) 63 | return Tr @ poses @ np.linalg.inv(Tr) 64 | 65 | def read_point_cloud(self, scan_file: str): 66 | points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 4))[:, :3].astype(np.float64) 67 | # points = points[points[:, 2] > -2.9] # Remove the annoying reflections 68 | points = self.correct_kitti_scan(points) 69 | return points 70 | 71 | def load_poses(self, poses_file): 72 | def _lidar_pose_gt(poses_gt): 73 | _tr = self.calibration["Tr"].reshape(3, 4) 74 | tr = np.eye(4, dtype=np.float64) 75 | tr[:3, :4] = _tr 76 | left = np.einsum("...ij,...jk->...ik", np.linalg.inv(tr), poses_gt) 77 | right = np.einsum("...ij,...jk->...ik", left, tr) 78 | return right 79 | 80 | poses = np.loadtxt(poses_file, delimiter=" ") 81 | n = poses.shape[0] 82 | poses = np.concatenate( 83 | (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), axis=1 84 | ) 85 | poses = poses.reshape((n, 4, 4)) # [N, 4, 4] 86 | return _lidar_pose_gt(poses) 87 | 88 | def get_frames_timestamps(self) -> np.ndarray: 89 | timestamps = np.loadtxt(os.path.join(self.kitti_sequence_dir, "times.txt")).reshape(-1, 1) 90 | return timestamps 91 | 92 | @staticmethod 93 | def read_calib_file(file_path: str) -> dict: 94 | calib_dict = {} 95 | with open(file_path, "r") as calib_file: 96 | for line in calib_file.readlines(): 97 | tokens = line.split(" ") 98 | if tokens[0] == "calib_time:": 99 | continue 100 | # Only read with float data 101 | if len(tokens) > 0: 102 | values = [float(token) for token in tokens[1:]] 103 | values = np.array(values, dtype=np.float32) 104 | 105 | # The format in KITTI's file is : ...\n -> Remove the ':' 106 | key = tokens[0][:-1] 107 | calib_dict[key] = values 108 | return calib_dict 109 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/helipr.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Benedikt Mersch, Saurabh Gupta, Ignacio Vizzo, 4 | # Tiziano Guadagnino, Cyrill Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | import struct 26 | import sys 27 | from pathlib import Path 28 | 29 | import numpy as np 30 | 31 | 32 | class HeLiPRDataset: 33 | def __init__(self, data_dir: Path, sequence: str, *_, **__): 34 | self.sequence_id = sequence 35 | self.sequence_dir = os.path.join(data_dir, "LiDAR", self.sequence_id) 36 | self.scan_files = sorted(glob.glob(self.sequence_dir + "/*.bin")) 37 | self.scan_timestamps = [int(Path(file).stem) for file in self.scan_files] 38 | 39 | self.gt_file = os.path.join(data_dir, "LiDAR_GT", f"global_{self.sequence_id}_gt.txt") 40 | self.gt_poses = self.load_poses(self.gt_file) 41 | 42 | if len(self.scan_files) == 0: 43 | raise ValueError(f"Tried to read point cloud files in {data_dir} but none found") 44 | 45 | # Obtain the pointcloud reader for the given data folder 46 | if self.sequence_id == "Avia": 47 | self.format_string = "fffBBBL" 48 | self.intensity_channel = None 49 | self.time_channel = 6 50 | elif self.sequence_id == "Aeva": 51 | self.format_string = "ffffflBf" 52 | self.format_string_no_intensity = "ffffflB" 53 | self.intensity_channel = 7 54 | self.time_channel = 5 55 | elif self.sequence_id == "Ouster": 56 | self.format_string = "ffffIHHH" 57 | self.intensity_channel = 3 58 | self.time_channel = 4 59 | elif self.sequence_id == "Velodyne": 60 | self.format_string = "ffffHf" 61 | self.intensity_channel = 3 62 | self.time_channel = 5 63 | else: 64 | print("[ERROR] Unsupported LiDAR Type") 65 | sys.exit() 66 | 67 | def __len__(self): 68 | return len(self.scan_files) 69 | 70 | def __getitem__(self, idx): 71 | data = self.get_data(idx) 72 | points = self.read_point_cloud(data) 73 | timestamps = self.read_timestamps(data) 74 | return points, timestamps 75 | 76 | def load_poses(self, poses_file): 77 | from pyquaternion import Quaternion 78 | 79 | poses = np.loadtxt(poses_file, delimiter=" ") 80 | 81 | xyz = poses[:, 1:4] 82 | rotations = np.array( 83 | [Quaternion(x=x, y=y, z=z, w=w).rotation_matrix for x, y, z, w in poses[:, 4:]] 84 | ) 85 | poses = np.eye(4, dtype=np.float64).reshape(1, 4, 4).repeat(self.__len__(), axis=0) 86 | poses[:, :3, :3] = rotations 87 | poses[:, :3, 3] = xyz 88 | 89 | return poses 90 | 91 | def get_data(self, idx: int): 92 | file_path = self.scan_files[idx] 93 | list_lines = [] 94 | 95 | # Special case, see https://github.com/minwoo0611/HeLiPR-File-Player/blob/e8d95e390454ece1415ae9deb51515f63730c10a/src/ROSThread.cpp#L632 96 | if self.sequence_id == "Aeva" and int(Path(file_path).stem) <= 1691936557946849179: 97 | self.intensity_channel = None 98 | format_string = self.format_string_no_intensity 99 | else: 100 | format_string = self.format_string 101 | 102 | chunk_size = struct.calcsize(f"={format_string}") 103 | with open(file_path, "rb") as f: 104 | binary = f.read() 105 | offset = 0 106 | while offset < len(binary) - chunk_size: 107 | list_lines.append(struct.unpack_from(f"={format_string}", binary, offset)) 108 | offset += chunk_size 109 | data = np.stack(list_lines) 110 | return data 111 | 112 | def read_timestamps(self, data: np.ndarray) -> np.ndarray: 113 | time = data[:, self.time_channel] 114 | return (time - time.min()) / (time.max() - time.min()) 115 | 116 | def read_point_cloud(self, data: np.ndarray) -> np.ndarray: 117 | return data[:, :3] 118 | -------------------------------------------------------------------------------- /python/README.md: -------------------------------------------------------------------------------- 1 |
2 |

KISS-ICP

3 | 4 | 5 | 6 | 7 | 8 |
9 |
10 | Demo 11 |   •   12 | Install 13 |   •   14 | ROS 2 15 |   •   16 | ROS Demo 18 |   •   19 | Paper 20 |   •   21 | Contact Us 22 |
23 |
24 | 25 | [KISS-ICP](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf) is a LiDAR Odometry pipeline that **just works** on most of the cases withouth tunning any parameter. 26 | 27 |

28 | KISS-ICP Demo 29 |

30 |
31 | 32 |
33 | 34 | ## Install 35 | 36 | ```sh 37 | pip install kiss-icp 38 | ``` 39 | 40 | If you also want to install all the *(optional)* dependencies, like Open3D for running the visualizer: 41 | 42 | ```sh 43 | pip install "kiss-icp[all]" 44 | ``` 45 | 46 | ## Running the system 47 | 48 | Next, follow the instructions on how to run the system by typing: 49 | 50 | ```sh 51 | kiss_icp_pipeline --help 52 | ``` 53 | 54 | This should print the following help message: 55 | ![out](https://user-images.githubusercontent.com/21349875/193282970-25a400aa-ebcd-487a-b839-faa04eeca5b9.png) 56 | 57 | ### Config 58 | 59 | You can generate a default `config.yaml` by typing 60 | 61 | ```sh 62 | kiss_icp_dump_config 63 | ``` 64 | 65 | Now, you can modify the parameters and pass the file to the `--config` option when running the `kiss_icp_pipeline`. 66 | 67 | ### Install Python API (developer mode) 68 | 69 | If you plan to modify the code then you need to setup the dev dependencies, luckily, the only real 70 | requirements are a modern C++ compiler and the `pip` package manager, nothing else!, in Ubuntu-based 71 | systems this can be done with: 72 | 73 | ```sh 74 | sudo apt install g++ python3-pip 75 | ``` 76 | 77 | After that you can clone the code and install the python api: 78 | 79 | ```sh 80 | git clone https://github.com/PRBonn/kiss-icp.git 81 | cd kiss-icp 82 | make editable 83 | ``` 84 | 85 | ### Install Python API (expert mode) 86 | 87 | If you want to have more controll over the build, you should then install `cmake`, ,`ninja`, `tbb`, 88 | `Eigen`, and `pybind11` as extra dependencies in your system, the ubuntu-way of doing this is: 89 | 90 | ```sh 91 | sudo apt install build-essential libeigen3-dev libtbb-dev pybind11-dev ninja-build 92 | ``` 93 | 94 | ## Citation 95 | 96 | If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf). 97 | 98 | ```bibtex 99 | @article{vizzo2023ral, 100 | author = {Vizzo, Ignacio and Guadagnino, Tiziano and Mersch, Benedikt and Wiesmann, Louis and Behley, Jens and Stachniss, Cyrill}, 101 | title = {{KISS-ICP: In Defense of Point-to-Point ICP -- Simple, Accurate, and Robust Registration If Done the Right Way}}, 102 | journal = {IEEE Robotics and Automation Letters (RA-L)}, 103 | pages = {1029--1036}, 104 | doi = {10.1109/LRA.2023.3236571}, 105 | volume = {8}, 106 | number = {2}, 107 | year = {2023}, 108 | codeurl = {https://github.com/PRBonn/kiss-icp}, 109 | } 110 | ``` 111 | 112 | ## Contributing 113 | 114 | We envision KISS-ICP as a comunity-driven project, we love to see how the project is growing thanks to the contributions from the comunity. We would love to see your face in the list below, just open a Pull Request! 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/ouster.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2023 Pavlo Bashmakov 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import os 26 | from typing import Optional 27 | 28 | import numpy as np 29 | 30 | 31 | class OusterDataloader: 32 | """Ouster pcap dataloader""" 33 | 34 | def __init__( 35 | self, 36 | data_dir: str, 37 | meta: Optional[str] = None, 38 | *_, 39 | **__, 40 | ): 41 | """Create Ouster pcap dataloader to read scans from a pcap file. 42 | 43 | Ouster pcap can be recorded with a `tcpdump` command or programmatically. 44 | Pcap file should contain raw lidar_packets and `meta` file (i.e. metadata.json) 45 | should be a corresponding sensor metadata stored at the time of pcap recording. 46 | 47 | 48 | NOTE: It's critical to have a metadata json stored in the same recording session 49 | as a pcap file, because pcap reader checks the `init_id` field in the UDP 50 | lidar_packets and expects it to match `initialization_id` 51 | in the metadata json, packets with different `init_id` just skipped. 52 | 53 | Metadata json can be obtainer with Ouster SDK: 54 | See examples here https://static.ouster.dev/sdk-docs/python/examples/basics-sensor.html#obtaining-sensor-metadata 55 | 56 | or with Sensor HTTP API endpoint GET /api/v1/sensor/metadata directly: 57 | See doc for details https://static.ouster.dev/sensor-docs/image_route1/image_route2/common_sections/API/http-api-v1.html#get-api-v1-sensor-metadata 58 | 59 | Args: 60 | data_dir: path to a pcap file (not a directory) 61 | meta: path to a metadata json file that should be recorded together with 62 | a pcap file. If `meta` is not provided attempts to find the best matching 63 | json file with the longest commong prefix of the pcap file (`data_dir`) in 64 | the same directory. 65 | """ 66 | 67 | try: 68 | from ouster.sdk import client, open_source 69 | except ImportError: 70 | print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"') 71 | exit(1) 72 | 73 | assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" 74 | 75 | # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity 76 | pcap_file = data_dir 77 | 78 | print("Indexing Ouster pcap to count the scans number ...") 79 | source = open_source(str(pcap_file), meta=[meta] if meta else [], index=True) 80 | 81 | # since we import ouster-sdk's client module locally, we keep reference 82 | # to it locally as well 83 | self._client = client 84 | 85 | self.data_dir = os.path.dirname(data_dir) 86 | 87 | # lookup table for 2D range image projection to a 3D point cloud 88 | self._xyz_lut = client.XYZLut(source.metadata) 89 | 90 | self._pcap_file = str(data_dir) 91 | 92 | self._scans_num = len(source) 93 | print(f"Ouster pcap total scans number: {self._scans_num}") 94 | 95 | # frame timestamps array 96 | self._timestamps = np.linspace(0, self._scans_num, self._scans_num, endpoint=False) 97 | 98 | self._source = source 99 | 100 | def __getitem__(self, idx): 101 | scan = self._source[idx] 102 | 103 | self._timestamps[idx] = 1e-9 * scan.timestamp[0] 104 | 105 | timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1)) 106 | 107 | # filtering our zero returns makes it substantially faster for kiss-icp 108 | sel_flag = scan.field(self._client.ChanField.RANGE) != 0 109 | xyz = self._xyz_lut(scan)[sel_flag] 110 | timestamps = timestamps[sel_flag] 111 | 112 | return xyz, timestamps 113 | 114 | def get_frames_timestamps(self): 115 | return self._timestamps 116 | 117 | def __len__(self): 118 | return self._scans_num 119 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/mcap.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | import sys 25 | 26 | import numpy as np 27 | 28 | 29 | class McapDataloader: 30 | def __init__(self, data_dir: str, topic: str, *_, **__): 31 | """Standalone .mcap dataloader withouth any ROS distribution.""" 32 | # Conditional imports to avoid injecting dependencies for non mcap users 33 | try: 34 | from mcap.reader import make_reader 35 | from mcap_ros2.reader import read_ros2_messages 36 | except ImportError as e: 37 | print("mcap plugins not installed: 'pip install mcap-ros2-support'") 38 | exit(1) 39 | 40 | from kiss_icp.tools.point_cloud2 import read_point_cloud 41 | 42 | # we expect `data_dir` param to be a path to the .mcap file, so rename for clarity 43 | assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file" 44 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 45 | self.mcap_file = str(data_dir) 46 | 47 | self.make_reader = make_reader 48 | self.read_ros2_messages = read_ros2_messages 49 | self.read_point_cloud = read_point_cloud 50 | 51 | self.bag = self.make_reader(open(self.mcap_file, "rb")) 52 | self.summary = self.bag.get_summary() 53 | self.topic = self.check_topic(topic) 54 | self.n_scans = self._get_n_scans() 55 | self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic]) 56 | self.timestamps = [] 57 | self.use_global_visualizer = True 58 | 59 | def __del__(self): 60 | if hasattr(self, "bag"): 61 | del self.bag 62 | 63 | def __getitem__(self, idx): 64 | msg = next(self.msgs).ros_msg 65 | self.timestamps.append(self.stamp_to_sec(msg.header.stamp)) 66 | return self.read_point_cloud(msg) 67 | 68 | def __len__(self): 69 | return self.n_scans 70 | 71 | def _get_n_scans(self) -> int: 72 | return sum( 73 | count 74 | for (id, count) in self.summary.statistics.channel_message_counts.items() 75 | if self.summary.channels[id].topic == self.topic 76 | ) 77 | 78 | def reset(self): 79 | self.timestamps = [] 80 | self.bag = self.make_reader(open(self.mcap_file, "rb")) 81 | self.msgs = self.read_ros2_messages(self.mcap_file, topics=[self.topic]) 82 | 83 | @staticmethod 84 | def stamp_to_sec(stamp): 85 | return stamp.sec + float(stamp.nanosec) / 1e9 86 | 87 | def get_frames_timestamps(self) -> list: 88 | return self.timestamps 89 | 90 | def check_topic(self, topic: str) -> str: 91 | # Extract schema id from the .mcap file that encodes the PointCloud2 msg 92 | schema_id = [ 93 | schema.id 94 | for schema in self.summary.schemas.values() 95 | if schema.name == "sensor_msgs/msg/PointCloud2" 96 | ][0] 97 | 98 | point_cloud_topics = [ 99 | channel.topic 100 | for channel in self.summary.channels.values() 101 | if channel.schema_id == schema_id 102 | ] 103 | 104 | def print_available_topics_and_exit(): 105 | print(50 * "-") 106 | for t in point_cloud_topics: 107 | print(f"--topic {t}") 108 | print(50 * "-") 109 | sys.exit(1) 110 | 111 | if topic and topic in point_cloud_topics: 112 | return topic 113 | # when user specified the topic check that exists 114 | if topic and topic not in point_cloud_topics: 115 | print( 116 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 117 | "Please select one of the following topics with the --topic flag" 118 | ) 119 | print_available_topics_and_exit() 120 | if len(point_cloud_topics) > 1: 121 | print( 122 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 123 | "Please select one of the following topics with the --topic flag" 124 | ) 125 | print_available_topics_and_exit() 126 | 127 | if len(point_cloud_topics) == 0: 128 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 129 | if len(point_cloud_topics) == 1: 130 | return point_cloud_topics[0] 131 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/rosbag.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import glob 24 | import os 25 | import sys 26 | from pathlib import Path 27 | from typing import Sequence 28 | 29 | import natsort 30 | 31 | 32 | class RosbagDataset: 33 | def __init__(self, data_dir: Path, topic: str, *_, **__): 34 | """ROS1 / ROS2 bagfile dataloader. 35 | 36 | It can take either one ROS2 bag file or one or more ROS1 bag files belonging to a split bag. 37 | The reader will replay ROS1 split bags in correct timestamp order. 38 | 39 | TODO: Merge mcap and rosbag dataloaders into 1 40 | """ 41 | try: 42 | from rosbags.highlevel import AnyReader 43 | except ModuleNotFoundError: 44 | print('rosbags library not installed, run "pip install -U rosbags"') 45 | sys.exit(1) 46 | 47 | from kiss_icp.tools.point_cloud2 import read_point_cloud 48 | 49 | self.read_point_cloud = read_point_cloud 50 | if data_dir.is_file(): 51 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 52 | self.bag = AnyReader([data_dir]) 53 | else: 54 | bagfiles = [Path(path) for path in glob.glob(os.path.join(data_dir, "*.bag"))] 55 | if len(bagfiles) > 0: 56 | self.sequence_id = os.path.basename(bagfiles[0]).split(".")[0] 57 | self.bag = AnyReader(bagfiles) 58 | else: 59 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 60 | self.bag = AnyReader([data_dir]) 61 | 62 | if len(self.bag.paths) > 1: 63 | print("Reading multiple .bag files in directory:") 64 | print("\n".join(natsort.natsorted([path.name for path in self.bag.paths]))) 65 | 66 | self.bag.open() 67 | self.topic = self.check_topic(topic) 68 | self.n_scans = self.bag.topics[self.topic].msgcount 69 | 70 | # limit connections to selected topic 71 | self.connections = [x for x in self.bag.connections if x.topic == self.topic] 72 | self.msgs = self.bag.messages(connections=self.connections) 73 | self.timestamps = [] 74 | 75 | # Visualization Options 76 | self.use_global_visualizer = True 77 | 78 | def __del__(self): 79 | if hasattr(self, "bag"): 80 | self.bag.close() 81 | 82 | def __len__(self): 83 | return self.n_scans 84 | 85 | def __getitem__(self, idx): 86 | connection, timestamp, rawdata = next(self.msgs) 87 | self.timestamps.append(self.to_sec(timestamp)) 88 | msg = self.bag.deserialize(rawdata, connection.msgtype) 89 | return self.read_point_cloud(msg) 90 | 91 | def reset(self): 92 | self.timestamps = [] 93 | self.bag.close() 94 | self.bag.open() 95 | self.msgs = self.bag.messages(connections=self.connections) 96 | 97 | @staticmethod 98 | def to_sec(nsec: int): 99 | return float(nsec) / 1e9 100 | 101 | def get_frames_timestamps(self) -> list: 102 | return self.timestamps 103 | 104 | def check_topic(self, topic: str) -> str: 105 | # Extract all PointCloud2 msg topics from the bagfile 106 | point_cloud_topics = [ 107 | topic[0] 108 | for topic in self.bag.topics.items() 109 | if topic[1].msgtype == "sensor_msgs/msg/PointCloud2" 110 | ] 111 | 112 | def print_available_topics_and_exit(): 113 | print(50 * "-") 114 | for t in point_cloud_topics: 115 | print(f"--topic {t}") 116 | print(50 * "-") 117 | sys.exit(1) 118 | 119 | if topic and topic in point_cloud_topics: 120 | return topic 121 | # when user specified the topic check that exists 122 | if topic and topic not in point_cloud_topics: 123 | print( 124 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 125 | "Please select one of the following topics with the --topic flag" 126 | ) 127 | print_available_topics_and_exit() 128 | if len(point_cloud_topics) > 1: 129 | print( 130 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 131 | "Please select one of the following topics with the --topic flag" 132 | ) 133 | print_available_topics_and_exit() 134 | 135 | if len(point_cloud_topics) == 0: 136 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 137 | if len(point_cloud_topics) == 1: 138 | return point_cloud_topics[0] 139 | -------------------------------------------------------------------------------- /python/kiss_icp/datasets/nclt.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | import sys 25 | from pathlib import Path 26 | 27 | import numpy as np 28 | 29 | 30 | class NCLTDataset: 31 | """Adapted from PyLidar-SLAM""" 32 | 33 | def __init__(self, data_dir: Path, *_, **__): 34 | self.sequence_id = os.path.basename(data_dir) 35 | self.sequence_dir = os.path.join(os.path.realpath(data_dir), "") 36 | self.scans_dir = os.path.join(self.sequence_dir, "velodyne_sync") 37 | scan_files = np.array(sorted(os.listdir(str(self.scans_dir))), dtype=str) 38 | poses_file = os.path.realpath( 39 | os.path.join( 40 | self.sequence_dir, 41 | "..", 42 | f"ground_truth/groundtruth_{self.sequence_id}.csv", 43 | ) 44 | ) 45 | gt_data = np.loadtxt(poses_file, delimiter=",") 46 | self.timestamps, timestamp_filter = self.load_valid_timestamps(gt_data, scan_files) 47 | self.scan_files = scan_files[timestamp_filter] 48 | self.gt_poses = self.load_gt_poses(gt_data) 49 | 50 | # Visualization Options 51 | self.use_global_visualizer = True 52 | 53 | def __len__(self): 54 | return len(self.scan_files) 55 | 56 | def __getitem__(self, idx): 57 | return self.read_point_cloud(os.path.join(self.scans_dir, self.scan_files[idx])), np.array( 58 | [] 59 | ) 60 | 61 | def read_point_cloud(self, file_path: str): 62 | def _convert(x_s, y_s, z_s): 63 | # Copied from http://robots.engin.umich.edu/nclt/python/read_vel_sync.py 64 | scaling = 0.005 65 | offset = -100.0 66 | 67 | x = x_s * scaling + offset 68 | y = y_s * scaling + offset 69 | z = z_s * scaling + offset 70 | return x, y, z 71 | 72 | binary = np.fromfile(file_path, dtype=np.int16) 73 | x = np.ascontiguousarray(binary[::4]) 74 | y = np.ascontiguousarray(binary[1::4]) 75 | z = np.ascontiguousarray(binary[2::4]) 76 | x = x.astype(np.float32).reshape(-1, 1) 77 | y = y.astype(np.float32).reshape(-1, 1) 78 | z = z.astype(np.float32).reshape(-1, 1) 79 | x, y, z = _convert(x, y, z) 80 | # Flip to have z pointing up 81 | points = np.concatenate([x, -y, -z], axis=1) 82 | return points.astype(np.float64) 83 | 84 | @staticmethod 85 | def load_valid_timestamps(gt_data: np.ndarray, scan_files: np.ndarray): 86 | # Ground truth timestamps and LiDARs don't match, interpolate 87 | gt_t = gt_data[:, 0] 88 | # Limit the sequence to timestamps for which a ground truth exists 89 | timestamps = np.array( 90 | [os.path.basename(file).split(".")[0] for file in scan_files], dtype=np.int64 91 | ) 92 | filter_ = (timestamps > np.min(gt_t)) * (timestamps < np.max(gt_t)) 93 | return timestamps[filter_], filter_ 94 | 95 | def load_gt_poses(self, gt_data: np.ndarray): 96 | try: 97 | from scipy import interpolate 98 | from scipy.spatial.transform import Rotation 99 | except ImportError: 100 | print('NCLT dataloader requires scipy: "pip install scipy"') 101 | sys.exit(1) 102 | 103 | inter = interpolate.interp1d(gt_data[:, 0], gt_data[:, 1:], kind="nearest", axis=0) 104 | 105 | # Limit the sequence to timestamps for which a ground truth exists 106 | gt = inter(self.timestamps) 107 | gt_tr = gt[:, :3] 108 | gt_euler = gt[:, 3:][:, [2, 1, 0]] 109 | gt_rot = Rotation.from_euler("ZYX", gt_euler).as_matrix() 110 | 111 | gt = np.eye(4, dtype=np.float32).reshape(1, 4, 4).repeat(gt.shape[0], axis=0) 112 | gt[:, :3, :3] = gt_rot 113 | gt[:, :3, 3] = gt_tr 114 | 115 | gt = np.einsum( 116 | "nij,jk->nik", 117 | gt, 118 | np.array( 119 | [ 120 | [1.0, 0.0, 0.0, 0.0], 121 | [0.0, -1.0, 0.0, 0.0], 122 | [0.0, 0.0, -1.0, 0.0], 123 | [0.0, 0.0, 0.0, 1.0], 124 | ], 125 | dtype=np.float32, 126 | ), 127 | ) 128 | gt = np.einsum( 129 | "ij,njk->nik", 130 | np.array( 131 | [ 132 | [1.0, 0.0, 0.0, 0.0], 133 | [0.0, -1.0, 0.0, 0.0], 134 | [0.0, 0.0, -1.0, 0.0], 135 | [0.0, 0.0, 0.0, 1.0], 136 | ], 137 | dtype=np.float32, 138 | ), 139 | gt, 140 | ) 141 | 142 | return gt 143 | -------------------------------------------------------------------------------- /python/kiss_icp/pybind/stl_vector_eigen.h: -------------------------------------------------------------------------------- 1 | // ---------------------------------------------------------------------------- 2 | // NOTE: This fily has been adapted from the Open3D project, but copyright 3 | // still belongs to Open3D. All rights reserved 4 | // ---------------------------------------------------------------------------- 5 | // - Open3D: www.open3d.org - 6 | // ---------------------------------------------------------------------------- 7 | // The MIT License (MIT) 8 | // 9 | // Copyright (c) 2018-2021 www.open3d.org 10 | // 11 | // Permission is hereby granted, free of charge, to any person obtaining a copy 12 | // of this software and associated documentation files (the "Software"), to deal 13 | // in the Software without restriction, including without limitation the rights 14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | // copies of the Software, and to permit persons to whom the Software is 16 | // furnished to do so, subject to the following conditions: 17 | // 18 | // The above copyright notice and this permission notice shall be included in 19 | // all copies or substantial portions of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 26 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 27 | // IN THE SOFTWARE. 28 | // ---------------------------------------------------------------------------- 29 | #pragma once 30 | #include 31 | #include 32 | 33 | // pollute namespace with py 34 | #include 35 | namespace py = pybind11; 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace pybind11 { 43 | 44 | template , typename... Args> 45 | py::class_ bind_vector_without_repr(py::module &m, 46 | std::string const &name, 47 | Args &&...args) { 48 | // hack function to disable __repr__ for the convenient function 49 | // bind_vector() 50 | using Class_ = py::class_; 51 | Class_ cl(m, name.c_str(), std::forward(args)...); 52 | cl.def(py::init<>()); 53 | cl.def( 54 | "__bool__", [](const Vector &v) -> bool { return !v.empty(); }, 55 | "Check whether the list is nonempty"); 56 | cl.def("__len__", &Vector::size); 57 | return cl; 58 | } 59 | 60 | // - This function is used by Pybind for std::vector constructor. 61 | // This optional constructor is added to avoid too many Python <-> C++ API 62 | // calls when the vector size is large using the default biding method. 63 | // Pybind matches np.float64 array to py::array_t buffer. 64 | // - Directly using templates for the py::array_t and py::array_t 65 | // and etc. doesn't work. The current solution is to explicitly implement 66 | // bindings for each py array types. 67 | template 68 | std::vector py_array_to_vectors_double( 69 | py::array_t array) { 70 | int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; 71 | if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { 72 | throw py::cast_error(); 73 | } 74 | std::vector eigen_vectors(array.shape(0)); 75 | auto array_unchecked = array.mutable_unchecked<2>(); 76 | for (auto i = 0; i < array_unchecked.shape(0); ++i) { 77 | eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); 78 | } 79 | return eigen_vectors; 80 | } 81 | 82 | } // namespace pybind11 83 | 84 | template , 86 | typename holder_type = std::unique_ptr, 87 | typename InitFunc> 88 | py::class_ pybind_eigen_vector_of_vector(py::module &m, 89 | const std::string &bind_name, 90 | const std::string &repr_name, 91 | InitFunc init_func) { 92 | using Scalar = typename EigenVector::Scalar; 93 | auto vec = py::bind_vector_without_repr>( 94 | m, bind_name, py::buffer_protocol(), py::module_local()); 95 | vec.def(py::init(init_func)); 96 | vec.def_buffer([](std::vector &v) -> py::buffer_info { 97 | size_t rows = EigenVector::RowsAtCompileTime; 98 | return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, 99 | {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); 100 | }); 101 | vec.def("__repr__", [repr_name](const std::vector &v) { 102 | return repr_name + std::string(" with ") + std::to_string(v.size()) + 103 | std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); 104 | }); 105 | vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); 106 | vec.def("__deepcopy__", 107 | [](std::vector &v) { return std::vector(v); }); 108 | 109 | // py::detail must be after custom constructor 110 | using Class_ = py::class_>; 111 | py::detail::vector_if_copy_constructible(vec); 112 | py::detail::vector_if_equal_operator(vec); 113 | py::detail::vector_modifiers(vec); 114 | py::detail::vector_accessor(vec); 115 | 116 | return vec; 117 | } 118 | --------------------------------------------------------------------------------