├── .clang-format ├── .cmake-format.yaml ├── .gitattributes ├── .github ├── CODEOWNERS └── workflows │ ├── cpp.yml │ ├── pre-commit.yml │ ├── pypi.yml │ └── python.yml ├── .gitignore ├── .pre-commit-config.yaml ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── Makefile ├── README.md ├── kiss_slam ├── __init__.py ├── config │ ├── __init__.py │ └── config.py ├── kiss_slam_pybind │ ├── 3rdparty │ │ ├── bonxai │ │ │ ├── LICENSE │ │ │ ├── bonxai.cmake │ │ │ └── bonxai.patch │ │ ├── eigen │ │ │ ├── LICENSE │ │ │ ├── eigen.cmake │ │ │ └── eigen.patch │ │ ├── find_dependencies.cmake │ │ ├── g2o │ │ │ ├── LICENSE │ │ │ └── g2o.cmake │ │ ├── suitesparse │ │ │ ├── LICENSE │ │ │ ├── suitesparse.cmake │ │ │ └── suitesparse.patch │ │ └── tsl_robin │ │ │ ├── LICENSE │ │ │ └── tsl_robin.cmake │ ├── CMakeLists.txt │ ├── LICENSE │ ├── cmake │ │ └── CompilerOptions.cmake │ ├── kiss_slam_pybind.cpp │ ├── occupancy_mapper │ │ ├── CMakeLists.txt │ │ ├── occupancy_mapper.cpp │ │ └── occupancy_mapper.hpp │ ├── pgo │ │ ├── CMakeLists.txt │ │ ├── pose_graph_optimizer.cpp │ │ └── pose_graph_optimizer.hpp │ ├── stl_vector_eigen.h │ └── voxel_map │ │ ├── CMakeLists.txt │ │ ├── voxel_map.cpp │ │ └── voxel_map.hpp ├── local_map_graph.py ├── loop_closer.py ├── occupancy_mapper.py ├── pipeline.py ├── pose_graph_optimizer.py ├── slam.py ├── tools │ ├── __init__.py │ ├── cli.py │ └── visualizer.py └── voxel_map.py └── pyproject.toml /.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 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | enable_markup: false 2 | line_width: 120 3 | format: 4 | max_subgroups_hwrap: 5 5 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.md linguist-documentation 2 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @benemer @tizianoGuadagnino @saurabh1002 2 | -------------------------------------------------------------------------------- /.github/workflows/cpp.yml: -------------------------------------------------------------------------------- 1 | name: C++ Pybind 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_dev: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-24.04, ubuntu-22.04] 18 | 19 | steps: 20 | - uses: actions/checkout@v3 21 | - name: Cache dependencies 22 | uses: actions/cache@v4 23 | with: 24 | path: ~/.apt/cache 25 | key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} 26 | restore-keys: | 27 | ${{ runner.os }}-apt- 28 | - name: Install dependencies 29 | run: | 30 | sudo apt-get update 31 | sudo apt-get install -y build-essential cmake git pybind11-dev libeigen3-dev libsuitesparse-dev 32 | - name: Configure CMake 33 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}} 34 | - name: Build 35 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 36 | -------------------------------------------------------------------------------- /.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/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}}/ 19 | - uses: actions/upload-artifact@v4 20 | with: 21 | path: dist/*.tar.gz 22 | 23 | cibuildwheel: 24 | name: Build wheels on ${{ matrix.os }} 25 | runs-on: ${{ matrix.os }} 26 | strategy: 27 | matrix: 28 | os: [ubuntu-24.04, macos-15] 29 | 30 | steps: 31 | - uses: actions/checkout@v3 32 | 33 | - name: Build test wheels (only PRs) 34 | if: github.event_name != 'release' 35 | uses: pypa/cibuildwheel@v2.22.0 36 | env: # build 1 build per platform just to make sure we can do it later when releasing 37 | CIBW_BUILD: "cp310-*" 38 | with: 39 | package-dir: ${{github.workspace}}/ 40 | 41 | - name: Build all wheels 42 | if: github.event_name == 'release' 43 | uses: pypa/cibuildwheel@v2.22.0 44 | with: 45 | package-dir: ${{github.workspace}}/ 46 | 47 | - uses: actions/upload-artifact@v4 48 | with: 49 | name: artifact-${{ matrix.os }} 50 | path: ./wheelhouse/*.whl 51 | 52 | pypi: 53 | if: github.event_name == 'release' 54 | needs: [cibuildwheel, build_sdist] 55 | runs-on: ubuntu-latest 56 | steps: 57 | - uses: actions/download-artifact@v4 58 | with: 59 | pattern: artifact* 60 | path: dist 61 | merge-multiple: true 62 | 63 | - uses: pypa/gh-action-pypi-publish@release/v1 64 | with: 65 | password: ${{ secrets.PYPI_API_TOKEN }} 66 | -------------------------------------------------------------------------------- /.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, macos-14, macos-15] 14 | 15 | steps: 16 | - uses: actions/checkout@v4 17 | - name: Set up Python3 18 | uses: actions/setup-python@v5 19 | with: 20 | python-version: '3.12' 21 | - name: Install python dependencies 22 | run: | 23 | python -m pip install --upgrade pip 24 | - name: Build pip package 25 | run: | 26 | python -m pip install --verbose . 27 | - name: Test installation 28 | run: | 29 | kiss_slam_pipeline --help 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | **/*.zip 2 | **/slam*/** 3 | trajectories/** 4 | **/*.txt 5 | **/*.ply 6 | # Created by https://www.toptal.com/developers/gitignore/api/c++,python,cmake 7 | # Edit at https://www.toptal.com/developers/gitignore?templates=c++,python,cmake 8 | *.g2o 9 | **/*.g2o 10 | **/*.png 11 | 12 | ### C++ ### 13 | # Prerequisites 14 | *.d 15 | 16 | # Compiled Object files 17 | *.slo 18 | *.lo 19 | *.o 20 | *.obj 21 | 22 | # Precompiled Headers 23 | *.gch 24 | *.pch 25 | 26 | # Compiled Dynamic libraries 27 | *.so 28 | *.dylib 29 | *.dll 30 | 31 | # Fortran module files 32 | *.mod 33 | *.smod 34 | 35 | # Compiled Static libraries 36 | *.lai 37 | *.la 38 | *.a 39 | *.lib 40 | 41 | # Executables 42 | *.exe 43 | *.out 44 | *.app 45 | 46 | ### CMake ### 47 | !**/CMakeLists.txt 48 | CMakeLists.txt.user 49 | CMakeCache.txt 50 | CMakeFiles 51 | CMakeScripts 52 | Testing 53 | Makefile 54 | cmake_install.cmake 55 | install_manifest.txt 56 | compile_commands.json 57 | CTestTestfile.cmake 58 | _deps 59 | 60 | ### CMake Patch ### 61 | # External projects 62 | *-prefix/ 63 | 64 | ### Python ### 65 | # Byte-compiled / optimized / DLL files 66 | __pycache__/ 67 | *.py[cod] 68 | *$py.class 69 | 70 | # C extensions 71 | 72 | # Distribution / packaging 73 | .Python 74 | build/ 75 | develop-eggs/ 76 | dist/ 77 | downloads/ 78 | eggs/ 79 | .eggs/ 80 | lib/ 81 | lib64/ 82 | parts/ 83 | sdist/ 84 | var/ 85 | wheels/ 86 | share/python-wheels/ 87 | *.egg-info/ 88 | .installed.cfg 89 | *.egg 90 | MANIFEST 91 | 92 | # PyInstaller 93 | # Usually these files are written by a python script from a template 94 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 95 | *.manifest 96 | *.spec 97 | 98 | # Installer logs 99 | pip-log.txt 100 | pip-delete-this-directory.txt 101 | 102 | # Unit test / coverage reports 103 | htmlcov/ 104 | .tox/ 105 | .nox/ 106 | .coverage 107 | .coverage.* 108 | .cache 109 | nosetests.xml 110 | coverage.xml 111 | *.cover 112 | *.py,cover 113 | .hypothesis/ 114 | .pytest_cache/ 115 | cover/ 116 | 117 | # Translations 118 | *.mo 119 | *.pot 120 | 121 | # Django stuff: 122 | *.log 123 | local_settings.py 124 | db.sqlite3 125 | db.sqlite3-journal 126 | 127 | # Flask stuff: 128 | instance/ 129 | .webassets-cache 130 | 131 | # Scrapy stuff: 132 | .scrapy 133 | 134 | # Sphinx documentation 135 | docs/_build/ 136 | 137 | # PyBuilder 138 | .pybuilder/ 139 | target/ 140 | 141 | # Jupyter Notebook 142 | .ipynb_checkpoints 143 | 144 | # IPython 145 | profile_default/ 146 | ipython_config.py 147 | 148 | # pyenv 149 | # For a library or package, you might want to ignore these files since the code is 150 | # intended to run in multiple environments; otherwise, check them in: 151 | # .python-version 152 | 153 | # pipenv 154 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 155 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 156 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 157 | # install all needed dependencies. 158 | #Pipfile.lock 159 | 160 | # poetry 161 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 162 | # This is especially recommended for binary packages to ensure reproducibility, and is more 163 | # commonly ignored for libraries. 164 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 165 | #poetry.lock 166 | 167 | # pdm 168 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 169 | #pdm.lock 170 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 171 | # in version control. 172 | # https://pdm.fming.dev/#use-with-ide 173 | .pdm.toml 174 | 175 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 176 | __pypackages__/ 177 | 178 | # Celery stuff 179 | celerybeat-schedule 180 | celerybeat.pid 181 | 182 | # SageMath parsed files 183 | *.sage.py 184 | 185 | # Environments 186 | .env 187 | .venv 188 | env/ 189 | venv/ 190 | ENV/ 191 | env.bak/ 192 | venv.bak/ 193 | 194 | # Spyder project settings 195 | .spyderproject 196 | .spyproject 197 | 198 | # Rope project settings 199 | .ropeproject 200 | 201 | # mkdocs documentation 202 | /site 203 | 204 | # mypy 205 | .mypy_cache/ 206 | .dmypy.json 207 | dmypy.json 208 | 209 | # Pyre type checker 210 | .pyre/ 211 | 212 | # pytype static type analyzer 213 | .pytype/ 214 | 215 | # Cython debug symbols 216 | cython_debug/ 217 | 218 | # PyCharm 219 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 220 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 221 | # and can be added to the global gitignore or merged into this file. For a more nuclear 222 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 223 | #.idea/ 224 | 225 | ### Python Patch ### 226 | # Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration 227 | poetry.toml 228 | 229 | # ruff 230 | .ruff_cache/ 231 | 232 | # LSP config files 233 | pyrightconfig.json 234 | 235 | # End of https://www.toptal.com/developers/gitignore/api/c++,python,cmake 236 | 237 | # Ignore Visual Studio Code files 238 | .vscode 239 | -------------------------------------------------------------------------------- /.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=./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=./pyproject.toml"] 29 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/kiss-slam/2e1d58aea79fbcfeccaebe181b45083816173504/CITATION.cff -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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_slam_pybind VERSION 0.0.2 LANGUAGES CXX) 25 | 26 | set(CMAKE_BUILD_TYPE Release) 27 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 28 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 29 | 30 | set(PYBIND11_NEWPYTHON ON) 31 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 32 | find_package(pybind11 CONFIG REQUIRED) 33 | 34 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/kiss_slam/kiss_slam_pybind) 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | install: 2 | @pip install --verbose . 3 | 4 | uninstall: 5 | @pip -v uninstall kiss_slam 6 | 7 | editable: 8 | @pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake 9 | @pip install --no-build-isolation -ve . 10 | 11 | cpp: 12 | @cmake -Bbuild . 13 | @cmake --build build -j$(nproc --all) 14 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

KISS-SLAM

3 | 4 | 5 | 6 | 7 |
8 |
9 | Install 10 |   •   11 | Paper 12 |   •   13 | Contact Us 14 |
15 |
16 | 17 | [KISS-SLAM](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kiss2025iros.pdf) is a simple, robust, and accurate 3D LiDAR SLAM system that **just works**. 18 | 19 | 20 | ![motivation](https://github.com/user-attachments/assets/66c3e50f-009a-4a36-9856-283a895c300f) 21 | 22 | 23 |
24 | 25 |
26 | 27 | ## Install 28 | 29 | ``` 30 | pip install kiss-slam 31 | ``` 32 | 33 | ## Running the system 34 | Next, follow the instructions on how to run the system by typing: 35 | ``` 36 | kiss_slam_pipeline --help 37 | ``` 38 | 39 | This should print the following help message: 40 | 41 | ![help](https://github.com/user-attachments/assets/5a6fe624-2aaf-466f-8a18-51039b794000) 42 | 43 | ### Config 44 | You can generate a default `config.yaml` by typing: 45 | 46 | ``` 47 | kiss_slam_dump_config 48 | ``` 49 | 50 | which will generate a `kiss_slam.yaml` file. Now, you can modify the parameters and pass the file to the `--config` option when running the `kiss_slam_pipeline`. 51 | 52 | ### Install Python API (developer mode) 53 | For development purposes: 54 | 55 | ``` 56 | sudo apt install git python3-pip libeigen3-dev libsuitesparse-dev 57 | python3 -m pip install --upgrade pip 58 | git clone https://github.com/PRBonn/kiss-slam.git 59 | cd kiss-slam 60 | make editable 61 | ``` 62 | 63 | ## Citation 64 | If you use this library for any academic work, please cite our original paper: 65 | ```bib 66 | @article{kiss2025arxiv, 67 | author = {T. Guadagnino and B. Mersch and S. Gupta and I. Vizzo and G. Grisetti and C. Stachniss}, 68 | title = {{KISS-SLAM: A Simple, Robust, and Accurate 3D LiDAR SLAM System With Enhanced Generalization Capabilities}}, 69 | journal = {arXiv preprint}, 70 | year = 2025, 71 | volume = {arXiv:2503.12660}, 72 | url = {https://arxiv.org/pdf/2503.12660}, 73 | } 74 | ``` 75 | 76 | ## Acknowledgements 77 | This project builds on top of [KISS-ICP](https://github.com/PRBonn/kiss-icp), [MapClosures](https://github.com/PRBonn/MapClosures), and [g2o](https://github.com/RainerKuemmerle/g2o). 78 | 79 | ## Contributing 80 | 81 | We envision KISS-SLAM 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; open a Pull Request! 82 | 83 | 84 | 85 | 86 | 87 | ## Contact Us 88 | For questions or feedback: 89 | - GitHub Issues: https://github.com/PRBonn/kiss-slam/issues 90 | -------------------------------------------------------------------------------- /kiss_slam/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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__ = "0.0.2" 24 | -------------------------------------------------------------------------------- /kiss_slam/config/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 .config import ( 24 | AdaptiveThresholdConfig, 25 | KissOdometryConfig, 26 | KissSLAMConfig, 27 | LocalMapperConfig, 28 | LoopCloserConfig, 29 | MapClosuresConfig, 30 | OccupancyMapperConfig, 31 | PoseGraphOptimizerConfig, 32 | load_config, 33 | ) 34 | -------------------------------------------------------------------------------- /kiss_slam/config/config.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 __future__ import annotations 24 | 25 | from pathlib import Path 26 | from typing import Any, Dict, Optional 27 | 28 | import yaml 29 | from kiss_icp.config.config import ( 30 | AdaptiveThresholdConfig, 31 | DataConfig, 32 | MappingConfig, 33 | RegistrationConfig, 34 | ) 35 | from kiss_icp.config.parser import KISSConfig 36 | from map_closures.config.config import MapClosuresConfig 37 | from pydantic import BaseModel 38 | from pydantic_settings import BaseSettings, SettingsConfigDict 39 | 40 | 41 | class KissOdometryConfig(BaseModel): 42 | preprocessing: DataConfig = DataConfig() 43 | registration: RegistrationConfig = RegistrationConfig() 44 | mapping: MappingConfig = MappingConfig() 45 | adaptive_threshold: AdaptiveThresholdConfig = AdaptiveThresholdConfig() 46 | 47 | 48 | class LoopCloserConfig(BaseModel): 49 | detector: MapClosuresConfig = MapClosuresConfig() 50 | overlap_threshold: float = 0.4 51 | 52 | 53 | class LocalMapperConfig(BaseModel): 54 | voxel_size: float = 0.5 55 | splitting_distance: float = 100.0 56 | 57 | 58 | class OccupancyMapperConfig(BaseModel): 59 | free_threshold: float = 0.2 60 | occupied_threshold: float = 0.65 61 | resolution: float = 0.5 62 | max_range: Optional[float] = None 63 | z_min: float = 0.1 64 | z_max: float = 0.5 65 | 66 | 67 | class PoseGraphOptimizerConfig(BaseModel): 68 | max_iterations: int = 10 69 | 70 | 71 | class KissSLAMConfig(BaseSettings): 72 | model_config = SettingsConfigDict(env_prefix="kiss_slam_") 73 | out_dir: str = "slam_output" 74 | odometry: KissOdometryConfig = KissOdometryConfig() 75 | local_mapper: LocalMapperConfig = LocalMapperConfig() 76 | occupancy_mapper: OccupancyMapperConfig = OccupancyMapperConfig() 77 | loop_closer: LoopCloserConfig = LoopCloserConfig() 78 | pose_graph_optimizer: PoseGraphOptimizerConfig = PoseGraphOptimizerConfig() 79 | 80 | def kiss_icp_config(self) -> KISSConfig: 81 | return KISSConfig( 82 | out_dir=self.out_dir, 83 | data=self.odometry.preprocessing, 84 | registration=self.odometry.registration, 85 | mapping=self.odometry.mapping, 86 | adaptive_threshold=self.odometry.adaptive_threshold, 87 | ) 88 | 89 | 90 | class KissDumper(yaml.Dumper): 91 | # HACK: insert blank lines between top-level objects 92 | # inspired by https://stackoverflow.com/a/44284819/3786245 93 | def write_line_break(self, data=None): 94 | super().write_line_break(data) 95 | 96 | if len(self.indents) == 1: 97 | super().write_line_break() 98 | 99 | 100 | def _yaml_source(config_file: Optional[Path]) -> Dict[str, Any]: 101 | data = None 102 | if config_file is not None: 103 | with open(config_file) as cfg_file: 104 | data = yaml.safe_load(cfg_file) 105 | return data or {} 106 | 107 | 108 | def load_config(config_file: Optional[Path]) -> KissSLAMConfig: 109 | """Load configuration from an Optional yaml file. Additionally, deskew and max_range can be 110 | also specified from the CLI interface""" 111 | 112 | config = KissSLAMConfig(**_yaml_source(config_file)) 113 | 114 | # Use specified voxel size or compute one using the max range 115 | if config.odometry.mapping.voxel_size is None: 116 | config.odometry.mapping.voxel_size = float(config.odometry.preprocessing.max_range / 100.0) 117 | 118 | if config.occupancy_mapper.max_range is None: 119 | config.occupancy_mapper.max_range = config.odometry.preprocessing.max_range 120 | 121 | return config 122 | 123 | 124 | def write_config(config: KissSLAMConfig = KissSLAMConfig(), filename: str = "kiss_slam.yaml"): 125 | with open(filename, "w") as outfile: 126 | yaml.dump( 127 | config.model_dump(), 128 | outfile, 129 | Dumper=KissDumper, 130 | default_flow_style=False, 131 | sort_keys=False, 132 | indent=4, 133 | ) 134 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/bonxai/LICENSE: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/bonxai/bonxai.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, 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 | include(FetchContent) 29 | FetchContent_Declare( 30 | bonxai URL https://github.com/facontidavide/Bonxai/archive/refs/tags/v0.6.0.tar.gz SOURCE_SUBDIR bonxai_core 31 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/bonxai.patch UPDATE_DISCONNECTED 1) 32 | FetchContent_MakeAvailable(bonxai) 33 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/bonxai/bonxai.patch: -------------------------------------------------------------------------------- 1 | diff --git a/bonxai_core/include/bonxai/bonxai.hpp b/bonxai_core/include/bonxai/bonxai.hpp 2 | index 7d360cf..9f488c1 100644 3 | --- a/bonxai_core/include/bonxai/bonxai.hpp 4 | +++ b/bonxai_core/include/bonxai/bonxai.hpp 5 | @@ -49,7 +49,7 @@ class Grid { 6 | 7 | public: 8 | Grid(size_t log2dim) 9 | - : dim_(1 << log2dim), 10 | + : dim_(static_cast(1 << log2dim)), 11 | mask_(log2dim) { 12 | if constexpr (!std::is_same_v) { 13 | size_ = dim_ * dim_ * dim_; 14 | @@ -58,7 +58,7 @@ class Grid { 15 | } 16 | 17 | Grid(size_t log2dim, DataT* preAllocatedMemory) 18 | - : dim_(1 << log2dim), 19 | + : dim_(static_cast(1 << log2dim)), 20 | data_(preAllocatedMemory), 21 | mask_(log2dim), 22 | external_memory_(true) {} 23 | @@ -367,7 +367,7 @@ inline size_t Grid::memUsage() const { 24 | template 25 | inline void VoxelGrid::releaseUnusedMemory() { 26 | std::vector keys_to_delete; 27 | - for (const auto& [key, inner_grid] : root_map) { 28 | + for (auto& [key, inner_grid] : root_map) { 29 | for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) { 30 | const int32_t inner_index = *inner_it; 31 | auto& leaf_grid = inner_grid.cell(inner_index); 32 | @@ -404,9 +404,9 @@ inline VoxelGrid::VoxelGrid(double voxel_size, uint8_t inner_bits, uint8_ 33 | template 34 | inline CoordT VoxelGrid::posToCoord(double x, double y, double z) const { 35 | return { 36 | - static_cast(std::nearbyint(x * inv_resolution)), 37 | - static_cast(std::nearbyint(y * inv_resolution)), 38 | - static_cast(std::nearbyint(z * inv_resolution))}; 39 | + static_cast(std::floor(x * inv_resolution)), 40 | + static_cast(std::floor(y * inv_resolution)), 41 | + static_cast(std::floor(z * inv_resolution))}; 42 | } 43 | 44 | template 45 | diff --git a/bonxai_core/include/bonxai/grid_allocator.hpp b/bonxai_core/include/bonxai/grid_allocator.hpp 46 | index 34a3231..fc40d61 100644 47 | --- a/bonxai_core/include/bonxai/grid_allocator.hpp 48 | +++ b/bonxai_core/include/bonxai/grid_allocator.hpp 49 | @@ -10,6 +10,7 @@ 50 | #pragma once 51 | 52 | #include 53 | +#include 54 | #include 55 | #include 56 | #include 57 | @@ -87,7 +88,7 @@ class GridBlockAllocator { 58 | template 59 | inline GridBlockAllocator::GridBlockAllocator(size_t log2dim) 60 | : log2dim_(log2dim), 61 | - block_bytes_(std::pow((1 << log2dim), 3) * sizeof(DataT)), 62 | + block_bytes_(static_cast(std::pow((1 << log2dim), 3) * sizeof(DataT))), 63 | mutex_(new std::mutex) {} 64 | 65 | template 66 | @@ -111,7 +112,7 @@ GridBlockAllocator::allocateBlock() { 67 | auto mask_index = chunk->mask.findFirstOn(); 68 | if (mask_index < chunk->mask.size()) { 69 | // found in this chunk 70 | - uint32_t data_index = block_bytes_ * mask_index; 71 | + size_t data_index = block_bytes_ * mask_index; 72 | DataT* ptr = reinterpret_cast(&chunk->data[data_index]); 73 | chunk->mask.setOff(mask_index); 74 | size_++; 75 | diff --git a/bonxai_core/include/bonxai/grid_coord.hpp b/bonxai_core/include/bonxai/grid_coord.hpp 76 | index da8fb7d..1f9727b 100644 77 | --- a/bonxai_core/include/bonxai/grid_coord.hpp 78 | +++ b/bonxai_core/include/bonxai/grid_coord.hpp 79 | @@ -79,9 +79,9 @@ struct CoordT { 80 | 81 | [[nodiscard]] inline CoordT PosToCoord(const Point3D& point, double inv_resolution) { 82 | return { 83 | - static_cast(std::nearbyint(point.x * inv_resolution)), 84 | - static_cast(std::nearbyint(point.y * inv_resolution)), 85 | - static_cast(std::nearbyint(point.z * inv_resolution))}; 86 | + static_cast(std::floor(point.x * inv_resolution)), 87 | + static_cast(std::floor(point.y * inv_resolution)), 88 | + static_cast(std::floor(point.z * inv_resolution))}; 89 | } 90 | 91 | [[nodiscard]] inline Point3D CoordToPos(const CoordT& coord, double resolution) { 92 | @@ -212,7 +212,7 @@ struct hash { 93 | std::size_t operator()(const Bonxai::CoordT& p) const { 94 | // same as OpenVDB 95 | return ((1 << 20) - 1) & (static_cast(p.x) * 73856093 ^ // 96 | - static_cast(p.y) * 19349663 ^ // 97 | + static_cast(p.y) * 19349669 ^ // 98 | static_cast(p.z) * 83492791); 99 | } 100 | }; 101 | diff --git a/bonxai_core/include/bonxai/serialization.hpp b/bonxai_core/include/bonxai/serialization.hpp 102 | index b519073..2caaf2f 100644 103 | --- a/bonxai_core/include/bonxai/serialization.hpp 104 | +++ b/bonxai_core/include/bonxai/serialization.hpp 105 | @@ -128,17 +128,17 @@ inline HeaderInfo GetHeaderInfo(std::string header) { 106 | if (header.rfind(expected_prefix, 0) != 0) { 107 | throw std::runtime_error("Header wasn't recognized"); 108 | } 109 | - int p1 = header.find(",", 18) + 1; 110 | + long unsigned int p1 = header.find(",", 18) + 1; 111 | auto part_type = header.substr(18, p1 - 18 - 1); 112 | 113 | - int p2 = header.find(",", p1 + 1) + 1; 114 | + long unsigned int p2 = header.find(",", p1 + 1) + 1; 115 | auto part_ibits = header.substr(p1, p2 - p1 - 1); 116 | 117 | - int p3 = header.find(">", p2) + 1; 118 | + long unsigned int p3 = header.find(">", p2) + 1; 119 | auto part_lbits = header.substr(p2, p3 - p2 - 1); 120 | 121 | - int p4 = header.find("(", p3) + 1; 122 | - int p5 = header.find(")", p4); 123 | + long unsigned int p4 = header.find("(", p3) + 1; 124 | + long unsigned int p5 = header.find(")", p4); 125 | auto part_res = header.substr(p4, p5 - p4); 126 | 127 | HeaderInfo info; 128 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/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 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # Copyright (c) 2024 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 | include(FetchContent) 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 | FetchContent_Declare(eigen URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz 31 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/eigen.patch UPDATE_DISCONNECTED 1) 32 | FetchContent_GetProperties(eigen) 33 | 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 | 47 | # Make sure g2o can find Eigen, see https://github.com/RainerKuemmerle/g2o/issues/596#issuecomment-1229435903 48 | set(EIGEN3_INCLUDE_DIR "${eigen_SOURCE_DIR}" CACHE PATH "Path to Eigen include directory") 49 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/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 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | if(CMAKE_VERSION VERSION_GREATER 3.24) 24 | cmake_policy(SET CMP0135 OLD) 25 | endif() 26 | 27 | function(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) 28 | string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) 29 | set(USE_FROM_SYSTEM_OPTION "USE_SYSTEM_${PACKAGE_NAME_UP}") 30 | if(${${USE_FROM_SYSTEM_OPTION}}) 31 | find_package(${PACKAGE_NAME} QUIET NO_MODULE) 32 | endif() 33 | if(NOT ${${USE_FROM_SYSTEM_OPTION}} OR NOT TARGET ${TARGET_NAME}) 34 | set(${USE_FROM_SYSTEM_OPTION} OFF PARENT_SCOPE) 35 | include(${INCLUDED_CMAKE_PATH}) 36 | endif() 37 | endfunction() 38 | 39 | find_external_dependency("tsl-robin-map" "tsl::robin_map" "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") 40 | find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") 41 | include(${CMAKE_CURRENT_LIST_DIR}/bonxai/bonxai.cmake) 42 | include(${CMAKE_CURRENT_LIST_DIR}/suitesparse/suitesparse.cmake) 43 | find_external_dependency("g2o" "g2o::core" "${CMAKE_CURRENT_LIST_DIR}/g2o/g2o.cmake") 44 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/g2o/LICENSE: -------------------------------------------------------------------------------- 1 | g2o - General Graph Optimization 2 | Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat, 3 | Kurt Konolige, and Wolfram Burgard 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are 8 | met: 9 | 10 | * Redistributions of source code must retain the above copyright notice, 11 | this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 17 | IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 18 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 19 | PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 20 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 21 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 22 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 23 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 24 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/g2o/g2o.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | 25 | set(G2O_BUILD_APPS OFF CACHE BOOL "Build g2o apps") 26 | set(G2O_BUILD_EXAMPLES OFF CACHE BOOL "Build g2o examples") 27 | set(G2O_USE_LOGGING OFF CACHE BOOL "Try to use spdlog for logging") 28 | set(G2O_BUILD_WITH_MARCH_NATIVE OFF CACHE BOOL "Build with \"-march native\"") 29 | set(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)") 30 | set(G2O_USE_OPENGL OFF CACHE BOOL "Build g2o with OpenGL support for visualization") 31 | set(BUILD_SHARED_LIBS OFF CACHE BOOL "Build Shared Libraries (preferred and required for the g2o plugin system)") 32 | set(G2O_BUILD_SLAM3D_ADDON_TYPES OFF CACHE BOOL "no SLAM 3D addons") 33 | set(G2O_BUILD_SIM3_TYPES OFF CACHE BOOL "no sim3") 34 | set(G2O_BUILD_SBA_TYPES OFF CACHE BOOL "no sparse bundle adjustment") 35 | set(G2O_BUILD_ICP_TYPES OFF CACHE BOOL "no icp") 36 | set(G2O_BUILD_DATA_TYPES OFF CACHE BOOL "Build SLAM2D data types") 37 | set(G2O_BUILD_SLAM2D_TYPES OFF CACHE BOOL "no SLAM 2D types") 38 | set(G2O_BUILD_SCLAM2D_TYPES OFF CACHE BOOL "Build SCLAM2D types") 39 | set(G2O_BUILD_SLAM2D_ADDON_TYPES OFF CACHE BOOL "Build SLAM2D addon types") 40 | 41 | set(G2O_USE_CHOLMOD ON CACHE BOOL "Build g2o with CHOLMOD support") 42 | set(G2O_BUILD_SLAM3D_TYPES ON CACHE BOOL "need just slam 3d types") 43 | 44 | FetchContent_Declare(g2o SYSTEM EXCLUDE_FROM_ALL 45 | URL https://github.com/RainerKuemmerle/g2o/archive/refs/tags/20241228_git.tar.gz) 46 | FetchContent_MakeAvailable(g2o) 47 | add_library(g2o::core ALIAS core) 48 | add_library(g2o::types_slam3d ALIAS types_slam3d) 49 | add_library(g2o::solver_cholmod ALIAS solver_cholmod) 50 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/suitesparse/suitesparse.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | 25 | set(SUITESPARSE_ENABLE_PROJECTS "suitesparse_config;amd;camd;ccolamd;colamd;cholmod" 26 | CACHE STRING "Only install required packages") 27 | 28 | set(SUITESPARSE_USE_PYTHON OFF CACHE BOOL "build Python interfaces for SuiteSparse packages (SPEX)") 29 | set(SUITESPARSE_USE_OPENMP OFF CACHE BOOL "Use OpenMP in libraries by default if available") 30 | set(SUITESPARSE_USE_CUDA OFF CACHE BOOL "Build SuiteSparse with CUDA support") 31 | set(CHOLMOD_SUPERNODAL OFF CACHE BOOL "Build SuiteSparse SuperNodal library") 32 | set(BUILD_TESTING OFF CACHE BOOL "SuiteSparse Build Testing") 33 | set(SUITESPARSE_USE_FORTRAN OFF CACHE BOOL "use Fortran") 34 | set(SUITESPARSE_DEMOS OFF CACHE BOOL "SuiteSparse Demos") 35 | set(BUILD_SHARED_LIBS OFF CACHE BOOL "shared off") 36 | # Keep this to avoid BLAS config error 37 | set(SuiteSparse_BLAS_integer "int64_t" CACHE STRING "BLAS Integer type") 38 | 39 | set(SUITESPARSE_USE_STRICT ON CACHE BOOL "treat all _USE__ settings as strict") 40 | set(BUILD_STATIC_LIBS ON CACHE BOOL "static on") 41 | 42 | FetchContent_Declare( 43 | suitesparse SYSTEM EXCLUDE_FROM_ALL 44 | URL https://github.com/DrTimothyAldenDavis/SuiteSparse/archive/refs/tags/v7.10.1.tar.gz 45 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/suitesparse.patch UPDATE_DISCONNECTED 1) 46 | FetchContent_MakeAvailable(suitesparse) 47 | if(TARGET SuiteSparse::CHOLMOD) 48 | set(SuiteSparse_CHOLMOD_FOUND ON CACHE BOOL "SuiteSparse::CHOLMOD Exists") 49 | set(SuiteSparse_FOUND ON CACHE BOOL "SuiteSparse exists if target SuiteSparse::CHOLMOD exists") 50 | set(SuiteSparse_NO_CMAKE ON CACHE BOOL "Do not try to find SuiteSparse") 51 | endif() 52 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/3rdparty/suitesparse/suitesparse.patch: -------------------------------------------------------------------------------- 1 | diff --git a/SuiteSparse_config/CMakeLists.txt b/SuiteSparse_config/CMakeLists.txt 2 | index 695a53897..017438898 100644 3 | --- a/SuiteSparse_config/CMakeLists.txt 4 | +++ b/SuiteSparse_config/CMakeLists.txt 5 | @@ -123,12 +123,6 @@ else ( ) 6 | message ( STATUS "No OpenMP and no clock_gettime available. Timing functions won't work." ) 7 | endif ( ) 8 | 9 | -#------------------------------------------------------------------------------- 10 | -# find the BLAS 11 | -#------------------------------------------------------------------------------- 12 | - 13 | -include ( SuiteSparseBLAS ) 14 | - 15 | #------------------------------------------------------------------------------- 16 | # configure files 17 | #------------------------------------------------------------------------------- 18 | @@ -241,16 +235,6 @@ else ( ) 19 | endif ( ) 20 | endif ( ) 21 | 22 | -# BLAS: 23 | -if ( BLAS_FOUND ) 24 | - # SuiteSparse_config does not itself require the BLAS. It just needs to 25 | - # know which BLAS is going to be used by the rest of SuiteSparse so it 26 | - # can configure SuiteSparse_config.h properly. 27 | - message ( STATUS "BLAS libraries: ${BLAS_LIBRARIES} ") 28 | - message ( STATUS "BLAS linker flags: ${BLAS_LINKER_FLAGS} ") 29 | - message ( STATUS "BLAS include: ${BLAS_INCLUDE_DIRS} ") 30 | -endif ( ) 31 | - 32 | #------------------------------------------------------------------------------- 33 | # SuiteSparseConfig installation location 34 | #------------------------------------------------------------------------------- 35 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/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 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/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 EXCLUDE_FROM_ALL 25 | URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.4.0.tar.gz) 26 | FetchContent_MakeAvailable(tessil) 27 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16...3.26) 2 | project(kiss_slam VERSION 0.0.2 LANGUAGES CXX) 3 | 4 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 5 | option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) 6 | option(USE_SYSTEM_G2O "Use a pre-installed version of g2o" ON) 7 | 8 | set(CMAKE_BUILD_TYPE Release) 9 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 10 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 11 | 12 | include(3rdparty/find_dependencies.cmake) 13 | include(cmake/CompilerOptions.cmake) 14 | 15 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/voxel_map) 16 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/pgo) 17 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/occupancy_mapper) 18 | 19 | pybind11_add_module(kiss_slam_pybind MODULE kiss_slam_pybind.cpp) 20 | target_link_libraries(kiss_slam_pybind PRIVATE pgo voxel_map Eigen3::Eigen occupancy_mapper) 21 | target_compile_features(kiss_slam_pybind PUBLIC cxx_std_20) 22 | install(TARGETS kiss_slam_pybind DESTINATION .) 23 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/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 | $<$:-Wno-deprecated-declarations> 39 | # GNU 40 | $<$:-fdiagnostics-color=always> 41 | $<$:-Werror> 42 | $<$:-Wall> 43 | $<$:-Wextra> 44 | $<$:-pedantic> 45 | $<$:-Wcast-align> 46 | $<$:-Wcast-qual> 47 | $<$:-Wconversion> 48 | $<$:-Wdisabled-optimization> 49 | $<$:-Woverloaded-virtual>) 50 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 51 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 52 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 53 | PUBLIC $ $) 54 | endfunction() 55 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/kiss_slam_pybind.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "occupancy_mapper.hpp" 35 | #include "stl_vector_eigen.h" 36 | #include "voxel_map.hpp" 37 | 38 | namespace py = pybind11; 39 | using namespace py::literals; 40 | PYBIND11_MAKE_OPAQUE(pgo::PoseGraphOptimizer::PoseIDMap); 41 | PYBIND11_MAKE_OPAQUE(std::vector); 42 | PYBIND11_MAKE_OPAQUE(std::vector); 43 | 44 | PYBIND11_MODULE(kiss_slam_pybind, m) { 45 | using namespace pgo; 46 | using namespace voxel_map; 47 | using namespace occupancy_mapper; 48 | auto vector3fvector = pybind_eigen_vector_of_vector( 49 | m, "_Vector3fVector", "std::vector", 50 | py::py_array_to_vectors); 51 | auto vector3ivector = pybind_eigen_vector_of_vector( 52 | m, "_Vector3iVector", "std::vector", 53 | py::py_array_to_vectors); 54 | 55 | py::bind_map(m, "PoseIDMap"); 56 | py::class_ pgo(m, "_PoseGraphOptimizer", "Don't use this"); 57 | pgo.def(py::init(), "max_iterations"_a) 58 | .def("_add_variable", &PoseGraphOptimizer::addVariable, "id"_a, "T"_a) 59 | .def("_fix_variable", &PoseGraphOptimizer::fixVariable, "id"_a) 60 | .def("_add_factor", &PoseGraphOptimizer::addFactor, "id_source"_a, "id_target"_a, "T"_a, 61 | "omega"_a) 62 | .def("_optimize", &PoseGraphOptimizer::optimize) 63 | .def("_estimates", &PoseGraphOptimizer::estimates) 64 | .def("_read_graph", &PoseGraphOptimizer::readGraph, "filename"_a) 65 | .def("_write_graph", &PoseGraphOptimizer::writeGraph, "filename"_a); 66 | 67 | py::class_ internal_map(m, "_VoxelMap", "Don't use this"); 68 | internal_map.def(py::init(), "voxel_size"_a) 69 | .def("_integrate_frame", &VoxelMap::IntegrateFrame, "points"_a, "pose"_a) 70 | .def("_add_points", &VoxelMap::AddPoints, "points"_a) 71 | .def("_point_cloud", &VoxelMap::Pointcloud) 72 | .def("_clear", &VoxelMap::Clear) 73 | .def("_num_voxels", &VoxelMap::NumVoxels) 74 | .def("_per_voxel_point_and_normal", &VoxelMap::PerVoxelPointAndNormal); 75 | 76 | py::class_ grid_mapper(m, "_OccupancyMapper", "Don't use this"); 77 | grid_mapper.def(py::init(), "resolution"_a, "max_range"_a) 78 | .def("_integrate_frame", &OccupancyMapper::IntegrateFrame, "pointcloud"_a, "pose"_a) 79 | .def("_get_active_voxels", &OccupancyMapper::GetOccupancyInformation) 80 | .def("_get_occupied_voxels", &OccupancyMapper::GetOccupiedVoxels, "probability_threshold"_a) 81 | .def("_save_occupancy_volume", &OccupancyMapper::SaveOccupancyVolume, "filename"_a); 82 | } 83 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/occupancy_mapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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(occupancy_mapper STATIC occupancy_mapper.cpp) 24 | target_link_libraries(occupancy_mapper PUBLIC Eigen3::Eigen bonxai_core) 25 | target_include_directories(occupancy_mapper PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${PROJECT_SOURCE_DIR}) 26 | set_global_target_properties(occupancy_mapper) 27 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/occupancy_mapper/occupancy_mapper.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 4 | // Meher Malladi 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 | #include "occupancy_mapper.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace { 33 | static constexpr float logodds_free = -0.8473f; 34 | static constexpr float logodds_occ = 2.1972f; 35 | inline float ProbabilityOccupied(const float logodds) { 36 | return 1.0f - (1.0f / (1.0f + std::exp(logodds))); 37 | } 38 | inline float LogOddsOccupied(const float probability) { 39 | return std::log((1.0f / (1.0f - probability)) - 1.0f); 40 | } 41 | } // namespace 42 | 43 | namespace occupancy_mapper { 44 | OccupancyMapper::OccupancyMapper(const float resolution, const float max_range) 45 | : max_range_(max_range), map_(resolution), accessor_(map_.createAccessor()) {} 46 | 47 | void OccupancyMapper::IntegrateFrame(const Vector3fVector &pointcloud, 48 | const Eigen::Matrix4f &pose) { 49 | const Eigen::Matrix3f &R = pose.block<3, 3>(0, 0); 50 | const Eigen::Vector3f &t = pose.block<3, 1>(0, 3); 51 | const auto start_coord = map_.posToCoord(t); 52 | std::for_each(pointcloud.cbegin(), pointcloud.cend(), [&](const Eigen::Vector3f &point) { 53 | const auto point_range = point.norm(); 54 | if (point_range < max_range_) { 55 | const Eigen::Vector3f point_tf = R * point + t; 56 | const auto end_coord = map_.posToCoord(point_tf); 57 | Bresenham3DLine(start_coord, end_coord); 58 | } 59 | }); 60 | } 61 | 62 | std::tuple> OccupancyMapper::GetOccupancyInformation() const { 63 | Vector3iVector voxel_indices; 64 | std::vector voxel_occupancies; 65 | const auto num_of_active_voxels = map_.activeCellsCount(); 66 | voxel_indices.reserve(num_of_active_voxels); 67 | voxel_occupancies.reserve(num_of_active_voxels); 68 | map_.forEachCell([&](const float &logodds, const Bonxai::CoordT &voxel) { 69 | voxel_indices.emplace_back(Bonxai::ConvertPoint(voxel)); 70 | voxel_occupancies.emplace_back(ProbabilityOccupied(logodds)); 71 | }); 72 | return std::make_tuple(voxel_indices, voxel_occupancies); 73 | } 74 | 75 | Vector3iVector OccupancyMapper::GetOccupiedVoxels( 76 | const float occupancy_probability_threshold) const { 77 | Vector3iVector voxel_indices; 78 | const float logodds_threshold = LogOddsOccupied(occupancy_probability_threshold); 79 | const auto num_of_active_voxels = map_.activeCellsCount(); 80 | voxel_indices.reserve(num_of_active_voxels); 81 | map_.forEachCell([&](const float &logodds, const Bonxai::CoordT &voxel) { 82 | if (logodds > logodds_threshold) { 83 | voxel_indices.emplace_back(Bonxai::ConvertPoint(voxel)); 84 | } 85 | }); 86 | voxel_indices.shrink_to_fit(); 87 | return voxel_indices; 88 | } 89 | 90 | void OccupancyMapper::UpdateVoxelOccupancy(const Bonxai::CoordT &coord, const float value) { 91 | // tg exploit Vdb caching 92 | accessor_.setCellOn(coord, 0.0); 93 | float *logodds = accessor_.value(coord); 94 | *logodds += value; 95 | } 96 | 97 | void OccupancyMapper::Bresenham3DLine(const Bonxai::CoordT &start_coord, 98 | const Bonxai::CoordT &end_coord) { 99 | const auto ray = end_coord - start_coord; 100 | const int x_sign = ray.x > 0 ? 1 : -1; 101 | const int y_sign = ray.y > 0 ? 1 : -1; 102 | const int z_sign = ray.z > 0 ? 1 : -1; 103 | 104 | const int ray_x_abs = x_sign * ray.x; 105 | const int ray_y_abs = y_sign * ray.y; 106 | const int ray_z_abs = z_sign * ray.z; 107 | 108 | int p1 = 0; 109 | int p2 = 0; 110 | Bonxai::CoordT delta{0, 0, 0}; 111 | if (ray_x_abs >= ray_y_abs && ray_x_abs > ray_z_abs) { 112 | p1 = 2 * ray_y_abs - ray_x_abs; 113 | p2 = 2 * ray_z_abs - ray_x_abs; 114 | while (std::abs(delta.x) < ray_x_abs) { 115 | const auto voxel = start_coord + delta; 116 | UpdateVoxelOccupancy(voxel, logodds_free); 117 | if (p1 >= 0) { 118 | delta.y += y_sign; 119 | p1 -= (2 * ray_x_abs); 120 | } 121 | if (p2 >= 0) { 122 | delta.z += z_sign; 123 | p2 -= (2 * ray_x_abs); 124 | } 125 | p1 += (2 * ray_y_abs); 126 | p2 += (2 * ray_z_abs); 127 | delta.x += x_sign; 128 | } 129 | } else if (ray_y_abs >= ray_x_abs && ray_y_abs > ray_z_abs) { 130 | p1 = 2 * ray_x_abs - ray_y_abs; 131 | p2 = 2 * ray_z_abs - ray_y_abs; 132 | while (std::abs(delta.y) < ray_y_abs) { 133 | const auto voxel = start_coord + delta; 134 | UpdateVoxelOccupancy(voxel, logodds_free); 135 | if (p1 >= 0) { 136 | delta.x += x_sign; 137 | p1 -= (2 * ray_y_abs); 138 | } 139 | if (p2 >= 0) { 140 | delta.z += z_sign; 141 | p2 -= (2 * ray_y_abs); 142 | } 143 | p1 += (2 * ray_x_abs); 144 | p2 += (2 * ray_z_abs); 145 | delta.y += y_sign; 146 | } 147 | } else { 148 | p1 = 2 * ray_y_abs - ray_z_abs; 149 | p2 = 2 * ray_x_abs - ray_z_abs; 150 | while (std::abs(delta.z) < ray_z_abs) { 151 | const auto voxel = start_coord + delta; 152 | UpdateVoxelOccupancy(voxel, logodds_free); 153 | if (p1 >= 0) { 154 | delta.y += y_sign; 155 | p1 -= (2 * ray_z_abs); 156 | } 157 | if (p2 >= 0) { 158 | delta.x += x_sign; 159 | p2 -= (2 * ray_z_abs); 160 | } 161 | p1 += (2 * ray_y_abs); 162 | p2 += (2 * ray_x_abs); 163 | delta.z += z_sign; 164 | } 165 | } 166 | UpdateVoxelOccupancy(end_coord, logodds_occ); 167 | } 168 | } // namespace occupancy_mapper 169 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/occupancy_mapper/occupancy_mapper.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 4 | // Meher Malladi 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 | #pragma once 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | using Vector3fVector = std::vector; 33 | using Vector3iVector = std::vector; 34 | 35 | namespace occupancy_mapper { 36 | class OccupancyMapper { 37 | public: 38 | OccupancyMapper(const float resolution, const float max_range); 39 | ~OccupancyMapper() = default; 40 | 41 | void IntegrateFrame(const Vector3fVector &pointcloud, const Eigen::Matrix4f &pose); 42 | std::tuple> GetOccupancyInformation() const; 43 | Vector3iVector GetOccupiedVoxels(const float occupancy_probability_threshold) const; 44 | void SaveOccupancyVolume(const std::string &filename) const { 45 | std::ofstream data(filename, std::ios::binary); 46 | Bonxai::Serialize(data, map_); 47 | }; 48 | 49 | private: 50 | void Bresenham3DLine(const Bonxai::CoordT &start_coord, const Bonxai::CoordT &end_coord); 51 | void UpdateVoxelOccupancy(const Bonxai::CoordT &coord, const float value); 52 | 53 | float max_range_ = 0.0f; 54 | Bonxai::VoxelGrid map_; 55 | using AccessorType = typename Bonxai::VoxelGrid::Accessor; 56 | AccessorType accessor_; 57 | }; 58 | } // namespace occupancy_mapper 59 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/pgo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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(pgo STATIC pose_graph_optimizer.cpp) 24 | target_link_libraries(pgo PUBLIC g2o::core g2o::types_slam3d g2o::solver_cholmod Eigen3::Eigen) 25 | target_include_directories(pgo PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${PROJECT_SOURCE_DIR}) 26 | set_global_target_properties(pgo) 27 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/pgo/pose_graph_optimizer.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino 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 "pose_graph_optimizer.hpp" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace { 38 | static constexpr double epsilon = 1e-6; 39 | } 40 | // clang-format off 41 | namespace g2o { 42 | G2O_REGISTER_TYPE(VERTEX_SE3:QUAT, VertexSE3) 43 | G2O_REGISTER_TYPE(EDGE_SE3:QUAT, EdgeSE3) 44 | } // namespace g2o 45 | // clang-format on 46 | 47 | namespace pgo { 48 | using BlockSolverType = g2o::BlockSolver>; 49 | using LinearSolverType = g2o::LinearSolverCholmod; 50 | using AlgorithmType = g2o::OptimizationAlgorithmDogleg; 51 | 52 | PoseGraphOptimizer::PoseGraphOptimizer(const int max_iterations) : max_iterations_(max_iterations) { 53 | graph = std::make_unique(); 54 | graph->setVerbose(true); 55 | 56 | auto solver = 57 | new AlgorithmType(std::make_unique(std::make_unique())); 58 | 59 | auto terminateAction = new g2o::SparseOptimizerTerminateAction; 60 | terminateAction->setGainThreshold(epsilon); 61 | graph->addPostIterationAction(terminateAction); 62 | graph->setAlgorithm(solver); 63 | } 64 | 65 | void PoseGraphOptimizer::fixVariable(const int id) { graph->vertex(id)->setFixed(true); } 66 | 67 | void PoseGraphOptimizer::addVariable(const int id, const Eigen::Matrix4d &T) { 68 | Eigen::Isometry3d pose(T); 69 | g2o::VertexSE3 *variable = new g2o::VertexSE3(); 70 | variable->setId(id); 71 | variable->setEstimate(pose); 72 | graph->addVertex(variable); 73 | } 74 | 75 | void PoseGraphOptimizer::addFactor(const int id_source, 76 | const int id_target, 77 | const Eigen::Matrix4d &T, 78 | const Eigen::Matrix6d &information_matrix) { 79 | Eigen::Isometry3d relative_pose(T); 80 | g2o::EdgeSE3 *factor = new g2o::EdgeSE3(); 81 | factor->setVertex(0, graph->vertex(id_target)); 82 | factor->setVertex(1, graph->vertex(id_source)); 83 | factor->setInformation(information_matrix); 84 | factor->setMeasurement(relative_pose); 85 | graph->addEdge(factor); 86 | } 87 | 88 | PoseGraphOptimizer::PoseIDMap PoseGraphOptimizer::estimates() const { 89 | const g2o::HyperGraph::VertexIDMap &variables = graph->vertices(); 90 | PoseIDMap poses; 91 | std::transform(variables.cbegin(), variables.cend(), std::inserter(poses, poses.end()), 92 | [](const auto &id_var) { 93 | const auto &[id, v] = id_var; 94 | Eigen::Isometry3d pose = static_cast(v)->estimate(); 95 | return std::make_pair(id, pose.matrix()); 96 | }); 97 | return poses; 98 | } 99 | 100 | void PoseGraphOptimizer::optimize() { 101 | graph->initializeOptimization(); 102 | graph->optimize(max_iterations_); 103 | } 104 | } // namespace pgo 105 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/pgo/pose_graph_optimizer.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino 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 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace Eigen { 31 | using Matrix4d = Eigen::Matrix; 32 | using Matrix6d = Eigen::Matrix; 33 | } // namespace Eigen 34 | namespace pgo { 35 | class PoseGraphOptimizer { 36 | public: 37 | using PoseIDMap = std::map; 38 | explicit PoseGraphOptimizer(const int max_iterations); 39 | 40 | void fixVariable(const int id); 41 | void addVariable(const int id, const Eigen::Matrix4d &T); 42 | 43 | void addFactor(const int id_source, 44 | const int id_target, 45 | const Eigen::Matrix4d &T, 46 | const Eigen::Matrix6d &information_matrix); 47 | 48 | [[nodiscard]] PoseIDMap estimates() const; 49 | 50 | inline void readGraph(const std::string &filename) { 51 | std::ifstream file(filename.c_str()); 52 | graph->clear(); 53 | graph->load(file); 54 | } 55 | inline void writeGraph(const std::string &filename) const { graph->save(filename.c_str()); } 56 | 57 | void optimize(); 58 | 59 | private: 60 | std::unique_ptr graph; 61 | int max_iterations_; 62 | }; 63 | } // namespace pgo 64 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_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 | template 61 | std::vector py_array_to_vectors( 62 | py::array_t array) { 63 | int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; 64 | if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { 65 | throw py::cast_error(); 66 | } 67 | std::vector eigen_vectors(array.shape(0)); 68 | auto array_unchecked = array.template mutable_unchecked<2>(); 69 | for (auto i = 0; i < array_unchecked.shape(0); ++i) { 70 | eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); 71 | } 72 | return eigen_vectors; 73 | } 74 | 75 | } // namespace pybind11 76 | 77 | template , 79 | typename holder_type = std::unique_ptr, 80 | typename InitFunc> 81 | py::class_ pybind_eigen_vector_of_vector(py::module &m, 82 | const std::string &bind_name, 83 | const std::string &repr_name, 84 | InitFunc init_func) { 85 | using Scalar = typename EigenVector::Scalar; 86 | auto vec = py::bind_vector_without_repr>( 87 | m, bind_name, py::buffer_protocol(), py::module_local()); 88 | vec.def(py::init(init_func)); 89 | vec.def_buffer([](std::vector &v) -> py::buffer_info { 90 | size_t rows = EigenVector::RowsAtCompileTime; 91 | return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, 92 | {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); 93 | }); 94 | vec.def("__repr__", [repr_name](const std::vector &v) { 95 | return repr_name + std::string(" with ") + std::to_string(v.size()) + 96 | std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); 97 | }); 98 | vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); 99 | vec.def("__deepcopy__", 100 | [](std::vector &v) { return std::vector(v); }); 101 | 102 | // py::detail must be after custom constructor 103 | using Class_ = py::class_>; 104 | py::detail::vector_if_copy_constructible(vec); 105 | py::detail::vector_if_equal_operator(vec); 106 | py::detail::vector_modifiers(vec); 107 | py::detail::vector_accessor(vec); 108 | 109 | return vec; 110 | } 111 | template , 113 | typename Vector = std::vector, 114 | typename holder_type = std::unique_ptr> 115 | py::class_ pybind_eigen_vector_of_matrix(py::module &m, 116 | const std::string &bind_name, 117 | const std::string &repr_name) { 118 | typedef typename EigenMatrix::Scalar Scalar; 119 | auto vec = py::bind_vector_without_repr>( 120 | m, bind_name, py::buffer_protocol()); 121 | vec.def_buffer([](std::vector &v) -> py::buffer_info { 122 | // We use this function to bind Eigen default matrix. 123 | // Thus they are all column major. 124 | size_t rows = EigenMatrix::RowsAtCompileTime; 125 | size_t cols = EigenMatrix::ColsAtCompileTime; 126 | return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 3, 127 | {v.size(), rows, cols}, 128 | {sizeof(EigenMatrix), sizeof(Scalar), sizeof(Scalar) * rows}); 129 | }); 130 | vec.def("__repr__", [repr_name](const std::vector &v) { 131 | return repr_name + std::string(" with ") + std::to_string(v.size()) + 132 | std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); 133 | }); 134 | vec.def("__copy__", [](std::vector &v) { 135 | return std::vector(v); 136 | }); 137 | vec.def("__deepcopy__", [](std::vector &v, py::dict &memo) { 138 | return std::vector(v); 139 | }); 140 | 141 | // py::detail must be after custom constructor 142 | using Class_ = py::class_>; 143 | py::detail::vector_if_copy_constructible(vec); 144 | py::detail::vector_if_equal_operator(vec); 145 | py::detail::vector_modifiers(vec); 146 | py::detail::vector_accessor(vec); 147 | 148 | return vec; 149 | } 150 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/voxel_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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(voxel_map STATIC voxel_map.cpp) 24 | target_link_libraries(voxel_map PUBLIC tsl::robin_map Eigen3::Eigen) 25 | target_include_directories(voxel_map PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${PROJECT_SOURCE_DIR}) 26 | set_global_target_properties(voxel_map) 27 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/voxel_map/voxel_map.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 "voxel_map.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace { 34 | 35 | inline Voxel ToVoxelCoordinates(const Eigen::Vector3f &point, const float voxel_size) { 36 | return Voxel(static_cast(std::floor(point.x() / voxel_size)), 37 | static_cast(std::floor(point.y() / voxel_size)), 38 | static_cast(std::floor(point.z() / voxel_size))); 39 | } 40 | 41 | static constexpr unsigned int min_points_for_covariance_computation = 5; 42 | 43 | std::tuple ComputeCentroidAndNormal( 44 | const voxel_map::VoxelBlock &coordinates) { 45 | const float num_points = static_cast(coordinates.size()); 46 | const Eigen::Vector3f &mean = 47 | std::reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Vector3f().setZero()) / 48 | num_points; 49 | 50 | const Eigen::Matrix3f &covariance = 51 | std::transform_reduce(coordinates.cbegin(), coordinates.cend(), Eigen::Matrix3f().setZero(), 52 | std::plus(), 53 | [&mean](const Eigen::Vector3f &point) { 54 | const Eigen::Vector3f ¢ered = point - mean; 55 | const Eigen::Matrix3f S = centered * centered.transpose(); 56 | return S; 57 | }) / 58 | (num_points - 1); 59 | Eigen::SelfAdjointEigenSolver solver(covariance); 60 | const Eigen::Vector3f normal = solver.eigenvectors().col(0); 61 | return std::make_tuple(mean, normal); 62 | } 63 | 64 | } // namespace 65 | 66 | namespace voxel_map { 67 | 68 | void VoxelBlock::emplace_back(const Eigen::Vector3f &p) { 69 | if (size() < max_points_per_normal_computation) { 70 | points.at(num_points) = p; 71 | ++num_points; 72 | } 73 | } 74 | 75 | VoxelMap::VoxelMap(const float voxel_size) 76 | : voxel_size_(voxel_size), 77 | map_resolution_(voxel_size / 78 | static_cast(std::sqrt(max_points_per_normal_computation))) {} 79 | 80 | std::vector VoxelMap::Pointcloud() const { 81 | std::vector points; 82 | points.reserve(map_.size() * max_points_per_normal_computation); 83 | std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { 84 | const auto &voxel_points = map_element.second; 85 | std::for_each(voxel_points.cbegin(), voxel_points.cend(), 86 | [&](const auto &p) { points.emplace_back(p.template cast()); }); 87 | }); 88 | points.shrink_to_fit(); 89 | return points; 90 | } 91 | 92 | void VoxelMap::IntegrateFrame(const std::vector &points, 93 | const Eigen::Matrix4f &pose) { 94 | std::vector points_transformed(points.size()); 95 | const auto &R = pose.block<3, 3>(0, 0); 96 | const auto &t = pose.block<3, 1>(0, 3); 97 | std::transform(points.cbegin(), points.cend(), points_transformed.begin(), 98 | [&](const auto &point) { return R * point + t; }); 99 | AddPoints(points_transformed); 100 | } 101 | 102 | void VoxelMap::AddPoints(const std::vector &points) { 103 | std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { 104 | const auto voxel = ToVoxelCoordinates(point, voxel_size_); 105 | const auto &[it, inserted] = map_.insert({voxel, VoxelBlock()}); 106 | if (!inserted) { 107 | auto &voxel_points = it.value(); 108 | if (voxel_points.size() == max_points_per_normal_computation || 109 | std::any_of(voxel_points.cbegin(), voxel_points.cend(), 110 | [&](const auto &voxel_point) { 111 | return (voxel_point - point).norm() < map_resolution_; 112 | })) { 113 | return; 114 | } 115 | } 116 | it.value().emplace_back(point); 117 | }); 118 | } 119 | 120 | std::tuple VoxelMap::PerVoxelPointAndNormal() const { 121 | Vector3fVector points; 122 | points.reserve(map_.size()); 123 | Vector3fVector normals; 124 | normals.reserve(map_.size()); 125 | std::for_each(map_.cbegin(), map_.cend(), [&](const auto &inner_block) { 126 | const auto &voxel_block = inner_block.second; 127 | if (voxel_block.size() >= min_points_for_covariance_computation) { 128 | const auto &[mean, normal] = ComputeCentroidAndNormal(voxel_block); 129 | points.emplace_back(mean); 130 | normals.emplace_back(normal); 131 | } 132 | }); 133 | points.shrink_to_fit(); 134 | normals.shrink_to_fit(); 135 | return std::make_tuple(points, normals); 136 | } 137 | 138 | } // namespace voxel_map 139 | -------------------------------------------------------------------------------- /kiss_slam/kiss_slam_pybind/voxel_map/voxel_map.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | using Vector3fVector = std::vector; 33 | 34 | using Voxel = Eigen::Vector3i; 35 | template <> 36 | struct std::hash { 37 | std::size_t operator()(const Voxel &voxel) const { 38 | const uint32_t *vec = reinterpret_cast(voxel.data()); 39 | return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); 40 | } 41 | }; 42 | 43 | // Same default as Open3d 44 | static constexpr unsigned int max_points_per_normal_computation = 20; 45 | 46 | namespace voxel_map { 47 | 48 | struct VoxelBlock { 49 | void emplace_back(const Eigen::Vector3f &point); 50 | inline constexpr size_t size() const { return num_points; } 51 | auto cbegin() const { return points.cbegin(); } 52 | auto cend() const { return std::next(points.cbegin(), num_points); } 53 | std::array points; 54 | size_t num_points = 0; 55 | }; 56 | 57 | struct VoxelMap { 58 | explicit VoxelMap(const float voxel_size); 59 | 60 | inline void Clear() { map_.clear(); } 61 | inline bool Empty() const { return map_.empty(); } 62 | void IntegrateFrame(const std::vector &points, const Eigen::Matrix4f &pose); 63 | void AddPoints(const std::vector &points); 64 | Vector3fVector Pointcloud() const; 65 | 66 | size_t NumVoxels() const { return map_.size(); } 67 | 68 | std::tuple PerVoxelPointAndNormal() const; 69 | 70 | float voxel_size_; 71 | float map_resolution_; 72 | tsl::robin_map map_; 73 | }; 74 | } // namespace voxel_map 75 | -------------------------------------------------------------------------------- /kiss_slam/local_map_graph.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 copy import deepcopy as copy 24 | 25 | import numpy as np 26 | import open3d as o3d 27 | 28 | from kiss_slam.voxel_map import VoxelMap 29 | 30 | 31 | class LocalMap: 32 | def __init__(self, id: np.uint64, keypose: np.ndarray): 33 | self.id = id 34 | self.keypose = keypose 35 | self.local_trajectory = [np.eye(4)] 36 | self.pcd = None 37 | 38 | @property 39 | def endpose(self): 40 | return self.keypose @ self.local_trajectory[-1] 41 | 42 | def write(self, filename): 43 | local_map_pcd = copy(self.pcd) 44 | local_map_pcd.transform(self.keypose) 45 | o3d.t.io.write_point_cloud(filename, local_map_pcd) 46 | 47 | 48 | class LocalMapGraph: 49 | def __init__(self): 50 | self.graph = dict() 51 | local_map0 = LocalMap(id=0, keypose=np.eye(4)) 52 | local_map0.local_trajectory.clear() 53 | self.graph[0] = local_map0 54 | 55 | def __getitem__(self, key): 56 | return self.graph[key] 57 | 58 | def local_maps(self): 59 | for local_map in self.graph.values(): 60 | yield local_map 61 | 62 | def keyposes(self): 63 | for local_map in self.graph.values(): 64 | yield local_map.keypose 65 | 66 | @property 67 | def last_id(self): 68 | last_id = next(reversed(self.graph)) 69 | return last_id 70 | 71 | @property 72 | def last_local_map(self): 73 | return self.graph[self.last_id] 74 | 75 | @property 76 | def last_keypose(self): 77 | return self.last_local_map.keypose 78 | 79 | def erase_local_map(self, key: np.uint64): 80 | self.graph.pop(key) 81 | 82 | def erase_last_local_map(self): 83 | self.erase_local_map(self.last_id) 84 | 85 | def finalize_local_map(self, voxel_grid: VoxelMap): 86 | local_map = self.last_local_map 87 | local_map.pcd = voxel_grid.open3d_pcd_with_normals() 88 | proto_id = local_map.id + 1 89 | proto_keypose = local_map.endpose 90 | new_local_map = LocalMap( 91 | proto_id, 92 | np.copy(proto_keypose), 93 | ) 94 | self.graph[new_local_map.id] = new_local_map 95 | -------------------------------------------------------------------------------- /kiss_slam/loop_closer.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | import open3d as o3d 25 | from map_closures.map_closures import MapClosures 26 | 27 | from kiss_slam.config import LoopCloserConfig 28 | from kiss_slam.local_map_graph import LocalMapGraph 29 | from kiss_slam.voxel_map import VoxelMap 30 | 31 | 32 | class LoopCloser: 33 | def __init__(self, config: LoopCloserConfig): 34 | self.config = config 35 | self.detector = MapClosures(config.detector) 36 | self.local_map_voxel_size = config.detector.density_map_resolution 37 | self.icp_threshold = np.sqrt(3) * self.local_map_voxel_size 38 | self.icp_algorithm = o3d.t.pipelines.registration.TransformationEstimationPointToPlane() 39 | self.termination_criteria = o3d.t.pipelines.registration.ICPConvergenceCriteria( 40 | relative_rmse=1e-4 41 | ) 42 | self.overlap_threshold = config.overlap_threshold 43 | 44 | def compute(self, query_id, points, local_map_graph: LocalMapGraph): 45 | closure = self.detector.get_best_closure(query_id, points) 46 | is_good = False 47 | ref_id = -1 48 | pose_constraint = np.eye(4) 49 | if closure.number_of_inliers >= self.config.detector.inliers_threshold: 50 | ref_id = closure.source_id 51 | source = local_map_graph[ref_id].pcd 52 | target = local_map_graph[query_id].pcd 53 | print("\nKissSLAM| Closure Detected") 54 | is_good, pose_constraint = self.validate_closure(source, target, closure.pose) 55 | return is_good, ref_id, query_id, pose_constraint 56 | 57 | # This is the thing that takes the most time 58 | def validate_closure(self, source, target, initial_guess): 59 | registration_result = o3d.t.pipelines.registration.icp( 60 | source, 61 | target, 62 | self.icp_threshold, 63 | initial_guess, 64 | self.icp_algorithm, 65 | self.termination_criteria, 66 | ) 67 | union_map = VoxelMap(self.local_map_voxel_size) 68 | source_pts = source.point.positions.numpy().astype(np.float64) 69 | target_pts = target.point.positions.numpy().astype(np.float64) 70 | pose = registration_result.transformation.numpy() 71 | union_map.integrate_frame(source_pts, pose) 72 | num_source_voxels = union_map.num_voxels() 73 | num_target_voxels = len(target_pts) 74 | union_map.add_points(target_pts) 75 | union = union_map.num_voxels() 76 | intersection = num_source_voxels + num_target_voxels - union 77 | overlap = intersection / np.min([num_source_voxels, num_target_voxels]) 78 | closure_is_accepted = overlap > self.overlap_threshold 79 | print(f"KissSLAM| LocalMaps Overlap: {overlap}") 80 | if closure_is_accepted: 81 | print("KissSLAM| Closure Accepted") 82 | else: 83 | print(f"KissSLAM| Closure rejected for low overlap.") 84 | return closure_is_accepted, pose 85 | -------------------------------------------------------------------------------- /kiss_slam/occupancy_mapper.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | import numpy as np 26 | import open3d as o3d 27 | import yaml 28 | from kiss_icp.voxelization import voxel_down_sample 29 | from PIL import Image 30 | 31 | from kiss_slam.config import OccupancyMapperConfig 32 | from kiss_slam.kiss_slam_pybind import kiss_slam_pybind 33 | 34 | 35 | class OccupancyGridMapper: 36 | def __init__( 37 | self, 38 | config: OccupancyMapperConfig, 39 | ): 40 | self.config = config 41 | self.occupancy_mapping_pipeline = kiss_slam_pybind._OccupancyMapper( 42 | self.config.resolution, self.config.max_range 43 | ) 44 | 45 | def integrate_frame(self, frame: np.ndarray, pose: np.ndarray): 46 | frame_downsampled = voxel_down_sample(frame, self.config.resolution).astype(np.float32) 47 | self.occupancy_mapping_pipeline._integrate_frame( 48 | kiss_slam_pybind._Vector3fVector(frame_downsampled), pose 49 | ) 50 | 51 | def compute_3d_occupancy_information(self): 52 | active_voxels, occupancies = self.occupancy_mapping_pipeline._get_active_voxels() 53 | self.active_voxels = np.asarray(active_voxels, np.int32) 54 | self.occupancies = np.asarray(occupancies, float) 55 | self.occupied_voxels = self.active_voxels[ 56 | np.where(self.occupancies > self.config.occupied_threshold)[0] 57 | ] 58 | 59 | def compute_3d_occupied_voxels(self): 60 | occupied_voxels = self.occupancy_mapping_pipeline._get_occupied_voxels( 61 | self.config.occupied_threshold 62 | ) 63 | self.occupied_voxels = np.asarray(occupied_voxels, np.int32) 64 | 65 | def compute_2d_occupancy_information(self): 66 | min_z_idx = int(self.config.z_min // self.config.resolution) 67 | max_z_idx = int(self.config.z_max // self.config.resolution) 68 | 69 | indices_in_range = np.where( 70 | (self.active_voxels[:, 2] <= max_z_idx) & (self.active_voxels[:, 2] >= min_z_idx) 71 | )[0] 72 | voxels_in_range = self.active_voxels[indices_in_range] 73 | 74 | self.lower_bound = np.min(voxels_in_range, 0) 75 | self.upper_bound = np.max(voxels_in_range, 0) 76 | nrows, ncols, nslices = self.upper_bound - self.lower_bound + 1 77 | occupancy_grid = np.ones((nrows, ncols, nslices)) * 0.5 78 | 79 | occupancy_grid[ 80 | voxels_in_range[:, 0] - self.lower_bound[0], 81 | voxels_in_range[:, 1] - self.lower_bound[1], 82 | voxels_in_range[:, 2] - self.lower_bound[2], 83 | ] = self.occupancies[indices_in_range] 84 | self.occupancy_grid = 1.0 - np.max(occupancy_grid, 2) 85 | 86 | def write_3d_occupancy_grid(self, output_dir): 87 | map_points = (0.5 + self.occupied_voxels) * self.config.resolution 88 | o3d_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(map_points)) 89 | o3d_pcd.estimate_normals() 90 | o3d.io.write_point_cloud(os.path.join(output_dir, "occupancy_pcd.ply"), o3d_pcd) 91 | self.occupancy_mapping_pipeline._save_occupancy_volume( 92 | os.path.join(output_dir, "occupancy_grid_bonxai.bin") 93 | ) 94 | 95 | def write_2d_occupancy_grid(self, output_dir): 96 | image_filename = "map.png" 97 | occupancy_image = np.rot90(self.occupancy_grid, 0) * 255.0 98 | Image.fromarray(np.asarray(occupancy_image, np.uint8)).save( 99 | os.path.join(output_dir, image_filename) 100 | ) 101 | grid_info = { 102 | "image": image_filename, 103 | "resolution": self.config.resolution, 104 | "origin": [ 105 | float(self.lower_bound[0]) * self.config.resolution, 106 | float(self.lower_bound[1]) * self.config.resolution, 107 | 0.0, 108 | ], 109 | "occupied_thresh": self.config.occupied_threshold, 110 | "free_thresh": self.config.free_threshold, 111 | "negate": 0, 112 | } 113 | with open(os.path.join(output_dir, "map.yaml"), "w") as yaml_file: 114 | yaml.dump(grid_info, yaml_file, default_flow_style=False, sort_keys=False) 115 | -------------------------------------------------------------------------------- /kiss_slam/pipeline.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 time 25 | from pathlib import Path 26 | from typing import Optional 27 | 28 | import numpy as np 29 | from kiss_icp.pipeline import OdometryPipeline 30 | from map_closures import map_closures 31 | from tqdm import tqdm, trange 32 | 33 | from kiss_slam.config import load_config 34 | from kiss_slam.occupancy_mapper import OccupancyGridMapper 35 | from kiss_slam.slam import KissSLAM 36 | from kiss_slam.tools.visualizer import RegistrationVisualizer, StubVisualizer 37 | 38 | 39 | class SlamPipeline(OdometryPipeline): 40 | def __init__( 41 | self, 42 | dataset, 43 | config_file: Optional[Path] = None, 44 | visualize: bool = False, 45 | n_scans: int = -1, 46 | jump: int = 0, 47 | refuse_scans: bool = False, 48 | ): 49 | super().__init__(dataset=dataset, config=None, n_scans=n_scans, jump=jump) 50 | self.slam_config = load_config(config_file) 51 | self.config = self.slam_config.kiss_icp_config() 52 | self.visualize = visualize 53 | self.kiss_slam = KissSLAM(self.slam_config) 54 | self.visualizer = RegistrationVisualizer() if self.visualize else StubVisualizer() 55 | self.refuse_scans = refuse_scans 56 | 57 | def run(self): 58 | self._run_pipeline() 59 | self._run_evaluation() 60 | self._evaluate_closures() 61 | self._create_output_dir() 62 | self._write_result_poses() 63 | self._write_gt_poses() 64 | self._write_cfg() 65 | self._write_log() 66 | self._write_graph() 67 | self._write_closures() 68 | self._write_local_maps() 69 | self._global_mapping() 70 | return self.results 71 | 72 | def _run_pipeline(self): 73 | for idx in trange(self._first, self._last, unit=" frames", dynamic_ncols=True): 74 | scan, timestamps = self._next(idx) 75 | start_time = time.perf_counter_ns() 76 | self.kiss_slam.process_scan(scan, timestamps) 77 | self.times[idx - self._first] = time.perf_counter_ns() - start_time 78 | self.visualizer.update(self.kiss_slam) 79 | self.kiss_slam.generate_new_node() 80 | self.kiss_slam.local_map_graph.erase_last_local_map() 81 | self.poses, self.pose_graph = self.kiss_slam.fine_grained_optimization() 82 | self.poses = np.array(self.poses) 83 | 84 | def _global_mapping(self): 85 | if self.refuse_scans: 86 | if hasattr(self._dataset, "reset"): 87 | self._dataset.reset() 88 | ref_ground_alignment = map_closures.align_map_to_local_ground( 89 | self.kiss_slam.local_map_graph[0].pcd.point.positions.cpu().numpy(), 90 | self.slam_config.odometry.mapping.voxel_size, 91 | ) 92 | occupancy_grid_mapper = OccupancyGridMapper(self.slam_config.occupancy_mapper) 93 | print("KissSLAM| Computing Occupancy Grid") 94 | for idx in trange(self._first, self._last, unit=" frames", dynamic_ncols=True): 95 | scan, _ = self._next(idx) 96 | occupancy_grid_mapper.integrate_frame( 97 | scan, ref_ground_alignment @ self.poses[idx - self._first] 98 | ) 99 | occupancy_grid_mapper.compute_3d_occupancy_information() 100 | occupancy_grid_mapper.compute_2d_occupancy_information() 101 | occupancy_dir = os.path.join(self.results_dir, "occupancy_grid") 102 | os.makedirs(occupancy_dir, exist_ok=True) 103 | occupancy_grid_mapper.write_3d_occupancy_grid(occupancy_dir) 104 | occupancy_2d_map_dir = os.path.join(occupancy_dir, "map2d") 105 | os.makedirs(occupancy_2d_map_dir, exist_ok=True) 106 | occupancy_grid_mapper.write_2d_occupancy_grid(occupancy_2d_map_dir) 107 | 108 | def _write_local_maps(self): 109 | local_maps_dir = os.path.join(self.results_dir, "local_maps") 110 | os.makedirs(local_maps_dir, exist_ok=True) 111 | self.kiss_slam.optimizer.write_graph(os.path.join(local_maps_dir, "local_map_graph.g2o")) 112 | plys_dir = os.path.join(local_maps_dir, "plys") 113 | os.makedirs(plys_dir, exist_ok=True) 114 | print("KissSLAM| Writing Local Maps on Disk") 115 | for local_map in tqdm(self.kiss_slam.local_map_graph.local_maps()): 116 | filename = os.path.join(plys_dir, "{:06d}.ply".format(local_map.id)) 117 | local_map.write(filename) 118 | 119 | def _evaluate_closures(self): 120 | self.results.append( 121 | desc="Number of closures found", units="closures", value=len(self.kiss_slam.closures) 122 | ) 123 | 124 | def _write_closures(self): 125 | import matplotlib.pyplot as plt 126 | 127 | locations = [pose[:3, -1] for pose in self.poses] 128 | loc_x = [loc[0] for loc in locations] 129 | loc_y = [loc[1] for loc in locations] 130 | plt.scatter(loc_x, loc_y, s=0.1, color="black") 131 | key_poses = self.kiss_slam.get_keyposes() 132 | for closure in self.kiss_slam.closures: 133 | i, j = closure 134 | plt.plot( 135 | [key_poses[i][0, -1], key_poses[j][0, -1]], 136 | [key_poses[i][1, -1], key_poses[j][1, -1]], 137 | color="red", 138 | linewidth=1, 139 | markersize=1, 140 | ) 141 | plt.savefig(os.path.join(self.results_dir, "trajectory.png"), dpi=2000) 142 | 143 | def _write_graph(self): 144 | self.pose_graph.write_graph(os.path.join(self.results_dir, "trajectory.g2o")) 145 | 146 | def _next(self, idx): 147 | dataframe = self._dataset[idx] 148 | frame, timestamps = dataframe 149 | return frame, timestamps 150 | -------------------------------------------------------------------------------- /kiss_slam/pose_graph_optimizer.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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_slam.config.config import PoseGraphOptimizerConfig 26 | from kiss_slam.kiss_slam_pybind import kiss_slam_pybind 27 | 28 | 29 | class PoseGraphOptimizer: 30 | def __init__(self, config: PoseGraphOptimizerConfig): 31 | self.pgo = kiss_slam_pybind._PoseGraphOptimizer(config.max_iterations) 32 | 33 | def add_variable(self, id_: int, pose: np.ndarray): 34 | self.pgo._add_variable(id_, pose) 35 | 36 | def fix_variable(self, id_: int): 37 | self.pgo._fix_variable(id_) 38 | 39 | def add_factor(self, id_source, id_target, relative_pose, information_matrix): 40 | self.pgo._add_factor(id_source, id_target, relative_pose, information_matrix) 41 | 42 | def optimize(self): 43 | print("KissSLAM| Optimize Pose Graph") 44 | self.pgo._optimize() 45 | 46 | def estimates(self): 47 | return self.pgo._estimates() 48 | 49 | def read_graph(self, filename: str): 50 | self.pgo._read_graph(filename) 51 | 52 | def write_graph(self, filename: str): 53 | self.pgo._write_graph(filename) 54 | -------------------------------------------------------------------------------- /kiss_slam/slam.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | from kiss_icp.kiss_icp import KissICP 25 | from kiss_icp.voxelization import voxel_down_sample 26 | 27 | from kiss_slam.config import KissSLAMConfig 28 | from kiss_slam.local_map_graph import LocalMapGraph 29 | from kiss_slam.loop_closer import LoopCloser 30 | from kiss_slam.pose_graph_optimizer import PoseGraphOptimizer 31 | from kiss_slam.voxel_map import VoxelMap 32 | 33 | 34 | def transform_points(pcd, T): 35 | R = T[:3, :3] 36 | t = T[:3, -1] 37 | return pcd @ R.T + t 38 | 39 | 40 | class KissSLAM: 41 | def __init__(self, config: KissSLAMConfig): 42 | self.config = config 43 | self.odometry = KissICP(config.kiss_icp_config()) 44 | self.closer = LoopCloser(config.loop_closer) 45 | local_map_config = self.config.local_mapper 46 | self.local_map_voxel_size = local_map_config.voxel_size 47 | self.voxel_grid = VoxelMap(self.local_map_voxel_size) 48 | self.local_map_graph = LocalMapGraph() 49 | self.local_map_splitting_distance = local_map_config.splitting_distance 50 | self.optimizer = PoseGraphOptimizer(config.pose_graph_optimizer) 51 | self.optimizer.add_variable(self.local_map_graph.last_id, self.local_map_graph.last_keypose) 52 | self.optimizer.fix_variable(self.local_map_graph.last_id) 53 | self.closures = [] 54 | 55 | def get_closures(self): 56 | return self.closures 57 | 58 | def get_keyposes(self): 59 | return list(self.local_map_graph.keyposes()) 60 | 61 | def process_scan(self, frame, timestamps): 62 | deskewed_frame, _ = self.odometry.register_frame(frame, timestamps) 63 | current_pose = self.odometry.last_pose 64 | mapping_frame = voxel_down_sample(deskewed_frame, self.local_map_voxel_size) 65 | self.voxel_grid.integrate_frame(mapping_frame, current_pose) 66 | self.local_map_graph.last_local_map.local_trajectory.append(current_pose) 67 | traveled_distance = np.linalg.norm(current_pose[:3, -1]) 68 | if traveled_distance > self.local_map_splitting_distance: 69 | self.generate_new_node() 70 | 71 | def compute_closures(self, query_id, query): 72 | is_good, source_id, target_id, pose_constraint = self.closer.compute( 73 | query_id, query, self.local_map_graph 74 | ) 75 | if is_good: 76 | self.closures.append((source_id, target_id)) 77 | self.optimizer.add_factor(source_id, target_id, pose_constraint, np.eye(6)) 78 | self.optimize_pose_graph() 79 | 80 | def optimize_pose_graph(self): 81 | self.optimizer.optimize() 82 | estimates = self.optimizer.estimates() 83 | for id_, pose in estimates.items(): 84 | self.local_map_graph[id_].keypose = np.copy(pose) 85 | 86 | def generate_new_node(self): 87 | points = self.odometry.local_map.point_cloud() 88 | # Reset odometry 89 | last_local_map = self.local_map_graph.last_local_map 90 | relative_motion = last_local_map.local_trajectory[-1] 91 | inverse_relative_motion = np.linalg.inv(relative_motion) 92 | transformed_local_map = transform_points(points, inverse_relative_motion) 93 | 94 | self.odometry.local_map.clear() 95 | self.odometry.local_map.add_points(transformed_local_map) 96 | self.odometry.last_pose = np.eye(4) 97 | 98 | query_id = last_local_map.id 99 | query_points = self.voxel_grid.point_cloud() 100 | self.local_map_graph.finalize_local_map(self.voxel_grid) 101 | self.voxel_grid.clear() 102 | self.voxel_grid.add_points(transformed_local_map) 103 | self.optimizer.add_variable(self.local_map_graph.last_id, self.local_map_graph.last_keypose) 104 | self.optimizer.add_factor( 105 | self.local_map_graph.last_id, query_id, relative_motion, np.eye(6) 106 | ) 107 | self.compute_closures(query_id, query_points) 108 | 109 | @property 110 | def poses(self): 111 | poses = [np.eye(4)] 112 | for node in self.local_map_graph.local_maps(): 113 | for rel_pose in node.local_trajectory[1:]: 114 | poses.append(node.keypose @ rel_pose) 115 | return poses 116 | 117 | def fine_grained_optimization(self): 118 | pgo = PoseGraphOptimizer(self.config.pose_graph_optimizer) 119 | id_ = 0 120 | pgo.add_variable(id_, self.local_map_graph[id_].keypose) 121 | pgo.fix_variable(id_) 122 | for node in self.local_map_graph.local_maps(): 123 | odometry_factors = [ 124 | np.linalg.inv(T0) @ T1 125 | for T0, T1 in zip(node.local_trajectory[:-1], node.local_trajectory[1:]) 126 | ] 127 | for i, factor in enumerate(odometry_factors): 128 | pgo.add_variable(id_ + 1, node.keypose @ node.local_trajectory[i + 1]) 129 | pgo.add_factor(id_ + 1, id_, factor, np.eye(6)) 130 | id_ += 1 131 | pgo.fix_variable(id_ - 1) 132 | 133 | pgo.optimize() 134 | poses = [x for x in pgo.estimates().values()] 135 | return poses, pgo 136 | -------------------------------------------------------------------------------- /kiss_slam/tools/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | -------------------------------------------------------------------------------- /kiss_slam/tools/cli.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 Optional 25 | 26 | import typer 27 | from kiss_icp.datasets import available_dataloaders, sequence_dataloaders 28 | from kiss_icp.tools.cmd import guess_dataloader 29 | 30 | 31 | def name_callback(value: str): 32 | if not value: 33 | return value 34 | dl = available_dataloaders() 35 | if value not in dl: 36 | raise typer.BadParameter(f"Supported dataloaders are:\n{', '.join(dl)}") 37 | return value 38 | 39 | 40 | app = typer.Typer(add_completion=False, rich_markup_mode="rich") 41 | 42 | # Remove from the help those dataloaders we explicitly say how to use 43 | _available_dl_help = available_dataloaders() 44 | _available_dl_help.remove("generic") 45 | _available_dl_help.remove("mcap") 46 | _available_dl_help.remove("ouster") 47 | _available_dl_help.remove("rosbag") 48 | 49 | 50 | @app.command() 51 | def kiss_slam( 52 | data: Path = typer.Argument( 53 | ..., 54 | help="The data directory used by the specified dataloader", 55 | show_default=False, 56 | ), 57 | dataloader: str = typer.Option( 58 | None, 59 | show_default=False, 60 | case_sensitive=False, 61 | autocompletion=available_dataloaders, 62 | callback=name_callback, 63 | help="[Optional] Use a specific dataloader from those supported by KISS-ICP", 64 | ), 65 | visualize: bool = typer.Option( 66 | False, 67 | "--visualize", 68 | "-v", 69 | help="[Optional] Visualize Ground Truth Loop Closures in the data sequence", 70 | rich_help_panel="Additional Options", 71 | ), 72 | refuse_scans: bool = typer.Option( 73 | False, 74 | "--refuse-scans", 75 | "-rs", 76 | help="[Optional] At the end of the SLAM run, refuse all the scans into a Global Map using the Pose Estimates", 77 | rich_help_panel="Additional Options", 78 | ), 79 | sequence: Optional[str] = typer.Option( 80 | None, 81 | "--sequence", 82 | "-s", 83 | show_default=False, 84 | help="[Optional] For some dataloaders, you need to specify a given sequence", 85 | rich_help_panel="Additional Options", 86 | ), 87 | topic: Optional[str] = typer.Option( 88 | None, 89 | "--topic", 90 | "-t", 91 | show_default=False, 92 | help="[Optional] Only valid when processing rosbag files", 93 | rich_help_panel="Additional Options", 94 | ), 95 | n_scans: int = typer.Option( 96 | -1, 97 | "--n-scans", 98 | "-n", 99 | show_default=False, 100 | help="[Optional] Specify the number of scans to process, default is the entire dataset", 101 | rich_help_panel="Additional Options", 102 | ), 103 | jump: int = typer.Option( 104 | 0, 105 | "--jump", 106 | "-j", 107 | show_default=False, 108 | help="[Optional] Specify if you want to start to process scans from a given starting point", 109 | rich_help_panel="Additional Options", 110 | ), 111 | config: Optional[Path] = typer.Option( 112 | None, 113 | "--config", 114 | exists=True, 115 | show_default=False, 116 | help="[Optional] Path to the configuration file", 117 | ), 118 | meta: Optional[Path] = typer.Option( 119 | None, 120 | "--meta", 121 | "-m", 122 | exists=True, 123 | show_default=False, 124 | help="[Optional] For Ouster pcap dataloader, specify metadata json file path explicitly", 125 | rich_help_panel="Additional Options", 126 | ), 127 | ): 128 | # Attempt to guess some common file extensions to avoid using the --dataloader flag 129 | if not dataloader: 130 | dataloader, data = guess_dataloader(data, default_dataloader="generic") 131 | 132 | # Validate some options 133 | if dataloader in sequence_dataloaders() and sequence is None: 134 | print('You must specify a sequence "--sequence"') 135 | raise typer.Exit(code=1) 136 | 137 | # Lazy-loading for faster CLI 138 | from kiss_icp.datasets import dataset_factory 139 | 140 | from kiss_slam.pipeline import SlamPipeline 141 | 142 | SlamPipeline( 143 | dataset=dataset_factory( 144 | dataloader=dataloader, 145 | data_dir=data, 146 | sequence=sequence, 147 | topic=topic, 148 | meta=meta, 149 | ), 150 | config_file=config, 151 | visualize=visualize, 152 | n_scans=n_scans, 153 | jump=jump, 154 | refuse_scans=refuse_scans, 155 | ).run().print() 156 | 157 | 158 | def run(): 159 | app() 160 | 161 | 162 | if __name__ == "__main__": 163 | run() 164 | -------------------------------------------------------------------------------- /kiss_slam/tools/visualizer.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 abc import ABC 26 | from functools import partial 27 | from typing import Callable, List 28 | 29 | import numpy as np 30 | 31 | YELLOW = np.array([1, 0.706, 0]) 32 | RED = np.array([128, 0, 0]) / 255.0 33 | BLACK = np.array([0, 0, 0]) / 255.0 34 | BLUE = np.array([0.4, 0.5, 0.9]) 35 | GREEN = np.array([0.4, 0.9, 0.5]) 36 | SPHERE_SIZE_KEYPOSES = 1.0 37 | SPHERE_SIZE_ODOMETRY = 0.2 38 | 39 | 40 | def transform_points(pcd, T): 41 | R = T[:3, :3] 42 | t = T[:3, -1] 43 | return pcd @ R.T + t 44 | 45 | 46 | class StubVisualizer(ABC): 47 | def __init__(self): 48 | pass 49 | 50 | def update(self, slam): 51 | pass 52 | 53 | 54 | class RegistrationVisualizer(StubVisualizer): 55 | # Public Interaface ---------------------------------------------------------------------------- 56 | def __init__(self): 57 | try: 58 | self.o3d = importlib.import_module("open3d") 59 | except ModuleNotFoundError as err: 60 | print(f'open3d is not installed on your system, run "pip install open3d"') 61 | exit(1) 62 | 63 | # Initialize GUI controls 64 | self.block_vis = True 65 | self.play_crun = False 66 | self.reset_bounding_box = True 67 | 68 | # Create data 69 | self.local_map = self.o3d.geometry.PointCloud() 70 | self.closures = [] 71 | self.key_poses = [] 72 | self.key_frames = [] 73 | self.global_frames = [] 74 | self.odom_frames = [] 75 | self.edges = [] 76 | self.current_node = None 77 | 78 | # Initialize visualizer 79 | self.vis = self.o3d.visualization.VisualizerWithKeyCallback() 80 | self._register_key_callbacks() 81 | self._initialize_visualizer() 82 | 83 | def update(self, slam): 84 | self._update_geometries(slam) 85 | while self.block_vis: 86 | self.vis.poll_events() 87 | self.vis.update_renderer() 88 | if self.play_crun: 89 | break 90 | self.block_vis = not self.block_vis 91 | 92 | # Private Interaface --------------------------------------------------------------------------- 93 | def _initialize_visualizer(self): 94 | w_name = self.__class__.__name__ 95 | self.vis.create_window(window_name=w_name, width=1920, height=1080) 96 | self.vis.add_geometry(self.local_map) 97 | self._set_black_background(self.vis) 98 | self.vis.get_render_option().point_size = 1 99 | self.vis.get_render_option().line_width = 10 100 | print( 101 | f"{w_name} initialized. Press:\n" 102 | "\t[SPACE] to pause/start\n" 103 | "\t [ESC] to exit\n" 104 | "\t [N] to step\n" 105 | "\t [C] to center the viewpoint\n" 106 | "\t [W] to toggle a white background\n" 107 | "\t [B] to toggle a black background\n" 108 | ) 109 | 110 | def _register_key_callback(self, keys: List, callback: Callable): 111 | for key in keys: 112 | self.vis.register_key_callback(ord(str(key)), partial(callback)) 113 | 114 | def _register_key_callbacks(self): 115 | self._register_key_callback(["Ā", "Q", "\x1b"], self._quit) 116 | self._register_key_callback([" "], self._start_stop) 117 | self._register_key_callback(["N"], self._next_frame) 118 | self._register_key_callback(["C"], self._center_viewpoint) 119 | self._register_key_callback(["B"], self._set_black_background) 120 | self._register_key_callback(["W"], self._set_white_background) 121 | 122 | def _set_black_background(self, vis): 123 | vis.get_render_option().background_color = [0.0, 0.0, 0.0] 124 | 125 | def _set_white_background(self, vis): 126 | vis.get_render_option().background_color = [1.0, 1.0, 1.0] 127 | 128 | def _quit(self, vis): 129 | print("Destroying Visualizer") 130 | vis.destroy_window() 131 | os._exit(0) 132 | 133 | def _next_frame(self, vis): 134 | self.block_vis = not self.block_vis 135 | 136 | def _start_stop(self, vis): 137 | self.play_crun = not self.play_crun 138 | 139 | def _center_viewpoint(self, vis): 140 | vis.reset_view_point(True) 141 | 142 | def _add_line(self, pose0, pose1, color): 143 | lines = [[0, 1]] 144 | colors = [color for i in range(len(lines))] 145 | line_set_closure = self.o3d.geometry.LineSet() 146 | line_set_closure.points = self.o3d.utility.Vector3dVector([pose0, pose1]) 147 | line_set_closure.lines = self.o3d.utility.Vector2iVector(lines) 148 | line_set_closure.colors = self.o3d.utility.Vector3dVector(colors) 149 | return line_set_closure 150 | 151 | def _add_frames(self, poses, size, color): 152 | frames = [] 153 | for pose in poses: 154 | new_frame = self._add_frame(pose, size, color) 155 | frames.append(new_frame) 156 | return frames 157 | 158 | def _add_frame(self, pose, size, color): 159 | new_frame = self.o3d.geometry.TriangleMesh.create_sphere(size) 160 | new_frame.paint_uniform_color(color) 161 | new_frame.compute_vertex_normals() 162 | new_frame.transform(pose) 163 | return new_frame 164 | 165 | def _update_geometries(self, slam): 166 | current_node = slam.local_map_graph.last_local_map 167 | local_map_in_global = transform_points(slam.voxel_grid.point_cloud(), current_node.keypose) 168 | self.local_map.points = self.o3d.utility.Vector3dVector(local_map_in_global) 169 | self.local_map.paint_uniform_color(YELLOW) 170 | self.vis.update_geometry(self.local_map) 171 | 172 | # Odometry in current local map 173 | current_pose = current_node.endpose 174 | 175 | odom_frame = self._add_frame(current_pose, SPHERE_SIZE_ODOMETRY, GREEN) 176 | self.odom_frames.append(odom_frame) 177 | self.vis.add_geometry( 178 | odom_frame, 179 | reset_bounding_box=False, 180 | ) 181 | 182 | # Optimized poses 183 | key_poses = slam.get_keyposes() 184 | if key_poses != self.key_poses: 185 | for frame in self.odom_frames: 186 | self.vis.remove_geometry(frame, reset_bounding_box=False) 187 | self.odom_frames = self._add_frames(slam.poses, SPHERE_SIZE_ODOMETRY, GREEN) 188 | for frame in self.odom_frames: 189 | self.vis.add_geometry( 190 | frame, 191 | reset_bounding_box=False, 192 | ) 193 | 194 | # Vertices 195 | for frame in self.key_frames: 196 | self.vis.remove_geometry(frame, reset_bounding_box=False) 197 | self.key_frames = self._add_frames(key_poses, SPHERE_SIZE_KEYPOSES, BLUE) 198 | for frame in self.key_frames: 199 | self.vis.add_geometry(frame, reset_bounding_box=False) 200 | self.key_poses = key_poses 201 | 202 | # Edges 203 | for edge in self.edges: 204 | self.vis.remove_geometry(edge, reset_bounding_box=False) 205 | 206 | self.edges = [] 207 | for frame0, frame1 in zip(self.key_frames[:-1], self.key_frames[1:]): 208 | pose0 = frame0.get_center() 209 | pose1 = frame1.get_center() 210 | self.edges.append(self._add_line(pose0, pose1, BLUE)) 211 | for closure in self.closures: 212 | idx0, idx1 = closure 213 | pose0 = self.key_frames[idx0].get_center() 214 | pose1 = self.key_frames[idx1].get_center() 215 | self.edges.append(self._add_line(pose0, pose1, RED)) 216 | 217 | for edge in self.edges: 218 | self.vis.add_geometry(edge, reset_bounding_box=False) 219 | 220 | if self.reset_bounding_box: 221 | self.vis.reset_view_point(True) 222 | self.reset_bounding_box = False 223 | -------------------------------------------------------------------------------- /kiss_slam/voxel_map.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Tiziano Guadagnino, Benedikt Mersch, Saurabh Gupta, 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 | import open3d as o3d 25 | 26 | from kiss_slam.kiss_slam_pybind import kiss_slam_pybind 27 | 28 | 29 | class VoxelMap: 30 | def __init__(self, voxel_size: float): 31 | self.map = kiss_slam_pybind._VoxelMap(voxel_size) 32 | 33 | def integrate_frame(self, points: np.ndarray, pose: np.ndarray): 34 | vector3fvector = kiss_slam_pybind._Vector3fVector(points.astype(np.float32)) 35 | self.map._integrate_frame(vector3fvector, pose) 36 | 37 | def add_points(self, points: np.ndarray): 38 | vector3fvector = kiss_slam_pybind._Vector3fVector(points.astype(np.float32)) 39 | self.map._add_points(vector3fvector) 40 | 41 | def point_cloud(self): 42 | return np.asarray(self.map._point_cloud()).astype(np.float64) 43 | 44 | def clear(self): 45 | self.map._clear() 46 | 47 | def num_voxels(self): 48 | return self.map._num_voxels() 49 | 50 | def open3d_pcd_with_normals(self): 51 | points, normals = self.map._per_voxel_point_and_normal() 52 | # Reduce memory footprint 53 | pcd = o3d.t.geometry.PointCloud() 54 | pcd.point.positions = o3d.core.Tensor(np.asarray(points), o3d.core.Dtype.Float32) 55 | pcd.point.normals = o3d.core.Tensor(np.asarray(normals), o3d.core.Dtype.Float32) 56 | return pcd 57 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit_build_core", "pybind11"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "kiss-slam" 7 | version = "0.0.2" 8 | description = "KISS-SLAM: A Simple, Robust, and Accurate 3D LiDAR SLAM System With Enhanced Generalization Capabilities" 9 | readme = "README.md" 10 | authors = [ 11 | { name = "Tiziano Guadagnino", email = "frevo93@gmail.com" }, 12 | { name = "Benedikt Mersch", email = "benedikt.mersch@gmail.com" }, 13 | { name = "Saurabh Gupta", email = "saurabh.gupta1002@gmail.com" }, 14 | ] 15 | requires-python = ">=3.8" 16 | keywords = [ 17 | "LiDAR", 18 | "SLAM", 19 | "Mapping", 20 | ] 21 | classifiers = [ 22 | "Intended Audience :: Developers", 23 | "Intended Audience :: Education", 24 | "Intended Audience :: Other Audience", 25 | "Intended Audience :: Science/Research", 26 | "License :: OSI Approved :: MIT License", 27 | "Operating System :: MacOS", 28 | "Operating System :: Microsoft :: Windows", 29 | "Operating System :: Unix", 30 | "Programming Language :: C++", 31 | "Programming Language :: Python :: 3", 32 | "Programming Language :: Python :: 3.8", 33 | "Programming Language :: Python :: 3.9", 34 | "Programming Language :: Python :: 3.10", 35 | "Programming Language :: Python :: 3.11", 36 | "Programming Language :: Python :: 3.12", 37 | ] 38 | dependencies = [ 39 | "kiss-icp>=1.2.3", 40 | "map_closures>=2.0.2", 41 | "open3d>=0.19.0", 42 | "numpy", 43 | "PyYAML", 44 | "pydantic>=2", 45 | "tqdm", 46 | "pydantic-settings", 47 | ] 48 | 49 | [project.scripts] 50 | kiss_slam_pipeline="kiss_slam.tools.cli:run" 51 | kiss_slam_dump_config="kiss_slam.config.config:write_config" 52 | 53 | [project.urls] 54 | Homepage = "https://github.com/PRBonn/kiss-slam" 55 | 56 | [tool.scikit-build] 57 | build-dir = "build/{wheel_tag}" 58 | cmake.verbose = false 59 | cmake.minimum-version = "3.22" 60 | editable.mode = "redirect" 61 | editable.rebuild = true 62 | editable.verbose = true 63 | sdist.exclude = ["kiss_slam_pybind/"] 64 | wheel.install-dir = "kiss_slam/kiss_slam_pybind/" 65 | 66 | [tool.black] 67 | line-length = 100 68 | 69 | [tool.isort] 70 | profile = "black" 71 | 72 | [tool.pylint.format] 73 | max-line-length = "100" 74 | 75 | [tool.cibuildwheel] 76 | archs = ["auto64"] 77 | skip = ["*-musllinux*", "pp*", "cp36-*"] 78 | 79 | [tool.cibuildwheel.config-settings] 80 | "cmake.define.USE_SYSTEM_EIGEN3" = "OFF" 81 | "cmake.define.USE_SYSTEM_G2O" = "OFF" 82 | "cmake.define.USE_SYSTEM_TSL-ROBIN-MAP" = "OFF" 83 | 84 | [tool.cibuildwheel.macos] 85 | environment = "MACOSX_DEPLOYMENT_TARGET=11.0" 86 | archs = ["auto64", "arm64"] 87 | --------------------------------------------------------------------------------