├── .clang-format ├── .cmake-format.yaml ├── .github └── workflows │ ├── cpp.yml │ ├── pre-commit.yml │ └── python.yml ├── .gitignore ├── .gitlab-ci.yml ├── .pre-commit-config.yaml ├── CITATION.cff ├── LICENSE ├── Makefile ├── README.md ├── config ├── apollo.yaml ├── example.yaml ├── helimos │ ├── all_training.yaml │ ├── inference.yaml │ ├── omni_training.yaml │ └── solid_training.yaml ├── kitti-tracking.yaml ├── kitti.yaml └── nuscenes.yaml ├── pyproject.toml ├── scripts ├── cache_to_ply.py ├── precache.py └── train.py └── src └── mapmos ├── __init__.py ├── cli.py ├── config ├── __init__.py ├── config.py └── parser.py ├── datasets ├── __init__.py ├── apollo.py ├── generic.py ├── helimos.py ├── kitti.py ├── kitti_tracking.py ├── mapmos_dataset.py ├── mcap.py ├── nuscenes.py ├── ouster.py └── rosbag.py ├── mapmos_net.py ├── mapping.py ├── metrics.py ├── minkunet.py ├── odometry.py ├── paper_pipeline.py ├── pipeline.py ├── pybind ├── 3rdparty │ ├── eigen │ │ ├── LICENSE │ │ └── eigen.cmake │ ├── find_dependencies.cmake │ ├── sophus │ │ ├── LICENSE │ │ └── sophus.cmake │ ├── tbb │ │ ├── LICENSE │ │ └── tbb.cmake │ └── tsl_robin │ │ ├── LICENSE │ │ └── tsl_robin.cmake ├── CMakeLists.txt ├── Deskew.cpp ├── Deskew.hpp ├── Registration.cpp ├── Registration.hpp ├── VoxelHashMap.cpp ├── VoxelHashMap.hpp ├── mapmos_pybind.cpp └── stl_vector_eigen.h ├── registration.py ├── training_module.py └── utils ├── __init__.py ├── augmentation.py ├── cache.py ├── pipeline_results.py ├── save.py ├── seed.py └── visualizer.py /.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 | -------------------------------------------------------------------------------- /.github/workflows/cpp.yml: -------------------------------------------------------------------------------- 1 | name: C++ Build 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | 12 | jobs: 13 | cpp_api: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-22.04, ubuntu-24.04] 18 | steps: 19 | - uses: actions/checkout@v3 20 | - name: Setup cmake 21 | uses: jwlawson/actions-setup-cmake@v1.13 22 | with: 23 | cmake-version: "3.25.x" 24 | - name: Install pybind 25 | run: python3 -m pip install pybind11[global] 26 | - name: Configure CMake 27 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/src/mapmos/pybind 28 | - name: Build 29 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 30 | 31 | # As the previous job will always install the dependencies from cmake, and this is guaranteed to 32 | # work, we also want to support dev sandboxes where the main dependencies are already 33 | # pre-installed in the system. For now, we only support dev machines under a GNU/Linux 34 | # environmnets. If you are reading this and need the same functionallity in Windows/macOS please 35 | # open a ticket. 36 | cpp_api_dev: 37 | runs-on: ${{ matrix.os }} 38 | strategy: 39 | matrix: 40 | os: [ubuntu-22.04, ubuntu-24.04] 41 | 42 | steps: 43 | - uses: actions/checkout@v3 44 | - name: Cache dependencies 45 | uses: actions/cache@v4 46 | with: 47 | path: ~/.apt/cache 48 | key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} 49 | restore-keys: | 50 | ${{ runner.os }}-apt- 51 | - name: Install dependencies 52 | run: | 53 | sudo apt-get update 54 | sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev 55 | - name: Install pybind 56 | run: python3 -m pip install pybind11[global] 57 | - name: Configure CMake 58 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/src/mapmos/pybind 59 | - name: Build 60 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 61 | -------------------------------------------------------------------------------- /.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/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-22.04, ubuntu-24.04] 14 | 15 | steps: 16 | - uses: actions/checkout@v3 17 | - name: Setup Python3 18 | uses: actions/setup-python@v3 19 | - name: Install dependencies 20 | run: python -m pip install --upgrade pip 21 | - name: Build pip package 22 | run: python -m pip install --verbose . 23 | - name: Test installation 24 | run: mapmos_pipeline --help 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | install/ 2 | log/ 3 | results/ 4 | _skbuild/ 5 | .gitlab-ci-local/ 6 | .polyscope.ini 7 | imgui.ini 8 | 9 | # Created by https://www.toptal.com/developers/gitignore/api/python,c++ 10 | # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ 11 | ### C++ ### 12 | # Prerequisites 13 | *.d 14 | 15 | # Compiled Object files 16 | *.slo 17 | *.lo 18 | *.o 19 | *.obj 20 | 21 | # Precompiled Headers 22 | *.gch 23 | *.pch 24 | 25 | # Compiled Dynamic libraries 26 | *.so 27 | *.dylib 28 | *.dll 29 | 30 | # Fortran module files 31 | *.mod 32 | *.smod 33 | 34 | # Compiled Static libraries 35 | *.lai 36 | *.la 37 | *.a 38 | *.lib 39 | 40 | # Executables 41 | *.exe 42 | *.out 43 | *.app 44 | 45 | ### Python ### 46 | # Byte-compiled / optimized / DLL files 47 | __pycache__/ 48 | *.py[cod] 49 | *$py.class 50 | 51 | # C extensions 52 | 53 | # Distribution / packaging 54 | .Python 55 | build/ 56 | develop-eggs/ 57 | dist/ 58 | downloads/ 59 | eggs/ 60 | .eggs/ 61 | lib/ 62 | lib64/ 63 | parts/ 64 | sdist/ 65 | var/ 66 | wheels/ 67 | share/python-wheels/ 68 | *.egg-info/ 69 | .installed.cfg 70 | *.egg 71 | MANIFEST 72 | 73 | # PyInstaller 74 | # Usually these files are written by a python script from a template 75 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 76 | *.manifest 77 | *.spec 78 | 79 | # Installer logs 80 | pip-log.txt 81 | pip-delete-this-directory.txt 82 | 83 | # Unit test / coverage reports 84 | htmlcov/ 85 | .tox/ 86 | .nox/ 87 | .coverage 88 | .coverage.* 89 | .cache 90 | nosetests.xml 91 | coverage.xml 92 | *.cover 93 | *.py,cover 94 | .hypothesis/ 95 | .pytest_cache/ 96 | cover/ 97 | 98 | # Translations 99 | *.mo 100 | *.pot 101 | 102 | # Django stuff: 103 | *.log 104 | local_settings.py 105 | db.sqlite3 106 | db.sqlite3-journal 107 | 108 | # Flask stuff: 109 | instance/ 110 | .webassets-cache 111 | 112 | # Scrapy stuff: 113 | .scrapy 114 | 115 | # Sphinx documentation 116 | docs/_build/ 117 | 118 | # PyBuilder 119 | .pybuilder/ 120 | target/ 121 | 122 | # Jupyter Notebook 123 | .ipynb_checkpoints 124 | 125 | # IPython 126 | profile_default/ 127 | ipython_config.py 128 | 129 | # pyenv 130 | # For a library or package, you might want to ignore these files since the code is 131 | # intended to run in multiple environments; otherwise, check them in: 132 | # .python-version 133 | 134 | # pipenv 135 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 136 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 137 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 138 | # install all needed dependencies. 139 | #Pipfile.lock 140 | 141 | # poetry 142 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 143 | # This is especially recommended for binary packages to ensure reproducibility, and is more 144 | # commonly ignored for libraries. 145 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 146 | #poetry.lock 147 | 148 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 149 | __pypackages__/ 150 | 151 | # Celery stuff 152 | celerybeat-schedule 153 | celerybeat.pid 154 | 155 | # SageMath parsed files 156 | *.sage.py 157 | 158 | # Environments 159 | .env 160 | .venv 161 | env/ 162 | venv/ 163 | ENV/ 164 | env.bak/ 165 | venv.bak/ 166 | 167 | # Spyder project settings 168 | .spyderproject 169 | .spyproject 170 | 171 | # Rope project settings 172 | .ropeproject 173 | 174 | # mkdocs documentation 175 | /site 176 | 177 | # mypy 178 | .mypy_cache/ 179 | .dmypy.json 180 | dmypy.json 181 | 182 | # Pyre type checker 183 | .pyre/ 184 | 185 | # pytype static type analyzer 186 | .pytype/ 187 | 188 | # Cython debug symbols 189 | cython_debug/ 190 | 191 | # PyCharm 192 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 193 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 194 | # and can be added to the global gitignore or merged into this file. For a more nuclear 195 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 196 | #.idea/ 197 | 198 | # End of https://www.toptal.com/developers/gitignore/api/python,c++ 199 | *.orig 200 | # Created by https://www.toptal.com/developers/gitignore/api/python 201 | # Edit at https://www.toptal.com/developers/gitignore?templates=python 202 | 203 | ### Python ### 204 | # Byte-compiled / optimized / DLL files 205 | __pycache__/ 206 | *.py[cod] 207 | *$py.class 208 | 209 | # C extensions 210 | *.so 211 | 212 | # Distribution / packaging 213 | .Python 214 | build/ 215 | develop-eggs/ 216 | dist/ 217 | downloads/ 218 | eggs/ 219 | .eggs/ 220 | lib/ 221 | lib64/ 222 | parts/ 223 | sdist/ 224 | var/ 225 | wheels/ 226 | share/python-wheels/ 227 | *.egg-info/ 228 | .installed.cfg 229 | *.egg 230 | MANIFEST 231 | 232 | # PyInstaller 233 | # Usually these files are written by a python script from a template 234 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 235 | *.manifest 236 | *.spec 237 | 238 | # Installer logs 239 | pip-log.txt 240 | pip-delete-this-directory.txt 241 | 242 | # Unit test / coverage reports 243 | htmlcov/ 244 | .tox/ 245 | .nox/ 246 | .coverage 247 | .coverage.* 248 | .cache 249 | nosetests.xml 250 | coverage.xml 251 | *.cover 252 | *.py,cover 253 | .hypothesis/ 254 | .pytest_cache/ 255 | cover/ 256 | 257 | # Translations 258 | *.mo 259 | *.pot 260 | 261 | # Django stuff: 262 | *.log 263 | local_settings.py 264 | db.sqlite3 265 | db.sqlite3-journal 266 | 267 | # Flask stuff: 268 | instance/ 269 | .webassets-cache 270 | 271 | # Scrapy stuff: 272 | .scrapy 273 | 274 | # Sphinx documentation 275 | docs/_build/ 276 | 277 | # PyBuilder 278 | .pybuilder/ 279 | target/ 280 | 281 | # Jupyter Notebook 282 | .ipynb_checkpoints 283 | 284 | # IPython 285 | profile_default/ 286 | ipython_config.py 287 | 288 | # pyenv 289 | # For a library or package, you might want to ignore these files since the code is 290 | # intended to run in multiple environments; otherwise, check them in: 291 | # .python-version 292 | 293 | # pipenv 294 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 295 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 296 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 297 | # install all needed dependencies. 298 | #Pipfile.lock 299 | 300 | # poetry 301 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 302 | # This is especially recommended for binary packages to ensure reproducibility, and is more 303 | # commonly ignored for libraries. 304 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 305 | #poetry.lock 306 | 307 | # pdm 308 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 309 | #pdm.lock 310 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 311 | # in version control. 312 | # https://pdm.fming.dev/#use-with-ide 313 | .pdm.toml 314 | 315 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 316 | __pypackages__/ 317 | 318 | # Celery stuff 319 | celerybeat-schedule 320 | celerybeat.pid 321 | 322 | # SageMath parsed files 323 | *.sage.py 324 | 325 | # Environments 326 | .env 327 | .venv 328 | env/ 329 | venv/ 330 | ENV/ 331 | env.bak/ 332 | venv.bak/ 333 | 334 | # Spyder project settings 335 | .spyderproject 336 | .spyproject 337 | 338 | # Rope project settings 339 | .ropeproject 340 | 341 | # mkdocs documentation 342 | /site 343 | 344 | # mypy 345 | .mypy_cache/ 346 | .dmypy.json 347 | dmypy.json 348 | 349 | # Pyre type checker 350 | .pyre/ 351 | 352 | # pytype static type analyzer 353 | .pytype/ 354 | 355 | # Cython debug symbols 356 | cython_debug/ 357 | 358 | # PyCharm 359 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 360 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 361 | # and can be added to the global gitignore or merged into this file. For a more nuclear 362 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 363 | #.idea/ 364 | 365 | # End of https://www.toptal.com/developers/gitignore/api/python 366 | nn 367 | 368 | # VSCode 369 | .vscode 370 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - format 3 | - build 4 | cache: 5 | paths: 6 | - .cache/pip 7 | 8 | #----- format stage -------------------------------------------------------------------------------- 9 | black: 10 | image: python:3.8 11 | stage: format 12 | before_script: 13 | - pip install black 14 | script: 15 | - black --line-length 100 --check $CI_PROJECT_DIR 16 | 17 | clang-format: 18 | image: ubuntu:22.04 19 | stage: format 20 | before_script: 21 | - apt-get update && apt-get install --no-install-recommends -y clang-format 22 | script: 23 | - clang-format -Werror --dry-run $(find . -regextype posix-extended -regex ".*\.(cpp|hpp|h)") 24 | 25 | #----- build stage --------------------------------------------------------------------------------- 26 | pip_package: 27 | image: python:3.8 28 | stage: build 29 | script: 30 | - VERBOSE=1 pip install --verbose . 31 | - mapmos_pipeline --help 32 | -------------------------------------------------------------------------------- /.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 | - repo: https://github.com/ahans/cmake-format-precommit 20 | rev: 8e52fb6506f169dddfaa87f88600d765fca48386 21 | hooks: 22 | - id: cmake-format 23 | - repo: https://github.com/pycqa/isort 24 | rev: 5.12.0 25 | hooks: 26 | - id: isort 27 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | preferred-citation: 3 | title: "Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation" 4 | doi: "10.1109/LRA.2023.3292583" 5 | year: "2023" 6 | type: article 7 | journal: "IEEE Robotics and Automation Letters (RA-L)" 8 | url: https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/mersch2023ral.pdf 9 | codeurl: https://github.com/PRBonn/MapMOS 10 | authors: 11 | - family-names: Mersch 12 | given-names: Benedikt 13 | - family-names: Guadagnino 14 | given-names: Tiziano 15 | - family-names: Chen 16 | given-names: Xieyuanli 17 | - family-names: Vizzo 18 | given-names: Ignacio 19 | - family-names: Behley 20 | given-names: Jens 21 | - family-names: Stachniss 22 | given-names: Cyrill 23 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | install: 2 | @pip install -v . 3 | 4 | install-all: 5 | @pip install -v ".[all]" 6 | 7 | uninstall: 8 | @pip -v uninstall mapmos 9 | 10 | editable: 11 | @pip install scikit-build-core pyproject_metadata pathspec pybind11 ninja cmake 12 | @pip install --no-build-isolation -ve . 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation

3 | 4 | 5 | 6 | 7 | 8 |

9 | 10 |

11 | 12 |

13 | Our approach identifies moving objects in the current scan (blue points) and the local map (black points) of the environment and maintains a volumetric belief map representing the dynamic environment. 14 |

15 | 16 |
17 | Click here for qualitative results! 18 | 19 | [![MapMOS](https://github.com/PRBonn/MapMOS/assets/38326482/a4238431-bd2d-4b2c-991b-7ff5e9378a8e)](https://github.com/PRBonn/MapMOS/assets/38326482/04c7e5a2-dd44-431a-95b0-c42d5605078a) 20 | 21 | Our predictions for the KITTI Tracking sequence 19 with true positives (green), false positives (red), and false negatives (blue). 22 | 23 |
24 | 25 | 26 |
27 | 28 | 29 | ## Installation 30 | First, make sure the [MinkowskiEngine](https://github.com/NVIDIA/MinkowskiEngine) is installed on your system, see [here](https://github.com/NVIDIA/MinkowskiEngine#installation) for more details. 31 | 32 | Next, clone our repository 33 | ```bash 34 | git clone git@github.com:PRBonn/MapMOS && cd MapMOS 35 | ``` 36 | 37 | and install with 38 | ```bash 39 | make install 40 | ``` 41 | 42 | **or** 43 | ```bash 44 | make install-all 45 | ``` 46 | if you want to install the project with all optional dependencies (needed for the visualizer). In case you want to edit the Python code, install in editable mode: 47 | ```bash 48 | make editable 49 | ``` 50 | 51 | ## How to Use It 52 | Just type 53 | 54 | ```bash 55 | mapmos_pipeline --help 56 | ``` 57 | to see how to run MapMOS. 58 |
59 | This is what you should see 60 | 61 | ![Screenshot from 2023-08-03 13-07-14](https://github.com/PRBonn/MapMOS/assets/38326482/c769afa6-709d-4648-b42d-11092d5b92ac) 62 | 63 |
64 | 65 | Check the [Download](#downloads) section for a pre-trained model. Like [KISS-ICP](https://github.com/PRBonn/kiss-icp), our pipeline runs on a variety of point cloud data formats like `bin`, `pcd`, `ply`, `xyz`, `rosbags`, and more. To visualize these, just type 66 | 67 | ```bash 68 | mapmos_pipeline --visualize /path/to/weights.ckpt /path/to/data 69 | ``` 70 | 71 |
72 | Want to evaluate with ground truth labels? 73 | 74 | Because these labels come in all shapes, you need to specify a dataloader. This is currently available for SemanticKITTI, NuScenes, HeLiMOS, and our labeled KITTI Tracking sequence 19 and Apollo sequences (see [Downloads](#downloads)). 75 | 76 |
77 | 78 |
79 | Want to reproduce the results from the paper? 80 | For reproducing the results of the paper, you need to pass the corresponding config file. They will make sure that the de-skewing option and the maximum range are set properly. To compare different map fusion strategies from our paper, just pass the `--paper` flag to the `mapmos_pipeline`. 81 | 82 |
83 | 84 | 85 | ## Training 86 | To train our approach, you need to first cache your data. To see how to do that, just `cd` into the `MapMOS` repository and type 87 | 88 | ```bash 89 | python3 scripts/precache.py --help 90 | ``` 91 | 92 | After this, you can run the training script. Again, `--help` shows you how: 93 | ```bash 94 | python3 scripts/train.py --help 95 | ``` 96 | 97 |
98 | Want to verify the cached data? 99 | 100 | You can inspect the cached training samples by using the script `python3 scripts/cache_to_ply.py --help`. 101 | 102 |
103 | 104 |
105 | Want to change the logging directory? 106 | 107 | The training log and checkpoints will be saved by default to the current working directory. To change that, export the `export LOGS=/your/path/to/logs` environment variable before running the training script. 108 | 109 |
110 | 111 | ## HeLiMOS 112 | We provide additional training and evaluation data for different sensor types in our [HeLiMOS paper](https://www.ipb.uni-bonn.de/pdfs/lim2024iros.pdf). To train on the HeLiMOS data, use the following commands: 113 | 114 | ```shell 115 | python3 scripts/precache.py /path/to/HeLiMOS helimos /path/to/cache --config config/helimos/*_training.yaml 116 | python3 scripts/train.py /path/to/HeLiMOS helimos /path/to/cache --config config/helimos/*_training.yaml 117 | ``` 118 | 119 | by replacing the paths and the config file names. To evaluate for example on the Velodyne test data, run 120 | 121 | ```shell 122 | mapmos_pipeline /path/to/weights.ckpt /path/to/HeLiMOS --dataloader helimos -s Velodyne/test.txt --config config/helimos/inference.yaml 123 | ``` 124 | 125 | Note that our sequence `-s` encodes both the sensor type `Velodyne` and split `test.txt`, just replace these with `Ouster`, `Aeva`, or `Avia` and/or `train.txt` or `val.txt` to run MapMOS on different sensors and/or splits. 126 | 127 | ## Downloads 128 | You can download the post-processed and labeled [Apollo dataset](https://www.ipb.uni-bonn.de/html/projects/apollo_dataset/LiDAR-MOS.zip) and [KITTI Tracking sequence 19](https://www.ipb.uni-bonn.de/html/projects/kitti-tracking/post-processed/kitti-tracking.zip) from our website. 129 | 130 | The [weights](https://www.ipb.uni-bonn.de/html/projects/MapMOS/mapmos.ckpt) of our pre-trained model can be downloaded as well. 131 | 132 | ## Publication 133 | If you use our code in your academic work, please cite the corresponding [paper](https://www.ipb.uni-bonn.de/pdfs/mersch2023ral.pdf): 134 | 135 | ```bibtex 136 | @article{mersch2023ral, 137 | author = {B. Mersch and T. Guadagnino and X. Chen and I. Vizzo and J. Behley and C. Stachniss}, 138 | title = {{Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation}}, 139 | journal = {IEEE Robotics and Automation Letters (RA-L)}, 140 | volume = {8}, 141 | number = {8}, 142 | pages = {5180--5187}, 143 | year = {2023}, 144 | issn = {2377-3766}, 145 | doi = {10.1109/LRA.2023.3292583}, 146 | codeurl = {https://github.com/PRBonn/MapMOS}, 147 | } 148 | ``` 149 | 150 | ## Acknowledgments 151 | This implementation is heavily inspired by [KISS-ICP](https://github.com/PRBonn/kiss-icp). 152 | 153 | ## License 154 | This project is free software made available under the MIT License. For details see the LICENSE file. 155 | -------------------------------------------------------------------------------- /config/apollo.yaml: -------------------------------------------------------------------------------- 1 | data: 2 | deskew: False 3 | 4 | mos: 5 | max_range_mos: -1.0 # No limit 6 | -------------------------------------------------------------------------------- /config/example.yaml: -------------------------------------------------------------------------------- 1 | # This config shows all parameters that can be passed to the pipeline. If not, the defaults are set in the config.py. 2 | 3 | data: 4 | deskew: False 5 | max_range: 100.0 6 | min_range: 3.0 7 | 8 | odometry: 9 | voxel_size: 0.5 10 | max_points_per_voxel: 20 11 | initial_threshold: 2.0 12 | min_motion_th: 0.1 13 | 14 | mos: 15 | voxel_size_mos: 0.1 16 | delay_mos: 10 17 | max_range_mos: 50.0 18 | min_range_mos: 0.0 19 | voxel_size_belief: 0.25 20 | max_range_belief: 150 21 | 22 | training: 23 | id: "experiment_id" 24 | train: 25 | - "00" 26 | - "01" 27 | - "02" 28 | - "03" 29 | - "04" 30 | - "05" 31 | - "06" 32 | - "07" 33 | - "09" 34 | - "10" 35 | val: 36 | - "08" 37 | batch_size: 16 38 | accumulate_grad_batches: 1 39 | max_epochs: 100 40 | lr: 0.0001 41 | lr_epoch: 1 42 | lr_decay: 0.99 43 | weight_decay: 0.0001 44 | num_workers: 4 45 | -------------------------------------------------------------------------------- /config/helimos/all_training.yaml: -------------------------------------------------------------------------------- 1 | training: 2 | id: "helimos_all" 3 | train: 4 | - "Avia/train.txt" 5 | - "Aeva/train.txt" 6 | - "Velodyne/train.txt" 7 | - "Ouster/train.txt" 8 | val: 9 | - "Avia/val.txt" 10 | - "Aeva/val.txt" 11 | - "Velodyne/val.txt" 12 | - "Ouster/val.txt" 13 | -------------------------------------------------------------------------------- /config/helimos/inference.yaml: -------------------------------------------------------------------------------- 1 | mos: 2 | max_range_mos: -1.0 3 | max_range_belief: 50 4 | -------------------------------------------------------------------------------- /config/helimos/omni_training.yaml: -------------------------------------------------------------------------------- 1 | training: 2 | id: "helimos_omni" 3 | train: 4 | - "Velodyne/train.txt" 5 | - "Ouster/train.txt" 6 | val: 7 | - "Velodyne/val.txt" 8 | - "Ouster/val.txt" 9 | -------------------------------------------------------------------------------- /config/helimos/solid_training.yaml: -------------------------------------------------------------------------------- 1 | training: 2 | id: "helimos_solid" 3 | train: 4 | - "Avia/train.txt" 5 | - "Aeva/train.txt" 6 | val: 7 | - "Avia/val.txt" 8 | - "Aeva/val.txt" 9 | -------------------------------------------------------------------------------- /config/kitti-tracking.yaml: -------------------------------------------------------------------------------- 1 | data: 2 | deskew: True 3 | 4 | mos: 5 | max_range_mos: -1.0 # No limit 6 | -------------------------------------------------------------------------------- /config/kitti.yaml: -------------------------------------------------------------------------------- 1 | data: 2 | deskew: False 3 | 4 | mos: 5 | max_range_mos: -1.0 # No limit 6 | -------------------------------------------------------------------------------- /config/nuscenes.yaml: -------------------------------------------------------------------------------- 1 | data: 2 | max_range: 50.0 3 | 4 | mos: 5 | max_range_mos: -1.0 # No limit 6 | min_range_mos: 3.0 7 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "mapmos" 3 | version = "1.0.0" 4 | description = "Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation" 5 | readme = "README.md" 6 | authors = [ 7 | { name = "Benedikt Mersch", email = "benedikt.mersch@gmail.com" }, 8 | ] 9 | license = { file = "LICENSE" } 10 | dependencies = [ 11 | "kiss-icp>=1.2.0", 12 | "diskcache>=5.3.0", 13 | "pytorch_lightning>=1.6.4", 14 | ] 15 | 16 | [project.optional-dependencies] 17 | all = [ 18 | "PyYAML", 19 | "ouster-sdk>=0.7.1", 20 | "pyntcloud", 21 | "trimesh", 22 | "open3d>=0.13", 23 | ] 24 | 25 | [project.scripts] 26 | mapmos_pipeline = "mapmos.cli:app" 27 | 28 | [project.urls] 29 | Homepage = "https://github.com/PRBonn/MapMOS" 30 | 31 | [build-system] 32 | requires = [ 33 | "scikit_build_core","pybind11", 34 | ] 35 | build-backend = "scikit_build_core.build" 36 | 37 | [tool.scikit-build] 38 | build-dir = "build" 39 | cmake.verbose = false 40 | cmake.minimum-version = "3.16" 41 | cmake.source-dir = "src/mapmos/pybind/" 42 | editable.mode = "redirect" 43 | editable.rebuild = true 44 | editable.verbose = true 45 | sdist.exclude = ["pybind/"] 46 | wheel.install-dir = "mapmos/pybind/" 47 | 48 | [tool.black] 49 | line-length = 100 50 | 51 | [tool.isort] 52 | profile = "black" 53 | 54 | [tool.pylint.format] 55 | max-line-length = "100" 56 | -------------------------------------------------------------------------------- /scripts/cache_to_ply.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # MIT License 3 | # 4 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, 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 | 24 | import importlib 25 | import os 26 | from pathlib import Path 27 | from typing import List, Optional 28 | 29 | import numpy as np 30 | import torch 31 | import typer 32 | from torch.utils.data import DataLoader 33 | from tqdm import tqdm 34 | 35 | from mapmos.config import load_config 36 | from mapmos.datasets.mapmos_dataset import MapMOSDataset, collate_fn 37 | 38 | 39 | def cache_to_ply( 40 | data: Path = typer.Argument( 41 | ..., 42 | help="The data directory used by the specified dataloader", 43 | show_default=False, 44 | ), 45 | dataloader: str = typer.Argument( 46 | ..., 47 | help="The dataloader to be used", 48 | show_default=False, 49 | ), 50 | cache_dir: Path = typer.Argument( 51 | ..., 52 | help="The directory where the cache should be created", 53 | show_default=False, 54 | ), 55 | sequence: Optional[List[str]] = typer.Option( 56 | None, 57 | "--sequence", 58 | "-s", 59 | show_default=False, 60 | help="[Optional] For some dataloaders, you need to specify a given sequence", 61 | rich_help_panel="Additional Options", 62 | ), 63 | config: Optional[Path] = typer.Option( 64 | None, 65 | "--config", 66 | exists=True, 67 | show_default=False, 68 | help="[Optional] Path to the configuration file", 69 | ), 70 | ): 71 | try: 72 | o3d = importlib.import_module("open3d") 73 | except ModuleNotFoundError as err: 74 | print(f'open3d is not installed on your system, run "pip install open3d"') 75 | exit(1) 76 | 77 | # Run 78 | cfg = load_config(config) 79 | sequences = sequence if sequence != None else cfg.training.train + cfg.training.val 80 | 81 | data_iterable = DataLoader( 82 | MapMOSDataset( 83 | dataloader=dataloader, 84 | data_dir=data, 85 | config=cfg, 86 | sequences=sequences, 87 | cache_dir=cache_dir, 88 | ), 89 | batch_size=1, 90 | collate_fn=collate_fn, 91 | shuffle=False, 92 | num_workers=0, 93 | batch_sampler=None, 94 | ) 95 | 96 | dataset_sequence = ( 97 | data_iterable.dataset.datasets[sequence].sequence_id 98 | if hasattr(data_iterable.dataset.datasets[sequence], "sequence_id") 99 | else os.path.basename(data_iterable.dataset.datasets[seq].data_dir) 100 | ) 101 | path = os.path.join("ply", dataset_sequence) 102 | os.makedirs(path, exist_ok=True) 103 | 104 | for idx, batch in enumerate( 105 | tqdm(data_iterable, desc="Writing data to ply", unit=" items", dynamic_ncols=True) 106 | ): 107 | if len(batch) > 0: 108 | mask_scan = batch[:, 4] == idx 109 | scan_points = batch[mask_scan, 1:4] 110 | scan_labels = batch[mask_scan, 6] 111 | 112 | map_points = batch[~mask_scan, 1:4] 113 | map_timestamps = batch[~mask_scan, 5] 114 | map_labels = batch[~mask_scan, 6] 115 | 116 | min_time = torch.min(batch[:, 5]) 117 | max_time = torch.max(batch[:, 5]) 118 | 119 | pcd_scan = o3d.geometry.PointCloud( 120 | o3d.utility.Vector3dVector(scan_points.numpy()) 121 | ).paint_uniform_color([0, 0, 1]) 122 | scan_colors = np.array(pcd_scan.colors) 123 | scan_colors[scan_labels == 1] = [1, 0, 0] 124 | pcd_scan.colors = o3d.utility.Vector3dVector(scan_colors) 125 | 126 | pcd_map = o3d.geometry.PointCloud( 127 | o3d.utility.Vector3dVector(map_points.numpy()) 128 | ).paint_uniform_color([0, 0, 0]) 129 | map_colors = np.array(pcd_map.colors) 130 | map_timestamps_norm = (map_timestamps - min_time) / (max_time - min_time) 131 | for i in range(len(map_colors)): 132 | t = map_timestamps_norm[i] 133 | map_colors[i, :] = [t, t, t] 134 | map_colors[map_labels == 1] = [1, 0, 0] 135 | pcd_map.colors = o3d.utility.Vector3dVector(map_colors) 136 | 137 | o3d.io.write_point_cloud(os.path.join(path, f"{idx:06}.ply"), pcd_scan + pcd_map) 138 | 139 | 140 | if __name__ == "__main__": 141 | typer.run(cache_to_ply) 142 | -------------------------------------------------------------------------------- /scripts/precache.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # MIT License 3 | # 4 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, 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 | 24 | from pathlib import Path 25 | from typing import List, Optional 26 | 27 | import typer 28 | from tqdm import tqdm 29 | 30 | from mapmos.config import load_config 31 | 32 | 33 | def precache( 34 | data: Path = typer.Argument( 35 | ..., 36 | help="The data directory used by the specified dataloader", 37 | show_default=False, 38 | ), 39 | dataloader: str = typer.Argument( 40 | ..., 41 | help="The dataloader to be used", 42 | show_default=False, 43 | ), 44 | cache_dir: Path = typer.Argument( 45 | ..., 46 | help="The directory where the cache should be created", 47 | show_default=False, 48 | ), 49 | sequence: Optional[List[str]] = typer.Option( 50 | None, 51 | "--sequence", 52 | "-s", 53 | show_default=False, 54 | help="[Optional] Cache specific sequences", 55 | rich_help_panel="Additional Options", 56 | ), 57 | config: Optional[Path] = typer.Option( 58 | None, 59 | "--config", 60 | exists=True, 61 | show_default=False, 62 | help="[Optional] Path to the configuration file", 63 | ), 64 | ): 65 | from torch.utils.data import DataLoader 66 | 67 | from mapmos.datasets.mapmos_dataset import MapMOSDataset, collate_fn 68 | 69 | cfg = load_config(config) 70 | sequences = sequence if sequence != None else cfg.training.train + cfg.training.val 71 | 72 | data_iterable = DataLoader( 73 | MapMOSDataset( 74 | dataloader=dataloader, 75 | data_dir=data, 76 | config=cfg, 77 | sequences=sequences, 78 | cache_dir=cache_dir, 79 | ), 80 | batch_size=1, 81 | collate_fn=collate_fn, 82 | shuffle=False, 83 | num_workers=0, 84 | batch_sampler=None, 85 | ) 86 | 87 | for _ in tqdm(data_iterable, desc="Caching data", unit=" items", dynamic_ncols=True): 88 | pass 89 | 90 | 91 | if __name__ == "__main__": 92 | import torch 93 | 94 | with torch.no_grad(): 95 | typer.run(precache) 96 | -------------------------------------------------------------------------------- /scripts/train.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # MIT License 3 | # 4 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, 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 | 24 | import os 25 | from pathlib import Path 26 | from typing import Optional 27 | 28 | import torch 29 | import typer 30 | from pytorch_lightning import Trainer 31 | from pytorch_lightning import loggers as pl_loggers 32 | from pytorch_lightning.callbacks import LearningRateMonitor, ModelCheckpoint 33 | 34 | from mapmos.config import load_config 35 | from mapmos.datasets.mapmos_dataset import MapMOSDataModule 36 | from mapmos.training_module import TrainingModule 37 | from mapmos.utils.seed import set_seed 38 | 39 | 40 | def train( 41 | data: Path = typer.Argument( 42 | ..., 43 | help="The data directory used by the specified dataloader", 44 | show_default=False, 45 | ), 46 | dataloader: str = typer.Argument( 47 | ..., 48 | help="The dataloader to be used", 49 | show_default=False, 50 | ), 51 | cache_dir: Path = typer.Argument( 52 | ..., 53 | help="The directory where the cache should be created", 54 | show_default=False, 55 | ), 56 | config: Optional[Path] = typer.Option( 57 | None, 58 | "--config", 59 | exists=True, 60 | show_default=False, 61 | help="[Optional] Path to the configuration file", 62 | ), 63 | ): 64 | set_seed(66) 65 | 66 | cfg = load_config(config) 67 | model = TrainingModule(cfg) 68 | 69 | # Load data and model 70 | dataset = MapMOSDataModule( 71 | dataloader=dataloader, 72 | data_dir=data, 73 | config=cfg, 74 | cache_dir=cache_dir, 75 | ) 76 | 77 | # Add callbacks 78 | lr_monitor = LearningRateMonitor(logging_interval="step") 79 | checkpoint_saver = ModelCheckpoint( 80 | monitor="val_moving_iou_scan", 81 | filename=cfg.training.id + "_{epoch:03d}_{val_moving_iou_scan:.3f}", 82 | mode="max", 83 | save_last=True, 84 | ) 85 | 86 | # Logger 87 | environ = os.environ.get("LOGS") 88 | prefix = environ if environ is not None else "models" 89 | log_dir = os.path.join(prefix, "MapMOS") 90 | os.makedirs(log_dir, exist_ok=True) 91 | tb_logger = pl_loggers.TensorBoardLogger( 92 | log_dir, 93 | name=cfg.training.id, 94 | default_hp_metric=False, 95 | ) 96 | 97 | torch.set_float32_matmul_precision("high") 98 | # Setup trainer 99 | trainer = Trainer( 100 | accelerator="gpu", 101 | devices=1, 102 | logger=tb_logger, 103 | max_epochs=cfg.training.max_epochs, 104 | callbacks=[ 105 | lr_monitor, 106 | checkpoint_saver, 107 | ], 108 | ) 109 | 110 | # Train! 111 | trainer.fit(model, dataset) 112 | 113 | 114 | if __name__ == "__main__": 115 | typer.run(train) 116 | -------------------------------------------------------------------------------- /src/mapmos/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | -------------------------------------------------------------------------------- /src/mapmos/cli.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 2 | # Stachniss. 3 | # 4 | # Permission is hereby granted, free of charge, to any person obtaining a copy 5 | # of this software and associated documentation files (the "Software"), to deal 6 | # in the Software without restriction, including without limitation the rights 7 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | # copies of the Software, and to permit persons to whom the Software is 9 | # furnished to do so, subject to the following conditions: 10 | # 11 | # The above copyright notice and this permission notice shall be included in all 12 | # copies or substantial portions of the Software. 13 | # 14 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | # SOFTWARE. 21 | 22 | # Check that the python bindings are properly built and can be loaded at runtime 23 | try: 24 | from mapmos.pybind import mapmos_pybind 25 | except Exception as e: 26 | print(80 * "*") 27 | print("[ERRROR] MapMOS Python bindings not available. Please build!") 28 | print(80 * "*") 29 | raise SystemExit() 30 | 31 | import glob 32 | import os 33 | from pathlib import Path 34 | from typing import Optional 35 | 36 | import typer 37 | from kiss_icp.tools.cmd import guess_dataloader 38 | 39 | from mapmos.datasets import ( 40 | available_dataloaders, 41 | jumpable_dataloaders, 42 | sequence_dataloaders, 43 | supported_file_extensions, 44 | ) 45 | 46 | 47 | def name_callback(value: str): 48 | if not value: 49 | return value 50 | dl = available_dataloaders() 51 | if value not in dl: 52 | raise typer.BadParameter(f"Supported dataloaders are:\n{', '.join(dl)}") 53 | return value 54 | 55 | 56 | app = typer.Typer(add_completion=False, rich_markup_mode="rich") 57 | 58 | # Remove from the help those dataloaders we explicitly say how to use 59 | _available_dl_help = available_dataloaders() 60 | _available_dl_help.remove("generic") 61 | _available_dl_help.remove("mcap") 62 | _available_dl_help.remove("ouster") 63 | _available_dl_help.remove("rosbag") 64 | _available_dl_help.remove("mapmos_dataset") 65 | 66 | docstring = f""" 67 | MapMOS: Building Volumetric Beliefs for Dynamic Environments Exploiting Map-Based Moving Object Segmentation\n 68 | \b 69 | [bold green]Examples: [/bold green] 70 | # Process all pointclouds in the given \[{", ".join(supported_file_extensions())}] 71 | $ mapmos_pipeline --visualize :page_facing_up: :open_file_folder: 72 | 73 | # Process a given [bold]ROS1/ROS2 [/bold]rosbag file (directory:open_file_folder:, ".bag":page_facing_up:, or "metadata.yaml":page_facing_up:) 74 | $ mapmos_pipeline --visualize :page_facing_up: [:open_file_folder:/:page_facing_up:] 75 | 76 | # Process [bold]mcap [/bold] recording 77 | $ mapmos_pipeline --visualize :page_facing_up: :page_facing_up: 78 | 79 | # Process [bold]Ouster pcap[/bold] recording (requires ouster-sdk Python package installed) 80 | $ mapmos_pipeline --visualize :page_facing_up: :page_facing_up: \[--meta :page_facing_up:] 81 | 82 | # Use a more specific dataloader to also load GT moving labels and compute metrics: {", ".join(_available_dl_help)} 83 | $ mapmos_pipeline --dataloader kitti --sequence 07 --visualize :page_facing_up: :open_file_folder: 84 | """ 85 | 86 | 87 | @app.command(help=docstring) 88 | def mapmos_pipeline( 89 | weights: Path = typer.Argument( 90 | ..., 91 | help="Path to the model's weights (.ckpt)", 92 | show_default=False, 93 | ), 94 | data: Path = typer.Argument( 95 | ..., 96 | help="The data directory used by the specified dataloader", 97 | show_default=False, 98 | ), 99 | dataloader: str = typer.Option( 100 | None, 101 | show_default=False, 102 | case_sensitive=False, 103 | autocompletion=available_dataloaders, 104 | callback=name_callback, 105 | help="[Optional] Use a specific dataloader from those supported by MapMOS", 106 | ), 107 | config: Optional[Path] = typer.Option( 108 | None, 109 | "--config", 110 | exists=True, 111 | show_default=False, 112 | help="[Optional] Path to the configuration file", 113 | ), 114 | # Aditional Options --------------------------------------------------------------------------- 115 | visualize: bool = typer.Option( 116 | False, 117 | "--visualize", 118 | "-v", 119 | help="[Optional] Open an online visualization of the KISS-ICP pipeline", 120 | rich_help_panel="Additional Options", 121 | ), 122 | sequence: Optional[str] = typer.Option( 123 | None, 124 | "--sequence", 125 | "-s", 126 | show_default=False, 127 | help="[Optional] For some dataloaders, you need to specify a given sequence", 128 | rich_help_panel="Additional Options", 129 | ), 130 | topic: Optional[str] = typer.Option( 131 | None, 132 | "--topic", 133 | "-t", 134 | show_default=False, 135 | help="[Optional] Only valid when processing rosbag files", 136 | rich_help_panel="Additional Options", 137 | ), 138 | save_ply: bool = typer.Option( 139 | False, 140 | "--save_ply", 141 | "-ply", 142 | help="[Optional] Save predictions to ply file", 143 | rich_help_panel="Additional Options", 144 | ), 145 | save_kitti: bool = typer.Option( 146 | False, 147 | "--save_kitti", 148 | "-kitti", 149 | help="[Optional] Save predictions to SemanticKITTI .lable file", 150 | rich_help_panel="Additional Options", 151 | ), 152 | n_scans: int = typer.Option( 153 | -1, 154 | "--n-scans", 155 | "-n", 156 | show_default=False, 157 | help="[Optional] Specify the number of scans to process, default is the entire dataset", 158 | rich_help_panel="Additional Options", 159 | ), 160 | jump: int = typer.Option( 161 | 0, 162 | "--jump", 163 | "-j", 164 | show_default=False, 165 | help="[Optional] Specify if you want to start to process scans from a given starting point", 166 | rich_help_panel="Additional Options", 167 | ), 168 | meta: Optional[Path] = typer.Option( 169 | None, 170 | "--meta", 171 | "-m", 172 | exists=True, 173 | show_default=False, 174 | help="[Optional] For Ouster pcap dataloader, specify metadata json file path explicitly", 175 | rich_help_panel="Additional Options", 176 | ), 177 | paper: bool = typer.Option( 178 | False, 179 | "--paper", 180 | "-paper", 181 | help="[Optional] Use this to compare different belief fusion strategies as done in the original paper", 182 | rich_help_panel="Additional Options", 183 | ), 184 | ): 185 | # Attempt to guess some common file extensions to avoid using the --dataloader flag 186 | if not dataloader: 187 | dataloader, data = guess_dataloader(data, default_dataloader="generic") 188 | 189 | # Validate some options 190 | if dataloader in sequence_dataloaders() and sequence is None: 191 | print('You must specify a sequence "--sequence"') 192 | raise typer.Exit(code=1) 193 | 194 | if jump != 0 and dataloader not in jumpable_dataloaders(): 195 | print(f"[WARNING] '{dataloader}' does not support '--jump', starting from first frame") 196 | jump = 0 197 | # Lazy-loading for faster CLI 198 | from mapmos.datasets import dataset_factory 199 | 200 | if paper: 201 | from mapmos.paper_pipeline import PaperPipeline as Pipeline 202 | else: 203 | from mapmos.pipeline import MapMOSPipeline as Pipeline 204 | 205 | Pipeline( 206 | dataset=dataset_factory( 207 | dataloader=dataloader, 208 | data_dir=data, 209 | # Additional options 210 | sequence=sequence, 211 | topic=topic, 212 | meta=meta, 213 | ), 214 | weights=weights, 215 | config=config, 216 | visualize=visualize, 217 | save_ply=save_ply, 218 | save_kitti=save_kitti, 219 | n_scans=n_scans, 220 | jump=jump, 221 | ).run().print() 222 | 223 | 224 | def run(): 225 | app() 226 | -------------------------------------------------------------------------------- /src/mapmos/config/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | from .config import DataConfig, OdometryConfig 24 | from .parser import MapMOSConfig, load_config, write_config 25 | -------------------------------------------------------------------------------- /src/mapmos/config/config.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from typing import List 24 | 25 | from pydantic import BaseModel, Field 26 | 27 | 28 | class DataConfig(BaseModel): 29 | deskew: bool = False 30 | max_range: float = 100.0 31 | min_range: float = 3.0 32 | 33 | 34 | class OdometryConfig(BaseModel): 35 | voxel_size: float = 0.5 36 | max_points_per_voxel: int = 20 37 | initial_threshold: float = 2.0 38 | min_motion_th: float = 0.1 39 | 40 | 41 | class MOSConfig(BaseModel): 42 | voxel_size_mos: float = 0.1 43 | delay_mos: int = 10 44 | max_range_mos: float = 50.0 45 | min_range_mos: float = 0.0 46 | voxel_size_belief: float = 0.25 47 | max_range_belief: float = 150 48 | 49 | 50 | class TrainingConfig(BaseModel): 51 | id: str = "experiment_id" 52 | train: List[str] = Field( 53 | default_factory=lambda: [ 54 | "00", 55 | "01", 56 | "02", 57 | "03", 58 | "04", 59 | "05", 60 | "06", 61 | "07", 62 | "09", 63 | "10", 64 | ] 65 | ) 66 | val: List[str] = Field(default_factory=lambda: ["08"]) 67 | batch_size: int = 16 68 | accumulate_grad_batches: int = 1 69 | max_epochs: int = 100 70 | lr: float = 0.0001 71 | lr_epoch: int = 1 72 | lr_decay: float = 0.99 73 | weight_decay: float = 0.0001 74 | num_workers: int = 4 75 | -------------------------------------------------------------------------------- /src/mapmos/config/parser.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # NOTE: This module was contributed by Markus Pielmeier on PR #63 24 | from __future__ import annotations 25 | 26 | import importlib 27 | import sys 28 | from pathlib import Path 29 | from typing import Any, Dict, Optional 30 | 31 | from pydantic_settings import BaseSettings, SettingsConfigDict 32 | 33 | from mapmos.config.config import DataConfig, MOSConfig, OdometryConfig, TrainingConfig 34 | 35 | 36 | class MapMOSConfig(BaseSettings): 37 | model_config = SettingsConfigDict(env_prefix="mapmos_") 38 | out_dir: str = "results" 39 | data: DataConfig = DataConfig() 40 | odometry: OdometryConfig = OdometryConfig() 41 | mos: MOSConfig = MOSConfig() 42 | training: TrainingConfig = TrainingConfig() 43 | 44 | 45 | def _yaml_source(config_file: Optional[Path]) -> Dict[str, Any]: 46 | data = None 47 | if config_file is not None: 48 | try: 49 | yaml = importlib.import_module("yaml") 50 | except ModuleNotFoundError: 51 | print( 52 | "Custom configuration file specified but PyYAML is not installed on your system," 53 | ' run `pip install "kiss-icp[all]"`. You can also modify the config.py if your ' 54 | "system does not support PyYaml " 55 | ) 56 | sys.exit(1) 57 | with open(config_file) as cfg_file: 58 | data = yaml.safe_load(cfg_file) 59 | return data or {} 60 | 61 | 62 | def load_config(config_file: Optional[Path]) -> MapMOSConfig: 63 | """Load configuration from an Optional yaml file.""" 64 | config = MapMOSConfig(**_yaml_source(config_file)) 65 | return config 66 | 67 | 68 | def write_config(config: MapMOSConfig, filename: str): 69 | with open(filename, "w") as outfile: 70 | try: 71 | yaml = importlib.import_module("yaml") 72 | yaml.dump(config.model_dump(), outfile, default_flow_style=False) 73 | except ModuleNotFoundError: 74 | outfile.write(str(config.model_dump())) 75 | -------------------------------------------------------------------------------- /src/mapmos/datasets/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | from pathlib import Path 24 | from typing import Dict, List 25 | 26 | 27 | def supported_file_extensions(): 28 | return [ 29 | "bin", 30 | "pcd", 31 | "ply", 32 | "xyz", 33 | "obj", 34 | "ctm", 35 | "off", 36 | "stl", 37 | ] 38 | 39 | 40 | def sequence_dataloaders(): 41 | # TODO: automatically infer this 42 | return [ 43 | "kitti", 44 | "kitti_tracking", 45 | "nuscenes", 46 | "apollo", 47 | "helimos", 48 | ] 49 | 50 | 51 | def available_dataloaders() -> List: 52 | import os.path 53 | import pkgutil 54 | 55 | pkgpath = os.path.dirname(__file__) 56 | return [name for _, name, _ in pkgutil.iter_modules([pkgpath])] 57 | 58 | 59 | def jumpable_dataloaders(): 60 | _jumpable_dataloaders = available_dataloaders() 61 | _jumpable_dataloaders.remove("mcap") 62 | _jumpable_dataloaders.remove("ouster") 63 | _jumpable_dataloaders.remove("rosbag") 64 | return _jumpable_dataloaders 65 | 66 | 67 | def dataloader_types() -> Dict: 68 | import ast 69 | import importlib 70 | 71 | dataloaders = available_dataloaders() 72 | _types = {} 73 | for dataloader in dataloaders: 74 | script = importlib.util.find_spec(f".{dataloader}", __name__).origin 75 | with open(script) as f: 76 | tree = ast.parse(f.read(), script) 77 | classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] 78 | _types[dataloader] = classes[0].name # assuming there is only 1 class 79 | return _types 80 | 81 | 82 | def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): 83 | import importlib 84 | 85 | dataloader_type = dataloader_types()[dataloader] 86 | module = importlib.import_module(f".{dataloader}", __name__) 87 | assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" 88 | dataset = getattr(module, dataloader_type) 89 | return dataset(data_dir=data_dir, *args, **kwargs) 90 | -------------------------------------------------------------------------------- /src/mapmos/datasets/apollo.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | 25 | from mapmos.datasets.kitti import SemanticKITTIDataset 26 | 27 | 28 | class SemanticApolloDataset(SemanticKITTIDataset): 29 | def __init__(self, data_dir, sequence: str, *_, **__): 30 | super().__init__(data_dir=data_dir, sequence=sequence, *_, **__) 31 | self.correct_kitti_scan = lambda frame: frame 32 | 33 | def get_frames_timestamps(self) -> np.ndarray: 34 | return np.arange(0, self.__len__(), 1.0) 35 | 36 | def read_labels(self, filename): 37 | """Load moving object labels from .label file""" 38 | orig_labels = np.fromfile(filename, dtype=np.int32).reshape((-1)) 39 | orig_labels = orig_labels & 0xFFFF # Mask semantics in lower half 40 | labels = np.zeros_like(orig_labels) 41 | labels[orig_labels > 250] = 1 # Moving 42 | labels = labels.astype(dtype=np.int32).reshape(-1) 43 | return labels 44 | -------------------------------------------------------------------------------- /src/mapmos/datasets/generic.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | import sys 25 | from pathlib import Path 26 | 27 | import natsort 28 | import numpy as np 29 | 30 | from mapmos.datasets import supported_file_extensions 31 | 32 | 33 | class GenericDataset: 34 | def __init__(self, data_dir: Path, *_, **__): 35 | # Config stuff 36 | self.sequence_id = os.path.basename(data_dir) 37 | self.scans_dir = os.path.join(os.path.realpath(data_dir), "") 38 | self.scan_files = np.array( 39 | natsort.natsorted( 40 | [ 41 | os.path.join(self.scans_dir, fn) 42 | for fn in os.listdir(self.scans_dir) 43 | if any(fn.endswith(ext) for ext in supported_file_extensions()) 44 | ] 45 | ), 46 | dtype=str, 47 | ) 48 | if len(self.scan_files) == 0: 49 | raise ValueError(f"Tried to read point cloud files in {self.scans_dir} but none found") 50 | self.file_extension = self.scan_files[0].split(".")[-1] 51 | if self.file_extension not in supported_file_extensions(): 52 | raise ValueError(f"Supported formats are: {supported_file_extensions()}") 53 | 54 | # Obtain the pointcloud reader for the given data folder 55 | self._read_point_cloud = self._get_point_cloud_reader() 56 | 57 | def __len__(self): 58 | return len(self.scan_files) 59 | 60 | def __getitem__(self, idx): 61 | return self.read_point_cloud(self.scan_files[idx]) 62 | 63 | def read_point_cloud(self, file_path: str): 64 | points = self._read_point_cloud(file_path) 65 | return points.astype(np.float64) 66 | 67 | def _get_point_cloud_reader(self): 68 | """Attempt to guess with try/catch blocks which is the best point cloud reader to use for 69 | the given dataset folder. Supported readers so far are: 70 | - np.fromfile 71 | - trimesh.load 72 | - PyntCloud 73 | - open3d[optional] 74 | """ 75 | # This is easy, the old KITTI format 76 | if self.file_extension == "bin": 77 | print("[WARNING] Reading .bin files, the only format supported is the KITTI format") 78 | return lambda file: np.fromfile(file, dtype=np.float32).reshape((-1, 4))[:, :3] 79 | 80 | print('Trying to guess how to read your data: `pip install "mapmos[all]"` is required') 81 | first_scan_file = self.scan_files[0] 82 | 83 | # first try trimesh 84 | try: 85 | import trimesh 86 | 87 | trimesh.load(first_scan_file) 88 | return lambda file: np.asarray(trimesh.load(file).vertices) 89 | except: 90 | pass 91 | 92 | # then try pyntcloud 93 | try: 94 | from pyntcloud import PyntCloud 95 | 96 | PyntCloud.from_file(first_scan_file) 97 | return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy() 98 | except: 99 | pass 100 | 101 | # lastly with open3d 102 | try: 103 | import open3d as o3d 104 | 105 | o3d.io.read_point_cloud(first_scan_file) 106 | return lambda file: np.asarray(o3d.io.read_point_cloud(file).points, dtype=np.float64) 107 | except: 108 | print("[ERROR], File format not supported") 109 | sys.exit(1) 110 | -------------------------------------------------------------------------------- /src/mapmos/datasets/helimos.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import glob 24 | import os 25 | 26 | import numpy as np 27 | 28 | 29 | class HeliMOSDataset: 30 | def __init__(self, data_dir, sequence: str, *_, **__): 31 | self.sequence_id = sequence.split("/")[0] 32 | split_file = sequence.split("/")[1] 33 | self.sequence_dir = os.path.join(data_dir, self.sequence_id) 34 | self.scan_dir = os.path.join(self.sequence_dir, "velodyne/") 35 | 36 | self.scan_files = sorted(glob.glob(self.scan_dir + "*.bin")) 37 | self.calibration = self.read_calib_file(os.path.join(self.sequence_dir, "calib.txt")) 38 | 39 | # Load GT Poses (if available) 40 | self.poses_fn = os.path.join(self.sequence_dir, "poses.txt") 41 | if os.path.exists(self.poses_fn): 42 | self.gt_poses = self.load_poses(self.poses_fn) 43 | 44 | # No correction 45 | self.correct_kitti_scan = lambda frame: frame 46 | 47 | # Load labels 48 | self.label_dir = os.path.join(self.sequence_dir, "labels/") 49 | label_files = sorted(glob.glob(self.label_dir + "*.label")) 50 | 51 | # Get labels for train/val split if desired 52 | label_indices = np.loadtxt(os.path.join(data_dir, split_file), dtype=int).tolist() 53 | 54 | # Filter based on split if desired 55 | getIndex = lambda filename: int(os.path.basename(filename).split(".label")[0]) 56 | self.dict_label_files = { 57 | getIndex(filename): filename 58 | for filename in label_files 59 | if getIndex(filename) in label_indices 60 | } 61 | 62 | def __getitem__(self, idx): 63 | points = self.scans(idx) 64 | timestamps = np.zeros(len(points)) 65 | labels = ( 66 | self.read_labels(self.dict_label_files[idx]) 67 | if idx in self.dict_label_files.keys() 68 | else np.full(len(points), -1, dtype=np.int32) 69 | ) 70 | return points, timestamps, labels 71 | 72 | def __len__(self): 73 | return len(self.scan_files) 74 | 75 | def scans(self, idx): 76 | return self.read_point_cloud(self.scan_files[idx]) 77 | 78 | def apply_calibration(self, poses: np.ndarray) -> np.ndarray: 79 | """Converts from Velodyne to Camera Frame""" 80 | Tr = np.eye(4, dtype=np.float64) 81 | Tr[:3, :4] = self.calibration["Tr"].reshape(3, 4) 82 | return Tr @ poses @ np.linalg.inv(Tr) 83 | 84 | def read_point_cloud(self, scan_file: str): 85 | points = np.fromfile(scan_file, dtype=np.float32).reshape((-1, 4))[:, :3].astype(np.float64) 86 | return points 87 | 88 | def load_poses(self, poses_file): 89 | def _lidar_pose_gt(poses_gt): 90 | _tr = self.calibration["Tr"].reshape(3, 4) 91 | tr = np.eye(4, dtype=np.float64) 92 | tr[:3, :4] = _tr 93 | left = np.einsum("...ij,...jk->...ik", np.linalg.inv(tr), poses_gt) 94 | right = np.einsum("...ij,...jk->...ik", left, tr) 95 | return right 96 | 97 | poses = np.loadtxt(poses_file, delimiter=" ") 98 | n = poses.shape[0] 99 | poses = np.concatenate( 100 | (poses, np.zeros((n, 3), dtype=np.float32), np.ones((n, 1), dtype=np.float32)), axis=1 101 | ) 102 | poses = poses.reshape((n, 4, 4)) # [N, 4, 4] 103 | 104 | # Ensure rotations are SO3 105 | rotations = poses[:, :3, :3] 106 | U, _, Vh = np.linalg.svd(rotations) 107 | poses[:, :3, :3] = U @ Vh 108 | 109 | return _lidar_pose_gt(poses) 110 | 111 | @staticmethod 112 | def read_calib_file(file_path: str) -> dict: 113 | calib_dict = {} 114 | with open(file_path, "r") as calib_file: 115 | for line in calib_file.readlines(): 116 | tokens = line.split(" ") 117 | if tokens[0] == "calib_time:": 118 | continue 119 | # Only read with float data 120 | if len(tokens) > 0: 121 | values = [float(token) for token in tokens[1:]] 122 | values = np.array(values, dtype=np.float32) 123 | 124 | # The format in KITTI's file is : ...\n -> Remove the ':' 125 | key = tokens[0][:-1] 126 | calib_dict[key] = values 127 | return calib_dict 128 | 129 | def read_labels(self, filename): 130 | """Load moving object labels from .label file""" 131 | orig_labels = np.fromfile(filename, dtype=np.int32).reshape((-1)) 132 | orig_labels = orig_labels & 0xFFFF # Mask semantics in lower half 133 | 134 | labels = np.zeros_like(orig_labels) 135 | labels[orig_labels <= 1] = -1 # Unlabeled (0), outlier (1) 136 | labels[orig_labels > 250] = 1 # Moving 137 | labels = labels.astype(dtype=np.int32).reshape(-1) 138 | return labels 139 | -------------------------------------------------------------------------------- /src/mapmos/datasets/kitti.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import glob 24 | import os 25 | 26 | import numpy as np 27 | from kiss_icp.datasets.kitti import KITTIOdometryDataset 28 | 29 | 30 | class SemanticKITTIDataset(KITTIOdometryDataset): 31 | def __init__(self, data_dir, sequence: str, *_, **__): 32 | self.sequence_id = sequence.zfill(2) 33 | self.kitti_sequence_dir = os.path.join(data_dir, "sequences", self.sequence_id) 34 | self.velodyne_dir = os.path.join(self.kitti_sequence_dir, "velodyne/") 35 | 36 | self.scan_files = sorted(glob.glob(self.velodyne_dir + "*.bin")) 37 | self.calibration = self.read_calib_file(os.path.join(self.kitti_sequence_dir, "calib.txt")) 38 | 39 | # Load GT Poses (if available) 40 | self.poses_fn = os.path.join(data_dir, f"poses/{self.sequence_id}.txt") 41 | if os.path.exists(self.poses_fn): 42 | self.gt_poses = self.load_poses(self.poses_fn) 43 | 44 | # Add correction for KITTI datasets, can be easilty removed if unwanted 45 | from kiss_icp.pybind import kiss_icp_pybind 46 | 47 | self.correct_kitti_scan = lambda frame: np.asarray( 48 | kiss_icp_pybind._correct_kitti_scan(kiss_icp_pybind._Vector3dVector(frame)) 49 | ) 50 | 51 | self.label_dir = os.path.join(self.kitti_sequence_dir, "labels/") 52 | self.label_files = sorted(glob.glob(self.label_dir + "*.label")) 53 | 54 | # Account for incomplete label files 55 | label_map = {os.path.basename(path): path for path in self.label_files} 56 | if len(self.label_files) != len(self.scan_files): 57 | self.label_files = [ 58 | label_map.get(os.path.basename(scan_file).replace(".bin", ".label"), None) 59 | for scan_file in self.scan_files 60 | ] 61 | 62 | def __getitem__(self, idx): 63 | points, timestamps = self.scans(idx) 64 | labels = ( 65 | self.read_labels(self.label_files[idx]) 66 | if (self.label_files and self.label_files[idx] != None) 67 | else np.full((len(points), 1), -1, dtype=np.int32) 68 | ).reshape(-1) 69 | return points, timestamps, labels 70 | 71 | def read_labels(self, filename): 72 | """Load moving object labels from .label file""" 73 | orig_labels = np.fromfile(filename, dtype=np.int32).reshape((-1)) 74 | orig_labels = orig_labels & 0xFFFF # Mask semantics in lower half 75 | labels = np.zeros_like(orig_labels) 76 | labels[orig_labels <= 1] = -1 # Unlabeled (0), outlier (1) 77 | labels[orig_labels > 250] = 1 # Moving 78 | labels = labels.astype(dtype=np.int32).reshape(-1) 79 | return labels 80 | -------------------------------------------------------------------------------- /src/mapmos/datasets/kitti_tracking.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | 25 | from mapmos.datasets.kitti import SemanticKITTIDataset 26 | 27 | 28 | class SemanticKITTITrackingDataset(SemanticKITTIDataset): 29 | def __init__(self, data_dir, sequence: str, *_, **__): 30 | super().__init__(data_dir=data_dir, sequence=sequence, *_, **__) 31 | 32 | def get_frames_timestamps(self) -> np.ndarray: 33 | return np.arange(0, self.__len__(), 1.0) 34 | 35 | def read_labels(self, filename): 36 | """Load moving object labels from .label file""" 37 | orig_labels = np.fromfile(filename, dtype=np.int32).reshape((-1)) 38 | orig_labels = orig_labels & 0xFFFF # Mask semantics in lower half 39 | 40 | labels = np.zeros_like(orig_labels) 41 | labels[orig_labels <= 2] = -1 # Unlabeled (0), outlier (1) and reflection (2) 42 | labels[orig_labels > 250] = 1 # Moving 43 | labels = labels.astype(dtype=np.int32).reshape(-1) 44 | return labels 45 | -------------------------------------------------------------------------------- /src/mapmos/datasets/mapmos_dataset.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import os 24 | from pathlib import Path 25 | from typing import Dict 26 | 27 | import numpy as np 28 | import torch 29 | from pytorch_lightning import LightningDataModule 30 | from torch.utils.data import DataLoader, Dataset 31 | 32 | from mapmos.config import DataConfig, MapMOSConfig, OdometryConfig 33 | from mapmos.datasets import dataset_factory, sequence_dataloaders 34 | from mapmos.mapping import VoxelHashMap 35 | from mapmos.odometry import Odometry 36 | from mapmos.utils.cache import get_cache, memoize 37 | 38 | 39 | def collate_fn(batch): 40 | # Returns tensor of [batch, x, y, z, t, scan_index, label] 41 | tensor_batch = None 42 | for i, ( 43 | scan_points, 44 | map_points, 45 | scan_timestamps, 46 | map_timestamps, 47 | scan_labels, 48 | map_labels, 49 | ) in enumerate(batch): 50 | ones = torch.ones(len(scan_points), 1).type_as(scan_points) 51 | scan_points = torch.hstack( 52 | [ 53 | i * ones, 54 | scan_points, 55 | 0.0 * ones, 56 | scan_timestamps, 57 | scan_labels, 58 | ] 59 | ) 60 | 61 | ones = torch.ones(len(map_points), 1).type_as(map_points) 62 | map_points = torch.hstack( 63 | [ 64 | i * ones, 65 | map_points, 66 | -1.0 * ones, 67 | map_timestamps, 68 | map_labels, 69 | ] 70 | ) 71 | 72 | tensor = torch.vstack([scan_points, map_points]) 73 | tensor_batch = tensor if tensor_batch is None else torch.vstack([tensor_batch, tensor]) 74 | return tensor_batch 75 | 76 | 77 | class MapMOSDataModule(LightningDataModule): 78 | """Training and validation set for Pytorch Lightning""" 79 | 80 | def __init__(self, dataloader: str, data_dir: Path, config: MapMOSConfig, cache_dir: Path): 81 | super(MapMOSDataModule, self).__init__() 82 | self.dataloader = dataloader 83 | self.data_dir = data_dir 84 | self.config = config 85 | self.cache_dir = cache_dir 86 | if self.cache_dir == None: 87 | print("No cache specified, therefore disabling shuffle during training!") 88 | self.shuffle = True if self.cache_dir is not None else False 89 | 90 | assert dataloader in sequence_dataloaders() 91 | 92 | def prepare_data(self): 93 | pass 94 | 95 | def setup(self, stage=None): 96 | train_set = MapMOSDataset( 97 | self.dataloader, self.data_dir, self.config, self.config.training.train, self.cache_dir 98 | ) 99 | val_set = MapMOSDataset( 100 | self.dataloader, self.data_dir, self.config, self.config.training.val, self.cache_dir 101 | ) 102 | self.train_loader = DataLoader( 103 | dataset=train_set, 104 | batch_size=self.config.training.batch_size, 105 | collate_fn=collate_fn, 106 | shuffle=self.shuffle, 107 | num_workers=self.config.training.num_workers, 108 | pin_memory=True, 109 | persistent_workers=True, 110 | drop_last=False, 111 | timeout=0, 112 | ) 113 | 114 | self.train_iter = iter(self.train_loader) 115 | 116 | self.valid_loader = DataLoader( 117 | dataset=val_set, 118 | batch_size=1, 119 | collate_fn=collate_fn, 120 | shuffle=False, 121 | num_workers=self.config.training.num_workers, 122 | pin_memory=True, 123 | persistent_workers=True, 124 | drop_last=False, 125 | timeout=0, 126 | ) 127 | 128 | self.valid_iter = iter(self.valid_loader) 129 | 130 | print( 131 | "Loaded {:d} training and {:d} validation samples.".format(len(train_set), len(val_set)) 132 | ) 133 | 134 | def train_dataloader(self): 135 | return self.train_loader 136 | 137 | def val_dataloader(self): 138 | return self.valid_loader 139 | 140 | 141 | class MapMOSDataset(Dataset): 142 | """Caches and returns scan and local maps for multiple sequences""" 143 | 144 | def __init__( 145 | self, 146 | dataloader: str, 147 | data_dir: Path, 148 | config: MapMOSConfig, 149 | sequences: list, 150 | cache_dir: Path, 151 | ): 152 | self.config = config 153 | self.sequences = sequences 154 | self._print = False 155 | 156 | # Cache 157 | if cache_dir is not None: 158 | directory = os.path.join(cache_dir, dataloader) 159 | self.use_cache = True 160 | self.cache = get_cache(directory=directory) 161 | print("Using cache at ", directory) 162 | else: 163 | self.use_cache = False 164 | self.cache = get_cache(directory=os.path.join(data_dir, "cache")) 165 | 166 | # Create datasets and map a sample index to the sequence and scan index 167 | self.datasets = {} 168 | self.idx_mapper = {} 169 | idx = 0 170 | for sequence in self.sequences: 171 | self.datasets[sequence] = dataset_factory( 172 | dataloader=dataloader, 173 | data_dir=data_dir, 174 | sequence=sequence, 175 | ) 176 | for sample_idx in range(len(self.datasets[sequence])): 177 | self.idx_mapper[idx] = (sequence, sample_idx) 178 | idx += 1 179 | 180 | self.sequence = None 181 | self.odometry = Odometry(self.config.data, self.config.odometry) 182 | 183 | def __len__(self): 184 | return len(self.idx_mapper.keys()) 185 | 186 | def __getitem__(self, idx): 187 | sequence, scan_index = self.idx_mapper[idx] 188 | ( 189 | scan_points, 190 | map_points, 191 | scan_timestamps, 192 | map_timestamps, 193 | scan_labels, 194 | map_labels, 195 | ) = self.get_scan_and_map( 196 | sequence, 197 | scan_index, 198 | dict(self.config.data), 199 | dict(self.config.odometry), 200 | ) 201 | return scan_points, map_points, scan_timestamps, map_timestamps, scan_labels, map_labels 202 | 203 | @memoize() 204 | def get_scan_and_map( 205 | self, 206 | sequence: int, 207 | scan_index: int, 208 | data_config_dict: Dict, 209 | odometry_config_dict: Dict, 210 | ): 211 | """Returns scan points, map points in local frame and labels. Scan and map need to be in 212 | local frame to allow for efficient cropping (sample point does not change). We use the 213 | VoxelHashMap to keep track of the GT labels for map points. 214 | """ 215 | if not self._print: 216 | print("*****Caching now*****") 217 | self._print = True 218 | 219 | scan_points, timestamps, scan_labels = self.datasets[sequence][scan_index] 220 | 221 | # Only consider valid points 222 | valid_mask = scan_labels != -1 223 | scan_points = scan_points[valid_mask] 224 | scan_labels = scan_labels[valid_mask] 225 | 226 | if self.sequence != sequence or len(scan_points) == 0: 227 | data_config = DataConfig().model_validate(data_config_dict) 228 | odometry_config = OdometryConfig().model_validate(odometry_config_dict) 229 | 230 | self.odometry = Odometry(data_config, odometry_config) 231 | self.gt_map = VoxelHashMap(odometry_config.voxel_size, data_config.max_range) 232 | self.sequence = sequence 233 | 234 | registered_map_points, map_timestamps = self.odometry.get_map_points() 235 | map_belief = self.gt_map.get_belief(registered_map_points) 236 | map_labels = np.zeros_like(map_belief) 237 | map_labels[map_belief > 0] = 1.0 238 | 239 | scan_points_registered = self.odometry.register_points(scan_points, timestamps, scan_index) 240 | update = -1.0 * np.ones_like(scan_labels) 241 | update[scan_labels == 1] = 1.0 242 | self.gt_map.update_belief(scan_points_registered, update) 243 | 244 | self.gt_map.remove_voxels_far_from_location(self.odometry.current_location()) 245 | 246 | map_points = self.odometry.transform( 247 | registered_map_points, np.linalg.inv(self.odometry.last_pose) 248 | ) 249 | 250 | scan_timestamps = scan_index * np.ones(len(scan_points)) 251 | return ( 252 | torch.tensor(scan_points).to(torch.float32).reshape(-1, 3), 253 | torch.tensor(map_points).to(torch.float32).reshape(-1, 3), 254 | torch.tensor(scan_timestamps).to(torch.float32).reshape(-1, 1), 255 | torch.tensor(map_timestamps).to(torch.float32).reshape(-1, 1), 256 | torch.tensor(scan_labels).to(torch.float32).reshape(-1, 1), 257 | torch.tensor(map_labels).to(torch.float32).reshape(-1, 1), 258 | ) 259 | -------------------------------------------------------------------------------- /src/mapmos/datasets/mcap.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | import sys 25 | 26 | 27 | class McapDataloader: 28 | def __init__(self, data_dir: str, topic: str, *_, **__): 29 | """Standalone .mcap dataloader withouth any ROS distribution.""" 30 | # Conditional imports to avoid injecting dependencies for non mcap users 31 | try: 32 | from mcap.reader import make_reader 33 | from mcap_ros2.reader import read_ros2_messages 34 | except ImportError as e: 35 | print("mcap plugins not installed: 'pip install mcap-ros2-support'") 36 | exit(1) 37 | 38 | from kiss_icp.tools.point_cloud2 import read_point_cloud 39 | 40 | # we expect `data_dir` param to be a path to the .mcap file, so rename for clarity 41 | assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file" 42 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 43 | mcap_file = str(data_dir) 44 | 45 | self.bag = make_reader(open(mcap_file, "rb")) 46 | self.summary = self.bag.get_summary() 47 | self.topic = self.check_topic(topic) 48 | self.n_scans = self._get_n_scans() 49 | self.msgs = read_ros2_messages(mcap_file, topics=topic) 50 | self.read_point_cloud = read_point_cloud 51 | self.use_global_visualizer = True 52 | 53 | def __del__(self): 54 | if hasattr(self, "bag"): 55 | del self.bag 56 | 57 | def __getitem__(self, idx): 58 | msg = next(self.msgs).ros_msg 59 | return self.read_point_cloud(msg) 60 | 61 | def __len__(self): 62 | return self.n_scans 63 | 64 | def _get_n_scans(self) -> int: 65 | return sum( 66 | count 67 | for (id, count) in self.summary.statistics.channel_message_counts.items() 68 | if self.summary.channels[id].topic == self.topic 69 | ) 70 | 71 | def check_topic(self, topic: str) -> str: 72 | # Extract schema id from the .mcap file that encodes the PointCloud2 msg 73 | schema_id = [ 74 | schema.id 75 | for schema in self.summary.schemas.values() 76 | if schema.name == "sensor_msgs/msg/PointCloud2" 77 | ][0] 78 | 79 | point_cloud_topics = [ 80 | channel.topic 81 | for channel in self.summary.channels.values() 82 | if channel.schema_id == schema_id 83 | ] 84 | 85 | def print_available_topics_and_exit(): 86 | print(50 * "-") 87 | for t in point_cloud_topics: 88 | print(f"--topic {t}") 89 | print(50 * "-") 90 | sys.exit(1) 91 | 92 | if topic and topic in point_cloud_topics: 93 | return topic 94 | # when user specified the topic check that exists 95 | if topic and topic not in point_cloud_topics: 96 | print( 97 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 98 | "Please select one of the following topics with the --topic flag" 99 | ) 100 | print_available_topics_and_exit() 101 | if len(point_cloud_topics) > 1: 102 | print( 103 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 104 | "Please select one of the following topics with the --topic flag" 105 | ) 106 | print_available_topics_and_exit() 107 | 108 | if len(point_cloud_topics) == 0: 109 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 110 | if len(point_cloud_topics) == 1: 111 | return point_cloud_topics[0] 112 | -------------------------------------------------------------------------------- /src/mapmos/datasets/nuscenes.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import importlib 24 | import os 25 | import sys 26 | from pathlib import Path 27 | from typing import List 28 | 29 | import numpy as np 30 | from nuscenes.utils.geometry_utils import transform_matrix 31 | from pyquaternion import Quaternion 32 | 33 | 34 | class NuScenesDataset: 35 | def __init__(self, data_dir: Path, sequence: int, *_, **__): 36 | try: 37 | importlib.import_module("nuscenes") 38 | except ModuleNotFoundError: 39 | print("nuscenes-devkit is not installed on your system") 40 | print('run "pip install nuscenes-devkit"') 41 | sys.exit(1) 42 | 43 | # TODO: If someone needs more splits from nuScenes expose this 2 parameters 44 | # nusc_version: str = "v1.0-trainval" 45 | # split: str = "train" 46 | nusc_version: str = "v1.0-mini" 47 | split: str = "mini_train" 48 | self.lidar_name: str = "LIDAR_TOP" 49 | 50 | # Lazy loading 51 | from nuscenes.nuscenes import NuScenes 52 | from nuscenes.utils.splits import create_splits_logs 53 | 54 | self.sequence_id = str(int(sequence)).zfill(4) 55 | 56 | self.nusc = NuScenes(dataroot=str(data_dir), version=nusc_version) 57 | self.scene_name = f"scene-{self.sequence_id}" 58 | if self.scene_name not in [s["name"] for s in self.nusc.scene]: 59 | print(f'[ERROR] Sequence "{self.sequence_id}" not available scenes') 60 | print("\nAvailable scenes:") 61 | self.nusc.list_scenes() 62 | sys.exit(1) 63 | 64 | # Load nuScenes read from file inside dataloader module 65 | self.load_point_cloud = importlib.import_module( 66 | "nuscenes.utils.data_classes" 67 | ).LidarPointCloud.from_file 68 | 69 | # Get assignment of scenes to splits. 70 | from nuscenes.utils.splits import create_splits_logs 71 | 72 | split_logs = create_splits_logs(split, self.nusc) 73 | 74 | # Use only the samples from the current split. 75 | scene_token = self._get_scene_token(split_logs) 76 | self.lidar_tokens = self._get_lidar_tokens(scene_token) 77 | self.gt_poses = self._load_poses() 78 | 79 | # Define which attributes are considered moving 80 | self.moving_attributes = ["vehicle.moving", "cycle.with_rider", "pedestrian.moving"] 81 | 82 | def __len__(self): 83 | return len(self.lidar_tokens) 84 | 85 | def __getitem__(self, idx): 86 | points = self.read_point_cloud(self.lidar_tokens[idx]) 87 | timestamps = np.zeros(len(points)) 88 | labels = self.read_labels(points, idx) 89 | return points, timestamps, labels 90 | 91 | def read_labels(self, points, idx): 92 | lidar_token = self.lidar_tokens[idx] 93 | is_key_frame = self.nusc.get("sample_data", lidar_token)["is_key_frame"] 94 | 95 | sample_token = self.nusc.get("sample_data", lidar_token)["sample_token"] 96 | annotation_tokens = self.nusc.get("sample", sample_token)["anns"] 97 | 98 | # Annotations are only available for key frames 99 | if not is_key_frame or len(annotation_tokens) == 0: 100 | return np.full((len(points), 1), -1, dtype=np.int32) 101 | 102 | points_hom = np.hstack((points, np.ones((len(points), 1)))) 103 | global_points_hom = (self.global_pose(idx) @ points_hom.T).T 104 | labels = np.zeros((len(points), 1), dtype=np.int32) 105 | for annotation_token in annotation_tokens: 106 | annotation = self.nusc.get("sample_annotation", annotation_token) 107 | attribute_token = annotation["attribute_tokens"] 108 | if ( 109 | len(attribute_token) > 0 110 | and self.nusc.get("attribute", attribute_token[0])["name"] in self.moving_attributes 111 | ): 112 | box_center = self.nusc.get_box(annotation_token).center 113 | box_rotation = self.nusc.get_box(annotation_token).rotation_matrix 114 | box_wlh = self.nusc.get_box(annotation_token).wlh 115 | box_pose = np.vstack( 116 | [ 117 | np.hstack([box_rotation, box_center.reshape(-1, 1)]), 118 | np.array([[0, 0, 0, 1]]), 119 | ] 120 | ) 121 | local_points = (np.linalg.inv(box_pose) @ global_points_hom.T).T[:, :3] 122 | abs_local_points = np.abs(local_points) 123 | mask = abs_local_points[:, 0] < box_wlh[1] / 2 124 | mask = np.logical_and(mask, abs_local_points[:, 1] <= box_wlh[0] / 2) 125 | mask = np.logical_and(mask, abs_local_points[:, 2] <= box_wlh[2] / 2) 126 | labels[mask] = 1.0 127 | return labels 128 | 129 | @staticmethod 130 | def get_timestamps(points): 131 | x = points[:, 0] 132 | y = points[:, 1] 133 | yaw = -np.arctan2(y, x) 134 | timestamps = 0.5 * (yaw / np.pi + 1.0) 135 | return timestamps 136 | 137 | def global_pose(self, idx): 138 | sd_record_lid = self.nusc.get("sample_data", self.lidar_tokens[idx]) 139 | cs_record_lid = self.nusc.get("calibrated_sensor", sd_record_lid["calibrated_sensor_token"]) 140 | ep_record_lid = self.nusc.get("ego_pose", sd_record_lid["ego_pose_token"]) 141 | 142 | car_to_velo = transform_matrix( 143 | cs_record_lid["translation"], 144 | Quaternion(cs_record_lid["rotation"]), 145 | ) 146 | pose_car = transform_matrix( 147 | ep_record_lid["translation"], 148 | Quaternion(ep_record_lid["rotation"]), 149 | ) 150 | return pose_car @ car_to_velo 151 | 152 | def read_point_cloud(self, token: str): 153 | filename = self.nusc.get("sample_data", token)["filename"] 154 | pcl = self.load_point_cloud(os.path.join(self.nusc.dataroot, filename)) 155 | points = pcl.points.T[:, :3] 156 | return points.astype(np.float64) 157 | 158 | def _load_poses(self) -> np.ndarray: 159 | from nuscenes.utils.geometry_utils import transform_matrix 160 | from pyquaternion import Quaternion 161 | 162 | poses = np.empty((len(self), 4, 4), dtype=np.float32) 163 | for i, lidar_token in enumerate(self.lidar_tokens): 164 | sd_record_lid = self.nusc.get("sample_data", lidar_token) 165 | cs_record_lid = self.nusc.get( 166 | "calibrated_sensor", sd_record_lid["calibrated_sensor_token"] 167 | ) 168 | ep_record_lid = self.nusc.get("ego_pose", sd_record_lid["ego_pose_token"]) 169 | 170 | car_to_velo = transform_matrix( 171 | cs_record_lid["translation"], 172 | Quaternion(cs_record_lid["rotation"]), 173 | ) 174 | pose_car = transform_matrix( 175 | ep_record_lid["translation"], 176 | Quaternion(ep_record_lid["rotation"]), 177 | ) 178 | 179 | poses[i:, :] = pose_car @ car_to_velo 180 | 181 | # Convert from global coordinate poses to local poses 182 | first_pose = poses[0, :, :] 183 | poses = np.linalg.inv(first_pose) @ poses 184 | return poses 185 | 186 | def _get_scene_token(self, split_logs: List[str]) -> str: 187 | """ 188 | Convenience function to get the samples in a particular split. 189 | :param split_logs: A list of the log names in this split. 190 | :return: The list of samples. 191 | """ 192 | scene_tokens = [s["token"] for s in self.nusc.scene if s["name"] == self.scene_name][0] 193 | scene = self.nusc.get("scene", scene_tokens) 194 | log = self.nusc.get("log", scene["log_token"]) 195 | return scene["token"] if log["logfile"] in split_logs else "" 196 | 197 | def _get_lidar_tokens(self, scene_token: str) -> List[str]: 198 | # Get records from DB. 199 | scene_rec = self.nusc.get("scene", scene_token) 200 | start_sample_rec = self.nusc.get("sample", scene_rec["first_sample_token"]) 201 | sd_rec = self.nusc.get("sample_data", start_sample_rec["data"][self.lidar_name]) 202 | 203 | # Make list of frames 204 | cur_sd_rec = sd_rec 205 | sd_tokens = [cur_sd_rec["token"]] 206 | while cur_sd_rec["next"] != "": 207 | cur_sd_rec = self.nusc.get("sample_data", cur_sd_rec["next"]) 208 | sd_tokens.append(cur_sd_rec["token"]) 209 | return sd_tokens 210 | -------------------------------------------------------------------------------- /src/mapmos/datasets/ouster.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # Copyright (c) 2023 Pavlo Bashmakov 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import glob 26 | import os 27 | from typing import Optional 28 | 29 | import numpy as np 30 | 31 | 32 | def find_metadata_json(pcap_file: str) -> str: 33 | """Attempts to resolve the metadata json file for a provided pcap file.""" 34 | dir_path, filename = os.path.split(pcap_file) 35 | if not filename: 36 | return "" 37 | if not dir_path: 38 | dir_path = os.getcwd() 39 | json_candidates = sorted(glob.glob(f"{dir_path}/*.json")) 40 | if not json_candidates: 41 | return "" 42 | prefix_sizes = list( 43 | map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates) 44 | ) 45 | max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i]) 46 | return json_candidates[max_elem] 47 | 48 | 49 | class OusterDataloader: 50 | """Ouster pcap dataloader""" 51 | 52 | def __init__( 53 | self, 54 | data_dir: str, 55 | meta: Optional[str] = None, 56 | *_, 57 | **__, 58 | ): 59 | """Create Ouster pcap dataloader to read scans from a pcap file. 60 | 61 | Ouster pcap can be recorded with a `tcpdump` command or programmatically. 62 | Pcap file should contain raw lidar_packets and `meta` file (i.e. metadata.json) 63 | should be a corresponding sensor metadata stored at the time of pcap recording. 64 | 65 | 66 | NOTE: It's critical to have a metadata json stored in the same recording session 67 | as a pcap file, because pcap reader checks the `init_id` field in the UDP 68 | lidar_packets and expects it to match `initialization_id` 69 | in the metadata json, packets with different `init_id` just skipped. 70 | 71 | Metadata json can be obtainer with Ouster SDK: 72 | See examples here https://static.ouster.dev/sdk-docs/python/examples/basics-sensor.html#obtaining-sensor-metadata 73 | 74 | or with Sensor HTTP API endpoint GET /api/v1/sensor/metadata directly: 75 | See doc for details https://static.ouster.dev/sensor-docs/image_route1/image_route2/common_sections/API/http-api-v1.html#get-api-v1-sensor-metadata 76 | 77 | Args: 78 | data_dir: path to a pcap file (not a directory) 79 | meta: path to a metadata json file that should be recorded together with 80 | a pcap file. If `meta` is not provided attempts to find the best matching 81 | json file with the longest commong prefix of the pcap file (`data_dir`) in 82 | the same directory. 83 | """ 84 | 85 | try: 86 | import ouster.pcap as pcap 87 | from ouster import client 88 | except ImportError: 89 | print(f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"') 90 | exit(1) 91 | 92 | # since we import ouster-sdk's client module locally, we keep it locally as well 93 | self._client = client 94 | 95 | assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" 96 | 97 | # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity 98 | pcap_file = data_dir 99 | 100 | metadata_json = meta or find_metadata_json(pcap_file) 101 | if not metadata_json: 102 | print("Ouster pcap dataloader can't find metadata json file.") 103 | exit(1) 104 | print("Ouster pcap dataloader: using metadata json: ", metadata_json) 105 | 106 | self.data_dir = os.path.dirname(data_dir) 107 | 108 | with open(metadata_json) as json: 109 | self._info_json = json.read() 110 | self._info = client.SensorInfo(self._info_json) 111 | 112 | # lookup table for 2D range image projection to a 3D point cloud 113 | self._xyz_lut = client.XYZLut(self._info) 114 | 115 | self._pcap_file = str(data_dir) 116 | 117 | # read pcap file for the first pass to count scans 118 | print("Pre-reading Ouster pcap to count the scans number ...") 119 | self._source = pcap.Pcap(self._pcap_file, self._info) 120 | self._scans_num = sum((1 for _ in client.Scans(self._source))) 121 | print(f"Ouster pcap total scans number: {self._scans_num}") 122 | 123 | # cached timestamps array 124 | self._timestamps = np.empty(0) 125 | self._timestamps_initialized = False 126 | 127 | # start Scans iterator for consumption in __getitem__ 128 | self._source = pcap.Pcap(self._pcap_file, self._info) 129 | self._scans_iter = iter(client.Scans(self._source)) 130 | self._next_idx = 0 131 | 132 | def __getitem__(self, idx): 133 | # we assume that users always reads sequentially and do not 134 | # pass idx as for a random access collection 135 | assert self._next_idx == idx, ( 136 | "Ouster pcap dataloader supports only sequential reads. " 137 | f"Expected idx: {self._next_idx}, but got {idx}" 138 | ) 139 | scan = next(self._scans_iter) 140 | self._next_idx += 1 141 | 142 | if not self._timestamps_initialized: 143 | self._timestamps = np.tile(np.linspace(0, 1.0, scan.w, endpoint=False), (scan.h, 1)) 144 | self._timestamps_initialized = True 145 | 146 | # filtering our zero returns makes it substantially faster for kiss-icp 147 | sel_flag = scan.field(self._client.ChanField.RANGE) != 0 148 | xyz = self._xyz_lut(scan)[sel_flag] 149 | timestamps = self._timestamps[sel_flag] 150 | 151 | return xyz, timestamps 152 | 153 | def __len__(self): 154 | return self._scans_num 155 | -------------------------------------------------------------------------------- /src/mapmos/datasets/rosbag.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import os 24 | import sys 25 | from pathlib import Path 26 | from typing import Sequence 27 | 28 | import natsort 29 | 30 | 31 | class RosbagDataset: 32 | def __init__(self, data_dir: Sequence[Path], topic: str, *_, **__): 33 | """ROS1 / ROS2 bagfile dataloader. 34 | 35 | It can take either one ROS2 bag file or one or more ROS1 bag files belonging to a split bag. 36 | The reader will replay ROS1 split bags in correct timestamp order. 37 | 38 | TODO: Merge mcap and rosbag dataloaders into 1 39 | """ 40 | try: 41 | from rosbags.highlevel import AnyReader 42 | except ModuleNotFoundError: 43 | print('rosbags library not installed, run "pip install -U rosbags"') 44 | sys.exit(1) 45 | 46 | from kiss_icp.tools.point_cloud2 import read_point_cloud 47 | 48 | self.read_point_cloud = read_point_cloud 49 | 50 | # FIXME: This is quite hacky, trying to guess if we have multiple .bag, one or a dir 51 | if isinstance(data_dir, Path): 52 | self.sequence_id = os.path.basename(data_dir).split(".")[0] 53 | self.bag = AnyReader([data_dir]) 54 | else: 55 | self.sequence_id = os.path.basename(data_dir[0]).split(".")[0] 56 | self.bag = AnyReader(data_dir) 57 | print("Reading multiple .bag files in directory:") 58 | print("\n".join(natsort.natsorted([path.name for path in self.bag.paths]))) 59 | self.bag.open() 60 | self.topic = self.check_topic(topic) 61 | self.n_scans = self.bag.topics[self.topic].msgcount 62 | 63 | # limit connections to selected topic 64 | connections = [x for x in self.bag.connections if x.topic == self.topic] 65 | self.msgs = self.bag.messages(connections=connections) 66 | self.timestamps = [] 67 | 68 | # Visualization Options 69 | self.use_global_visualizer = True 70 | 71 | def __del__(self): 72 | if hasattr(self, "bag"): 73 | self.bag.close() 74 | 75 | def __len__(self): 76 | return self.n_scans 77 | 78 | def __getitem__(self, idx): 79 | connection, timestamp, rawdata = next(self.msgs) 80 | self.timestamps.append(self.to_sec(timestamp)) 81 | msg = self.bag.deserialize(rawdata, connection.msgtype) 82 | return self.read_point_cloud(msg) 83 | 84 | @staticmethod 85 | def to_sec(nsec: int): 86 | return float(nsec) / 1e9 87 | 88 | def get_frames_timestamps(self) -> list: 89 | return self.timestamps 90 | 91 | def check_topic(self, topic: str) -> str: 92 | # Extract all PointCloud2 msg topics from the bagfile 93 | point_cloud_topics = [ 94 | topic[0] 95 | for topic in self.bag.topics.items() 96 | if topic[1].msgtype == "sensor_msgs/msg/PointCloud2" 97 | ] 98 | 99 | def print_available_topics_and_exit(): 100 | print(50 * "-") 101 | for t in point_cloud_topics: 102 | print(f"--topic {t}") 103 | print(50 * "-") 104 | sys.exit(1) 105 | 106 | if topic and topic in point_cloud_topics: 107 | return topic 108 | # when user specified the topic check that exists 109 | if topic and topic not in point_cloud_topics: 110 | print( 111 | f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' 112 | "Please select one of the following topics with the --topic flag" 113 | ) 114 | print_available_topics_and_exit() 115 | if len(point_cloud_topics) > 1: 116 | print( 117 | "Multiple sensor_msgs/msg/PointCloud2 topics available." 118 | "Please select one of the following topics with the --topic flag" 119 | ) 120 | print_available_topics_and_exit() 121 | 122 | if len(point_cloud_topics) == 0: 123 | print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") 124 | if len(point_cloud_topics) == 1: 125 | return point_cloud_topics[0] 126 | -------------------------------------------------------------------------------- /src/mapmos/mapmos_net.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import copy 24 | 25 | import MinkowskiEngine as ME 26 | import torch 27 | from pytorch_lightning import LightningModule 28 | 29 | from mapmos.minkunet import CustomMinkUNet14 30 | 31 | 32 | class MapMOSNet(LightningModule): 33 | def __init__(self, voxel_size: float): 34 | super().__init__() 35 | self.voxel_size = voxel_size 36 | self.MinkUNet = CustomMinkUNet14(in_channels=1, out_channels=1, D=4) 37 | 38 | def predict(self, scan_input, map_input, scan_indices, map_indices): 39 | def extend(tensor, batch_idx, time_idx): 40 | ones = torch.ones(len(tensor), 1).type_as(tensor) 41 | return torch.hstack([batch_idx * ones, tensor, time_idx * ones]) 42 | 43 | # Extend to [batch_idx, x,y,z,t] 44 | scan_input = extend(scan_input, 0, 0) 45 | map_input = extend(map_input, 0, -1) 46 | 47 | coordinates = torch.vstack([scan_input.reshape(-1, 5), map_input.reshape(-1, 5)]) 48 | indices = torch.vstack([scan_indices.reshape(-1, 1), map_indices.reshape(-1, 1)]) 49 | 50 | logits = self.forward(coordinates, indices) 51 | 52 | # Get logits from current scan 53 | mask_scan = coordinates[:, 4] == 0.0 54 | logits_scan = logits[mask_scan] 55 | logits_map = logits[~mask_scan] 56 | return logits_scan, logits_map 57 | 58 | def forward(self, coordinates: torch.Tensor, indices: torch.Tensor): 59 | quantization = torch.Tensor( 60 | [1.0, self.voxel_size, self.voxel_size, self.voxel_size, 1.0] 61 | ).type_as(coordinates) 62 | coordinates = torch.div(coordinates, quantization) 63 | 64 | # Normalize indices 65 | i_max = torch.max(indices) 66 | i_min = torch.min(indices) 67 | if i_min == i_max: 68 | features = 1.0 * torch.ones_like(indices) 69 | else: 70 | features = 1 + (i_max - indices) / (i_max - i_min) 71 | 72 | tensor_field = ME.TensorField( 73 | features=features.reshape(-1, 1), coordinates=coordinates.reshape(-1, 5) 74 | ) 75 | 76 | sparse_tensor = tensor_field.sparse() 77 | 78 | predicted_sparse_tensor = self.MinkUNet(sparse_tensor) 79 | out = predicted_sparse_tensor.slice(tensor_field) 80 | 81 | logits = out.features.reshape(-1) 82 | return logits 83 | 84 | @staticmethod 85 | def to_label(logits): 86 | labels = copy.deepcopy(logits) 87 | mask = logits > 0 88 | labels[mask] = 1.0 89 | labels[~mask] = 0.0 90 | return labels 91 | -------------------------------------------------------------------------------- /src/mapmos/mapping.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | 25 | from mapmos.pybind import mapmos_pybind 26 | 27 | 28 | class VoxelHashMap: 29 | def __init__( 30 | self, 31 | voxel_size: float, 32 | max_distance: float, 33 | max_points_per_voxel: int = 1, 34 | ): 35 | self._internal_map = mapmos_pybind._VoxelHashMap( 36 | voxel_size=voxel_size, 37 | max_distance=max_distance, 38 | max_points_per_voxel=max_points_per_voxel, 39 | ) 40 | 41 | def clear(self): 42 | return self._internal_map._clear() 43 | 44 | def empty(self): 45 | return self._internal_map._empty() 46 | 47 | def update(self, points: np.ndarray, pose: np.ndarray, timestamp: int): 48 | self._internal_map._update(mapmos_pybind._Vector3dVector(points), pose, timestamp) 49 | 50 | def point_cloud_with_timestamps(self): 51 | map_points, map_timestamps = self._internal_map._point_cloud_with_timestamps() 52 | return np.asarray(map_points), np.asarray(map_timestamps) 53 | 54 | def voxels_with_belief(self): 55 | voxels, belief = self._internal_map._voxels_with_belief() 56 | return np.asarray(voxels), np.asarray(belief) 57 | 58 | def remove_voxels_far_from_location(self, location: np.ndarray): 59 | self._internal_map._remove_far_away_points(location) 60 | 61 | def update_belief(self, points: np.ndarray, logits: np.ndarray): 62 | self._internal_map._update_belief(mapmos_pybind._Vector3dVector(points), logits.ravel()) 63 | 64 | def get_belief(self, points: np.ndarray): 65 | belief = self._internal_map._get_belief(mapmos_pybind._Vector3dVector(points)) 66 | return np.asarray(belief) 67 | -------------------------------------------------------------------------------- /src/mapmos/metrics.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import torch 24 | 25 | 26 | def get_confusion_matrix(pred_labels, gt_labels): 27 | # Mask valid values -1 28 | valid_mask = gt_labels != -1 29 | pred_labels = pred_labels[valid_mask] 30 | gt_labels = gt_labels[valid_mask] 31 | 32 | idxs = torch.stack([pred_labels, gt_labels], dim=0).long() 33 | ones = torch.ones((idxs.shape[-1])).type_as(gt_labels) 34 | confusion_matrix = torch.zeros(2, 2).type_as(gt_labels) 35 | confusion_matrix = confusion_matrix.index_put_(tuple(idxs), ones, accumulate=True) 36 | return confusion_matrix 37 | 38 | 39 | def get_stats(confusion_matrix): 40 | tp = confusion_matrix.diag() 41 | fp = confusion_matrix.sum(dim=1) - tp 42 | fn = confusion_matrix.sum(dim=0) - tp 43 | return tp, fp, fn 44 | 45 | 46 | def get_iou(confusion_matrix): 47 | tp, fp, fn = get_stats(confusion_matrix) 48 | intersection = tp 49 | union = tp + fp + fn + 1e-15 50 | iou = intersection / union 51 | 52 | # If no GT labels, set to NaN 53 | iou[tp + fn == 0] = float("nan") 54 | 55 | return iou 56 | 57 | 58 | def get_precision(confusion_matrix): 59 | tp, fp, fn = get_stats(confusion_matrix) 60 | prec = tp / (tp + fp) 61 | 62 | # If no GT labels, set to NaN 63 | prec[tp + fn == 0] = float("nan") 64 | 65 | return prec 66 | 67 | 68 | def get_recall(confusion_matrix): 69 | tp, _, fn = get_stats(confusion_matrix) 70 | rec = tp / (tp + fn) 71 | 72 | # If no GT labels, set to NaN 73 | rec[tp + fn == 0] = float("nan") 74 | 75 | return rec 76 | 77 | 78 | def get_f1(confusion_matrix): 79 | if torch.sum(confusion_matrix) == 0: 80 | return torch.tensor([float("nan"), float("nan")]) 81 | prec = get_precision(confusion_matrix) 82 | rec = get_recall(confusion_matrix) 83 | f1 = 2 * prec * rec / (prec + rec) 84 | 85 | # If no GT labels or zero Prec and Recall, set to NaN 86 | f1[(prec == -1) | (rec == -1)] = float("nan") 87 | f1[prec + rec == 0] = float("nan") 88 | 89 | return f1 90 | 91 | 92 | def get_acc(confusion_matrix): 93 | if torch.sum(confusion_matrix) == 0: 94 | return torch.tensor([-1, -1]) 95 | tp, fp, _ = get_stats(confusion_matrix) 96 | total_tp = tp.sum() 97 | total = tp.sum() + fp.sum() + 1e-15 98 | acc_mean = total_tp / total 99 | return acc_mean 100 | -------------------------------------------------------------------------------- /src/mapmos/odometry.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | from kiss_icp.config import KISSConfig 25 | from kiss_icp.kiss_icp import KissICP, get_registration 26 | 27 | from mapmos.config import DataConfig, OdometryConfig 28 | from mapmos.mapping import VoxelHashMap 29 | from mapmos.pybind import mapmos_pybind 30 | from mapmos.registration import get_registration 31 | 32 | 33 | def parse_config(config_data: DataConfig, config_odometry: OdometryConfig): 34 | kiss_config = KISSConfig() 35 | kiss_config.data.deskew = config_data.deskew 36 | kiss_config.data.max_range = config_data.max_range 37 | kiss_config.data.min_range = config_data.min_range 38 | kiss_config.mapping.voxel_size = config_odometry.voxel_size 39 | kiss_config.mapping.max_points_per_voxel = config_odometry.max_points_per_voxel 40 | kiss_config.adaptive_threshold.initial_threshold = config_odometry.initial_threshold 41 | kiss_config.adaptive_threshold.min_motion_th = config_odometry.min_motion_th 42 | return kiss_config 43 | 44 | 45 | class Odometry(KissICP): 46 | def __init__( 47 | self, 48 | config_data: DataConfig, 49 | config_odometry: OdometryConfig, 50 | ): 51 | kiss_config = parse_config(config_data, config_odometry) 52 | super().__init__(kiss_config) 53 | 54 | self.local_map = VoxelHashMap( 55 | voxel_size=self.config.mapping.voxel_size, 56 | max_distance=self.config.data.max_range, 57 | max_points_per_voxel=self.config.mapping.max_points_per_voxel, 58 | ) 59 | self.registration = get_registration(kiss_config) 60 | 61 | def register_points(self, points, timestamps, scan_index): 62 | # Apply motion compensation 63 | points_prep = self.preprocessor.preprocess(points, timestamps, self.last_delta) 64 | 65 | # Voxelize 66 | source, points_downsample = self.voxelize(points_prep) 67 | 68 | # Get motion prediction and adaptive_threshold 69 | sigma = self.adaptive_threshold.get_threshold() 70 | 71 | # Compute initial_guess for ICP 72 | initial_guess = self.last_pose @ self.last_delta 73 | 74 | new_pose = self.registration.align_points_to_map( 75 | points=source, 76 | voxel_map=self.local_map, 77 | initial_guess=initial_guess, 78 | max_correspondance_distance=3 * sigma, 79 | kernel=sigma / 3, 80 | ) 81 | 82 | point_deskewed = self.deskew(points, timestamps, self.last_delta) 83 | 84 | # Compute the difference between the prediction and the actual estimate 85 | model_deviation = np.linalg.inv(initial_guess) @ new_pose 86 | 87 | # Update step: threshold, local map, delta, and the last pose 88 | self.adaptive_threshold.update_model_deviation(model_deviation) 89 | self.local_map.update(points_downsample, new_pose, scan_index) 90 | self.last_delta = np.linalg.inv(self.last_pose) @ new_pose 91 | self.last_pose = new_pose 92 | 93 | return self.transform(point_deskewed, self.last_pose) 94 | 95 | def get_map_points(self): 96 | map_points, map_timestamps = self.local_map.point_cloud_with_timestamps() 97 | return map_points.reshape(-1, 3), map_timestamps.reshape(-1, 1) 98 | 99 | def current_location(self): 100 | return self.last_pose[:3, 3] 101 | 102 | def transform(self, points, pose): 103 | points_hom = np.hstack((points, np.ones((len(points), 1)))) 104 | points = (pose @ points_hom.T).T[:, :3] 105 | return points 106 | 107 | def deskew(self, points, timestamps, relative_motion): 108 | return ( 109 | np.asarray( 110 | mapmos_pybind._deskew( 111 | frame=mapmos_pybind._Vector3dVector(points), 112 | timestamps=timestamps.ravel(), 113 | relative_motion=relative_motion, 114 | ) 115 | ) 116 | if self.config.data.deskew 117 | else points 118 | ) 119 | -------------------------------------------------------------------------------- /src/mapmos/paper_pipeline.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import time 24 | from pathlib import Path 25 | from typing import Optional 26 | 27 | import numpy as np 28 | import torch 29 | from tqdm.auto import trange 30 | 31 | from mapmos.mapping import VoxelHashMap 32 | from mapmos.metrics import get_confusion_matrix 33 | from mapmos.pipeline import MapMOSPipeline 34 | 35 | 36 | class PaperPipeline(MapMOSPipeline): 37 | def __init__( 38 | self, 39 | dataset, 40 | weights: Path, 41 | config: Optional[Path] = None, 42 | visualize: bool = False, 43 | save_ply: bool = False, 44 | save_kitti: bool = False, 45 | n_scans: int = -1, 46 | jump: int = 0, 47 | ): 48 | super().__init__( 49 | dataset=dataset, 50 | weights=weights, 51 | config=config, 52 | visualize=visualize, 53 | save_ply=save_ply, 54 | save_kitti=save_kitti, 55 | n_scans=n_scans, 56 | jump=jump, 57 | ) 58 | self.belief_scan_only = VoxelHashMap( 59 | voxel_size=self.config.mos.voxel_size_belief, 60 | max_distance=self.config.mos.max_range_belief, 61 | ) 62 | 63 | self.times_belief_scan_only = [] 64 | 65 | self.confusion_matrix_mos = torch.zeros(2, 2) 66 | self.confusion_matrix_belief_scan_only = torch.zeros(2, 2) 67 | self.confusion_matrix_belief_no_delay = torch.zeros(2, 2) 68 | 69 | # Private interface ------ 70 | def _run_pipeline(self): 71 | pbar = trange(self._first, self._last, unit=" frames", dynamic_ncols=True) 72 | for scan_index in pbar: 73 | local_scan, timestamps, gt_labels = self._next(scan_index) 74 | map_points, map_indices = self.odometry.get_map_points() 75 | 76 | scan_points = self.odometry.register_points(local_scan, timestamps, scan_index) 77 | self.poses[scan_index - self._first] = self.odometry.last_pose 78 | 79 | min_range_mos = self.config.mos.min_range_mos 80 | max_range_mos = self.config.mos.max_range_mos 81 | scan_mask = self._preprocess(scan_points, min_range_mos, max_range_mos) 82 | scan_points = torch.tensor(scan_points[scan_mask], dtype=torch.float32, device="cuda") 83 | gt_labels = gt_labels[scan_mask] 84 | 85 | map_mask = self._preprocess(map_points, min_range_mos, max_range_mos) 86 | map_points = torch.tensor(map_points[map_mask], dtype=torch.float32, device="cuda") 87 | map_indices = torch.tensor(map_indices[map_mask], dtype=torch.float32, device="cuda") 88 | 89 | start_time = time.perf_counter_ns() 90 | pred_logits_scan, pred_logits_map = self.model.predict( 91 | scan_points, 92 | map_points, 93 | scan_index * torch.ones(len(scan_points)).type_as(scan_points), 94 | map_indices, 95 | ) 96 | self.times_mos.append(time.perf_counter_ns() - start_time) 97 | 98 | # Detach, move to CPU 99 | pred_logits_scan = pred_logits_scan.detach().cpu().numpy().astype(np.float64) 100 | pred_logits_map = pred_logits_map.detach().cpu().numpy().astype(np.float64) 101 | scan_points = scan_points.cpu().numpy().astype(np.float64) 102 | map_points = map_points.cpu().numpy().astype(np.float64) 103 | torch.cuda.empty_cache() 104 | 105 | pred_labels_scan = self.model.to_label(pred_logits_scan) 106 | pred_labels_map = self.model.to_label(pred_logits_map) 107 | 108 | pred_labels_scan = ( 109 | np.zeros_like(pred_logits_scan) 110 | if len(map_points) == 0 111 | else self.model.to_label(pred_logits_scan) 112 | ) 113 | self.confusion_matrix_mos += get_confusion_matrix( 114 | torch.tensor(pred_labels_scan, dtype=torch.int32), 115 | torch.tensor(gt_labels, dtype=torch.int32), 116 | ) 117 | 118 | # Probabilistic volumetric fusion with scan prediction only 119 | scan_mask_belief = self._preprocess(scan_points, 0.0, self.config.mos.max_range_belief) 120 | start_time = time.perf_counter_ns() 121 | self.belief_scan_only.update_belief( 122 | scan_points[scan_mask_belief], pred_logits_scan[scan_mask_belief] 123 | ) 124 | belief = self.belief_scan_only.get_belief(scan_points) 125 | self.times_belief_scan_only.append(time.perf_counter_ns() - start_time) 126 | belief_labels = ( 127 | np.zeros_like(belief) if len(map_points) == 0 else self.model.to_label(belief) 128 | ) 129 | self.confusion_matrix_belief_scan_only += get_confusion_matrix( 130 | torch.tensor(belief_labels, dtype=torch.int32), 131 | torch.tensor(gt_labels, dtype=torch.int32), 132 | ) 133 | 134 | # Probabilistic Volumetric Fusion of predictions within the belief range 135 | map_mask_belief = pred_logits_map > 0 136 | map_mask_belief = np.logical_and( 137 | map_mask_belief, self._preprocess(map_points, 0.0, self.config.mos.max_range_belief) 138 | ) 139 | scan_mask_belief = self._preprocess(scan_points, 0.0, self.config.mos.max_range_belief) 140 | points_stacked = np.vstack([scan_points[scan_mask_belief], map_points[map_mask_belief]]) 141 | logits_stacked = np.vstack( 142 | [ 143 | pred_logits_scan[scan_mask_belief].reshape(-1, 1), 144 | pred_logits_map[map_mask_belief].reshape(-1, 1), 145 | ] 146 | ).reshape(-1) 147 | 148 | start_time = time.perf_counter_ns() 149 | self.belief.update_belief(points_stacked, logits_stacked) 150 | belief_with_map = self.belief.get_belief(scan_points) 151 | self.times_belief.append(time.perf_counter_ns() - start_time) 152 | belief_labels_with_map = ( 153 | np.zeros_like(belief_with_map) 154 | if len(map_points) == 0 155 | else self.model.to_label(belief_with_map) 156 | ) 157 | 158 | self.confusion_matrix_belief_no_delay += get_confusion_matrix( 159 | torch.tensor(belief_labels_with_map, dtype=torch.int32), 160 | torch.tensor(gt_labels, dtype=torch.int32), 161 | ) 162 | 163 | self.visualizer.update( 164 | scan_points, 165 | map_points, 166 | pred_labels_scan, 167 | pred_labels_map, 168 | self.belief, 169 | self.odometry.last_pose, 170 | ) 171 | 172 | # Probabilistic volumetric fusion with scan and moving map predictions and delay 173 | self.buffer.append([scan_index, scan_points, gt_labels]) 174 | if len(self.buffer) == self.buffer.maxlen: 175 | query_index, query_points, query_labels = self.buffer.popleft() 176 | self.process_final_prediction(query_index, query_points, query_labels) 177 | 178 | # Clean up 179 | self.belief_scan_only.remove_voxels_far_from_location(self.odometry.current_location()) 180 | self.belief.remove_voxels_far_from_location(self.odometry.current_location()) 181 | 182 | # Clear buffer at the end 183 | while len(self.buffer) != 0: 184 | query_index, query_points, query_labels = self.buffer.popleft() 185 | self.process_final_prediction(query_index, query_points, query_labels) 186 | 187 | def _run_evaluation(self): 188 | if self.has_gt: 189 | self.results.eval_odometry(self.poses, self.gt_poses) 190 | self.results.eval_mos(self.confusion_matrix_mos, desc="\nScan Prediction") 191 | self.results.eval_fps(self.times_mos, desc="Average Frequency MOS") 192 | self.results.eval_mos(self.confusion_matrix_belief_scan_only, desc="\nBelief, Scan Only") 193 | self.results.eval_fps( 194 | self.times_belief_scan_only, desc="Average Frequency Belief, Scan Only" 195 | ) 196 | self.results.eval_mos(self.confusion_matrix_belief_no_delay, desc="\nBelief, No Delay ") 197 | self.results.eval_mos( 198 | self.confusion_matrix_belief, 199 | desc="\nBelief", 200 | ) 201 | self.results.eval_fps(self.times_belief, desc="Average Frequency Belief") 202 | -------------------------------------------------------------------------------- /src/mapmos/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 | -------------------------------------------------------------------------------- /src/mapmos/pybind/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | # TODO: Yet another manual release dne by nacho. This should be updated whenever the Eigen team 25 | # release a new version that is not 3.4. That version does not include this necessary changes: 26 | # - https://gitlab.com/libeigen/eigen/-/merge_requests/893/diffs 27 | 28 | set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") 29 | set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") 30 | set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") 31 | set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") 32 | set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") 33 | 34 | include(FetchContent) 35 | FetchContent_Declare(eigen SYSTEM URL https://github.com/nachovizzo/eigen/archive/refs/tags/3.4.90.tar.gz) 36 | FetchContent_MakeAvailable(eigen) 37 | 38 | if(${CMAKE_VERSION} VERSION_LESS 3.25) 39 | get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) 40 | set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") 41 | endif() 42 | -------------------------------------------------------------------------------- /src/mapmos/pybind/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | # Silence timestamp warning 25 | if(CMAKE_VERSION VERSION_GREATER 3.24) 26 | cmake_policy(SET CMP0135 OLD) 27 | endif() 28 | 29 | if(USE_SYSTEM_EIGEN3) 30 | find_package(Eigen3 QUIET NO_MODULE) 31 | endif() 32 | if(NOT USE_SYSTEM_EIGEN3 OR NOT TARGET Eigen3::Eigen) 33 | set(USE_SYSTEM_EIGEN3 OFF) 34 | include(${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake) 35 | endif() 36 | 37 | if(USE_SYSTEM_SOPHUS) 38 | find_package(Sophus QUIET NO_MODULE) 39 | endif() 40 | if(NOT USE_SYSTEM_SOPHUS OR NOT TARGET Sophus::Sophus) 41 | set(USE_SYSTEM_SOPHUS OFF) 42 | include(${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake) 43 | endif() 44 | 45 | # tbb needs to be statically linked, so, also do it always :) 46 | if(USE_SYSTEM_TBB) 47 | find_package(TBB QUIET NO_MODULE) 48 | endif() 49 | if(NOT USE_SYSTEM_TBB OR NOT TARGET TBB::tbb) 50 | set(USE_SYSTEM_TBB OFF) 51 | include(${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake) 52 | endif() 53 | 54 | if(USE_SYSTEM_TSLMAP) 55 | find_package(tsl-robin-map QUIET NO_MODULE) 56 | endif() 57 | if(NOT USE_SYSTEM_TSLMAP OR NOT TARGET tsl::robin_map) 58 | set(USE_SYSTEM_TSLMAP OFF) 59 | include(${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake) 60 | endif() 61 | -------------------------------------------------------------------------------- /src/mapmos/pybind/3rdparty/sophus/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2015 Jesse Beder. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWAR 20 | -------------------------------------------------------------------------------- /src/mapmos/pybind/3rdparty/sophus/sophus.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of Bonn 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | include(FetchContent) 23 | 24 | set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") 25 | set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") 26 | set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") 27 | 28 | FetchContent_Declare(sophus SYSTEM URL https://github.com/strasdat/Sophus/archive/refs/tags/1.24.6.tar.gz) 29 | FetchContent_MakeAvailable(sophus) 30 | -------------------------------------------------------------------------------- /src/mapmos/pybind/3rdparty/tbb/tbb.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | option(BUILD_SHARED_LIBS OFF) 24 | option(TBBMALLOC_BUILD OFF) 25 | option(TBB_EXAMPLES OFF) 26 | option(TBB_STRICT OFF) 27 | option(TBB_TEST OFF) 28 | 29 | include(FetchContent) 30 | FetchContent_Declare(tbb URL https://github.com/uxlfoundation/oneTBB/archive/refs/tags/v2022.1.0.tar.gz) 31 | FetchContent_GetProperties(tbb) 32 | if(NOT tbb_POPULATED) 33 | FetchContent_Populate(tbb) 34 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 35 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 36 | else() 37 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 38 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 39 | add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) 40 | get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) 41 | set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") 42 | endif() 43 | endif() 44 | -------------------------------------------------------------------------------- /src/mapmos/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 | -------------------------------------------------------------------------------- /src/mapmos/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 URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.4.0.tar.gz) 25 | FetchContent_MakeAvailable(tessil) 26 | -------------------------------------------------------------------------------- /src/mapmos/pybind/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(mapmos_cpp VERSION 1.0.0 LANGUAGES CXX) 25 | 26 | # Setup build options 27 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 28 | option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) 29 | option(USE_SYSTEM_TSLMAP "Use system pre-installed tsl_robin" ON) 30 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 31 | 32 | # Set build type (repeat here for C++ only consumers) 33 | set(CMAKE_BUILD_TYPE Release) 34 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 35 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 36 | 37 | include(3rdparty/find_dependencies.cmake) 38 | 39 | set(PYBIND11_NEWPYTHON ON) 40 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 41 | find_package(pybind11 CONFIG REQUIRED) 42 | 43 | # Python bindings 44 | pybind11_add_module(mapmos_pybind MODULE mapmos_pybind.cpp VoxelHashMap.cpp Registration.cpp Deskew.cpp) 45 | target_compile_features(mapmos_pybind PRIVATE cxx_std_17) 46 | target_link_libraries(mapmos_pybind PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) 47 | install(TARGETS mapmos_pybind LIBRARY DESTINATION .) 48 | -------------------------------------------------------------------------------- /src/mapmos/pybind/Deskew.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Deskew.hpp" 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace mapmos { 35 | 36 | std::vector Deskew(const std::vector &frame, 37 | const std::vector ×tamps, 38 | const Sophus::SE3d &relative_motion) { 39 | const std::vector &deskewed_frame = [&]() { 40 | if (timestamps.empty()) { 41 | return frame; 42 | } else { 43 | const auto &[min, max] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); 44 | const double min_time = *min; 45 | const double max_time = *max; 46 | const auto normalize = [&](const double t) { 47 | return (t - min_time) / (max_time - min_time); 48 | }; 49 | const auto &omega = relative_motion.log(); 50 | std::vector deskewed_frame(frame.size()); 51 | tbb::parallel_for( 52 | // Index Range 53 | tbb::blocked_range{0, deskewed_frame.size()}, 54 | // Parallel Compute 55 | [&](const tbb::blocked_range &r) { 56 | for (size_t idx = r.begin(); idx < r.end(); ++idx) { 57 | const auto &point = frame.at(idx); 58 | const auto &stamp = normalize(timestamps.at(idx)); 59 | const auto pose = Sophus::SE3d::exp((stamp - 1.0) * omega); 60 | deskewed_frame.at(idx) = pose * point; 61 | }; 62 | }); 63 | return deskewed_frame; 64 | } 65 | }(); 66 | return deskewed_frame; 67 | } 68 | 69 | } // namespace mapmos 70 | -------------------------------------------------------------------------------- /src/mapmos/pybind/Deskew.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace mapmos { 32 | 33 | std::vector Deskew(const std::vector &frame, 34 | const std::vector ×tamps, 35 | const Sophus::SE3d &relative_motion); 36 | 37 | } // namespace mapmos 38 | -------------------------------------------------------------------------------- /src/mapmos/pybind/Registration.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Registration.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "VoxelHashMap.hpp" 39 | 40 | namespace Eigen { 41 | using Matrix6d = Eigen::Matrix; 42 | using Matrix3_6d = Eigen::Matrix; 43 | using Vector6d = Eigen::Matrix; 44 | } // namespace Eigen 45 | 46 | using Correspondences = std::vector>; 47 | using LinearSystem = std::pair; 48 | 49 | namespace { 50 | inline double square(double x) { return x * x; } 51 | 52 | void TransformPoints(const Sophus::SE3d &T, std::vector &points) { 53 | std::transform(points.cbegin(), points.cend(), points.begin(), 54 | [&](const auto &point) { return T * point; }); 55 | } 56 | 57 | using Voxel = mapmos::VoxelHashMap::Voxel; 58 | std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { 59 | std::vector voxel_neighborhood; 60 | for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { 61 | for (int j = voxel.y() - adjacent_voxels; j < voxel.y() + adjacent_voxels + 1; ++j) { 62 | for (int k = voxel.z() - adjacent_voxels; k < voxel.z() + adjacent_voxels + 1; ++k) { 63 | voxel_neighborhood.emplace_back(i, j, k); 64 | } 65 | } 66 | } 67 | return voxel_neighborhood; 68 | } 69 | 70 | std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, 71 | const mapmos::VoxelHashMap &voxel_map) { 72 | // Convert the point to voxel coordinates 73 | const auto &voxel = voxel_map.PointToVoxel(point); 74 | // Get nearby voxels on the map 75 | const auto &query_voxels = GetAdjacentVoxels(voxel); 76 | // Extract the points contained within the neighborhood voxels 77 | const auto &neighbors = voxel_map.GetPoints(query_voxels); 78 | 79 | // Find the nearest neighbor 80 | Eigen::Vector3d closest_neighbor; 81 | double closest_distance = std::numeric_limits::max(); 82 | std::for_each(neighbors.cbegin(), neighbors.cend(), [&](const auto &neighbor) { 83 | double distance = (neighbor - point).norm(); 84 | if (distance < closest_distance) { 85 | closest_neighbor = neighbor; 86 | closest_distance = distance; 87 | } 88 | }); 89 | return std::make_tuple(closest_neighbor, closest_distance); 90 | } 91 | 92 | Correspondences DataAssociation(const std::vector &points, 93 | const mapmos::VoxelHashMap &voxel_map, 94 | const double max_correspondance_distance) { 95 | using points_iterator = std::vector::const_iterator; 96 | Correspondences correspondences; 97 | correspondences.reserve(points.size()); 98 | correspondences = tbb::parallel_reduce( 99 | // Range 100 | tbb::blocked_range{points.cbegin(), points.cend()}, 101 | // Identity 102 | correspondences, 103 | // 1st lambda: Parallel computation 104 | [&](const tbb::blocked_range &r, Correspondences res) -> Correspondences { 105 | res.reserve(r.size()); 106 | std::for_each(r.begin(), r.end(), [&](const auto &point) { 107 | const auto &[closest_neighbor, distance] = GetClosestNeighbor(point, voxel_map); 108 | if (distance < max_correspondance_distance) { 109 | res.emplace_back(point, closest_neighbor); 110 | } 111 | }); 112 | return res; 113 | }, 114 | // 2nd lambda: Parallel reduction 115 | [](Correspondences a, const Correspondences &b) -> Correspondences { 116 | a.insert(a.end(), // 117 | std::make_move_iterator(b.cbegin()), // 118 | std::make_move_iterator(b.cend())); 119 | return a; 120 | }); 121 | 122 | return correspondences; 123 | } 124 | 125 | LinearSystem BuildLinearSystem(const Correspondences &correspondences, const double kernel_scale) { 126 | auto compute_jacobian_and_residual = [](const auto &correspondence) { 127 | const auto &[source, target] = correspondence; 128 | const Eigen::Vector3d residual = source - target; 129 | Eigen::Matrix3_6d J_r; 130 | J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 131 | J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source); 132 | return std::make_tuple(J_r, residual); 133 | }; 134 | 135 | auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { 136 | a.first += b.first; 137 | a.second += b.second; 138 | return a; 139 | }; 140 | 141 | auto GM_weight = [&](const double &residual2) { 142 | return square(kernel_scale) / square(kernel_scale + residual2); 143 | }; 144 | 145 | using correspondence_iterator = Correspondences::const_iterator; 146 | const auto &[JTJ, JTr] = tbb::parallel_reduce( 147 | // Range 148 | tbb::blocked_range{correspondences.cbegin(), 149 | correspondences.cend()}, 150 | // Identity 151 | LinearSystem(Eigen::Matrix6d::Zero(), Eigen::Vector6d::Zero()), 152 | // 1st Lambda: Parallel computation 153 | [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { 154 | return std::transform_reduce( 155 | r.begin(), r.end(), J, sum_linear_systems, [&](const auto &correspondence) { 156 | const auto &[J_r, residual] = compute_jacobian_and_residual(correspondence); 157 | const double w = GM_weight(residual.squaredNorm()); 158 | return LinearSystem(J_r.transpose() * w * J_r, // JTJ 159 | J_r.transpose() * w * residual); // JTr 160 | }); 161 | }, 162 | // 2nd Lambda: Parallel reduction of the private Jacboians 163 | sum_linear_systems); 164 | 165 | return {JTJ, JTr}; 166 | } 167 | } // namespace 168 | 169 | namespace mapmos { 170 | 171 | Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads) 172 | : max_num_iterations_(max_num_iteration), 173 | convergence_criterion_(convergence_criterion), 174 | // Only manipulate the number of threads if the user specifies something greater than 0 175 | max_num_threads_(max_num_threads > 0 ? max_num_threads 176 | : tbb::this_task_arena::max_concurrency()) { 177 | // This global variable requires static duration storage to be able to manipulate the max 178 | // concurrency from TBB across the entire class 179 | static const auto tbb_control_settings = tbb::global_control( 180 | tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); 181 | } 182 | 183 | Sophus::SE3d Registration::AlignPointsToMap(const std::vector &frame, 184 | const VoxelHashMap &voxel_map, 185 | const Sophus::SE3d &initial_guess, 186 | const double max_distance, 187 | const double kernel_scale) { 188 | if (voxel_map.Empty()) return initial_guess; 189 | 190 | // Equation (9) 191 | std::vector source = frame; 192 | TransformPoints(initial_guess, source); 193 | 194 | // ICP-loop 195 | Sophus::SE3d T_icp = Sophus::SE3d(); 196 | for (int j = 0; j < max_num_iterations_; ++j) { 197 | // Equation (10) 198 | const auto correspondences = DataAssociation(source, voxel_map, max_distance); 199 | // Equation (11) 200 | const auto &[JTJ, JTr] = BuildLinearSystem(correspondences, kernel_scale); 201 | const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); 202 | const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); 203 | // Equation (12) 204 | TransformPoints(estimation, source); 205 | // Update iterations 206 | T_icp = estimation * T_icp; 207 | // Termination criteria 208 | if (dx.norm() < convergence_criterion_) break; 209 | } 210 | // Spit the final transformation 211 | return T_icp * initial_guess; 212 | } 213 | 214 | } // namespace mapmos 215 | -------------------------------------------------------------------------------- /src/mapmos/pybind/Registration.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "VoxelHashMap.hpp" 30 | 31 | namespace mapmos { 32 | 33 | struct Registration { 34 | explicit Registration(int max_num_iteration, double convergence_criterion, int max_num_threads); 35 | 36 | Sophus::SE3d AlignPointsToMap(const std::vector &frame, 37 | const VoxelHashMap &voxel_map, 38 | const Sophus::SE3d &initial_guess, 39 | const double max_correspondence_distance, 40 | const double kernel_scale); 41 | 42 | int max_num_iterations_; 43 | double convergence_criterion_; 44 | int max_num_threads_; 45 | }; 46 | } // namespace mapmos 47 | -------------------------------------------------------------------------------- /src/mapmos/pybind/VoxelHashMap.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "VoxelHashMap.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | namespace mapmos { 36 | 37 | std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { 38 | std::vector points; 39 | points.reserve(query_voxels.size() * static_cast(max_points_per_voxel_)); 40 | std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) { 41 | auto search = map_.find(query); 42 | if (search != map_.end()) { 43 | for (const auto &point : search->second.points) { 44 | points.emplace_back(point); 45 | } 46 | } 47 | }); 48 | return points; 49 | } 50 | 51 | std::vector VoxelHashMap::Pointcloud() const { 52 | std::vector points; 53 | points.reserve(max_points_per_voxel_ * map_.size()); 54 | for (const auto &[voxel, voxel_block] : map_) { 55 | (void)voxel; 56 | for (const auto &point : voxel_block.points) { 57 | points.push_back(point); 58 | } 59 | } 60 | return points; 61 | } 62 | 63 | std::tuple, std::vector> VoxelHashMap::PointcloudWithTimestamps() 64 | const { 65 | std::vector points; 66 | std::vector timestamps; 67 | points.reserve(max_points_per_voxel_ * map_.size()); 68 | for (const auto &[voxel, voxel_block] : map_) { 69 | (void)voxel; 70 | for (const auto &point : voxel_block.points) { 71 | points.push_back(point); 72 | } 73 | for (const auto ×tamp : voxel_block.timestamps) { 74 | timestamps.push_back(timestamp); 75 | } 76 | } 77 | return std::make_tuple(points, timestamps); 78 | } 79 | 80 | std::tuple, std::vector> VoxelHashMap::VoxelsWithBelief() 81 | const { 82 | std::vector voxels; 83 | std::vector belief; 84 | for (auto map_element : map_) { 85 | voxels.push_back(map_element.first); 86 | belief.push_back(map_element.second.belief.value_); 87 | } 88 | return make_tuple(voxels, belief); 89 | } 90 | 91 | void VoxelHashMap::Update(const std::vector &points, 92 | const Eigen::Vector3d &origin, 93 | const int timestamp) { 94 | AddPoints(points, timestamp); 95 | RemovePointsFarFromLocation(origin); 96 | } 97 | 98 | void VoxelHashMap::Update(const std::vector &points, 99 | const Sophus::SE3d &pose, 100 | const int timestamp) { 101 | std::vector points_transformed(points.size()); 102 | std::transform(points.cbegin(), points.cend(), points_transformed.begin(), 103 | [&](const auto &point) { return pose * point; }); 104 | const Eigen::Vector3d &origin = pose.translation(); 105 | Update(points_transformed, origin, timestamp); 106 | } 107 | 108 | void VoxelHashMap::AddPoints(const std::vector &points, const int timestamp) { 109 | std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { 110 | const auto voxel = PointToVoxel(point); 111 | auto search = map_.find(voxel); 112 | if (search != map_.end()) { 113 | auto &voxel_block = search.value(); 114 | voxel_block.AddPoint(point, timestamp); 115 | } else { 116 | map_.insert( 117 | {voxel, VoxelBlock{{point}, {timestamp}, {Belief{}}, max_points_per_voxel_}}); 118 | } 119 | }); 120 | } 121 | 122 | void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { 123 | const auto max_distance2 = max_distance_ * max_distance_; 124 | for (auto it = map_.begin(); it != map_.end();) { 125 | const auto &[voxel, voxel_block] = *it; 126 | Eigen::Vector3d pt(voxel[0] * voxel_size_, voxel[1] * voxel_size_, voxel[2] * voxel_size_); 127 | if ((pt - origin).squaredNorm() >= (max_distance2)) { 128 | it = map_.erase(it); 129 | } else { 130 | ++it; 131 | } 132 | } 133 | } 134 | 135 | void VoxelHashMap::UpdateBelief(const std::vector &points, 136 | const std::vector &updates) { 137 | std::vector voxels_to_update; 138 | voxels_to_update.reserve(points.size()); 139 | for (size_t i = 0; i < points.size(); i++) { 140 | auto voxel = PointToVoxel(points[i]); 141 | voxels_to_update.emplace_back(voxel); 142 | map_[voxel].belief.accumulatePartialUpdate(updates[i]); 143 | } 144 | 145 | tbb::parallel_for_each(voxels_to_update.cbegin(), voxels_to_update.cend(), 146 | [this](const auto &voxel) { map_[voxel].belief.update(); }); 147 | } 148 | 149 | std::vector VoxelHashMap::GetBelief(const std::vector &points) const { 150 | std::vector beliefs(points.size(), 0.0); 151 | std::transform(points.cbegin(), points.cend(), beliefs.begin(), [this](const auto &p) { 152 | auto voxel = PointToVoxel(p); 153 | if (map_.contains(voxel)) { 154 | return map_.at(voxel).belief.value_; 155 | } 156 | return 0.0; 157 | }); 158 | return beliefs; 159 | } 160 | 161 | } // namespace mapmos 162 | -------------------------------------------------------------------------------- /src/mapmos/pybind/VoxelHashMap.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | // 24 | // NOTE: This implementation is heavily inspired in the original CT-ICP VoxelHashMap implementation, 25 | // although it was heavily modifed and drastically simplified, but if you are using this module you 26 | // should at least acknoowledge the work from CT-ICP by giving a star on GitHub 27 | #pragma once 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace mapmos { 36 | struct Belief { 37 | void update() { 38 | value_ += this->num > 0 ? this->sum / this->num : 0.0; 39 | this->sum = 0.0; 40 | this->num = 0; 41 | } 42 | void accumulatePartialUpdate(const double &update) { 43 | this->sum += update; 44 | this->num++; 45 | } 46 | double value_ = 0.0; 47 | 48 | protected: 49 | double sum = 0.0; 50 | int num = 0; 51 | }; 52 | 53 | struct VoxelHashMap { 54 | using Voxel = Eigen::Vector3i; 55 | struct VoxelBlock { 56 | // buffer of points with a max limit of n_points 57 | std::vector points; 58 | std::vector timestamps; 59 | Belief belief; 60 | int num_points_; 61 | inline void AddPoint(const Eigen::Vector3d &point, int timestamp) { 62 | if (points.size() < static_cast(num_points_)) { 63 | points.push_back(point); 64 | timestamps.push_back(timestamp); 65 | } 66 | } 67 | }; 68 | struct VoxelHash { 69 | size_t operator()(const Voxel &voxel) const { 70 | const uint32_t *vec = reinterpret_cast(voxel.data()); 71 | return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); 72 | } 73 | }; 74 | 75 | explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel) 76 | : voxel_size_(voxel_size), 77 | max_distance_(max_distance), 78 | max_points_per_voxel_(max_points_per_voxel) {} 79 | 80 | inline void Clear() { map_.clear(); } 81 | inline bool Empty() const { return map_.empty(); } 82 | inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { 83 | return Voxel(static_cast(std::floor(point.x() / voxel_size_)), 84 | static_cast(std::floor(point.y() / voxel_size_)), 85 | static_cast(std::floor(point.z() / voxel_size_))); 86 | } 87 | void Update(const std::vector &points, 88 | const Eigen::Vector3d &origin, 89 | const int timestamp); 90 | void Update(const std::vector &points, 91 | const Sophus::SE3d &pose, 92 | const int timestamp); 93 | void AddPoints(const std::vector &points, const int timestamp); 94 | std::vector GetBelief(const std::vector &points) const; 95 | void UpdateBelief(const std::vector &points, 96 | const std::vector &updates); 97 | void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); 98 | std::vector Pointcloud() const; 99 | std::tuple, std::vector> PointcloudWithTimestamps() const; 100 | std::tuple, std::vector> VoxelsWithBelief() const; 101 | std::vector GetPoints(const std::vector &query_voxels) const; 102 | 103 | double voxel_size_; 104 | double max_distance_; 105 | int max_points_per_voxel_; 106 | tsl::robin_map map_; 107 | }; 108 | } // namespace mapmos 109 | -------------------------------------------------------------------------------- /src/mapmos/pybind/mapmos_pybind.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in 14 | // all 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 | 33 | #include "Deskew.hpp" 34 | #include "Registration.hpp" 35 | #include "VoxelHashMap.hpp" 36 | #include "stl_vector_eigen.h" 37 | 38 | namespace py = pybind11; 39 | using namespace py::literals; 40 | 41 | PYBIND11_MAKE_OPAQUE(std::vector); 42 | 43 | namespace mapmos { 44 | PYBIND11_MODULE(mapmos_pybind, m) { 45 | auto vector3dvector = pybind_eigen_vector_of_vector( 46 | m, "_Vector3dVector", "std::vector", 47 | py::py_array_to_vectors_double); 48 | 49 | m.def( 50 | "_deskew", 51 | [](const std::vector &points, const std::vector ×tamps, 52 | const Eigen::Matrix4d &relative_motion) { 53 | Sophus::SE3d motion(relative_motion); 54 | return Deskew(points, timestamps, motion); 55 | }, 56 | "frame"_a, "timestamps"_a, "relative_motion"_a); 57 | 58 | // Map representation 59 | py::class_ internal_map(m, "_VoxelHashMap", "Don't use this"); 60 | internal_map 61 | .def(py::init(), "voxel_size"_a, "max_distance"_a, 62 | "max_points_per_voxel"_a) 63 | .def("_clear", &VoxelHashMap::Clear) 64 | .def("_empty", &VoxelHashMap::Empty) 65 | .def("_update", 66 | py::overload_cast &, const Eigen::Vector3d &, 67 | const int>(&VoxelHashMap::Update), 68 | "points"_a, "origin"_a, "timestamp"_a) 69 | .def( 70 | "_update", 71 | [](VoxelHashMap &self, const std::vector &points, 72 | const Eigen::Matrix4d &T, const int timestamp) { 73 | Sophus::SE3d pose(T); 74 | self.Update(points, pose, timestamp); 75 | }, 76 | "points"_a, "pose"_a, "timestamp"_a) 77 | .def("_add_points", &VoxelHashMap::AddPoints, "points"_a, "timestamp"_a) 78 | .def("_remove_far_away_points", &VoxelHashMap::RemovePointsFarFromLocation, "origin"_a) 79 | .def("_point_cloud", &VoxelHashMap::Pointcloud) 80 | .def("_point_cloud_with_timestamps", &VoxelHashMap::PointcloudWithTimestamps) 81 | .def("_voxels_with_belief", &VoxelHashMap::VoxelsWithBelief) 82 | .def("_update_belief", &VoxelHashMap::UpdateBelief, "points"_a, "updates"_a) 83 | .def("_get_belief", &VoxelHashMap::GetBelief, "points"_a); 84 | 85 | // Point Cloud registration 86 | py::class_ internal_registration(m, "_Registration", "Don't use this"); 87 | internal_registration 88 | .def(py::init(), "max_num_iterations"_a, "convergence_criterion"_a, 89 | "max_num_threads"_a) 90 | .def( 91 | "_align_points_to_map", 92 | [](Registration &self, const std::vector &points, 93 | const VoxelHashMap &voxel_map, const Eigen::Matrix4d &T_guess, 94 | double max_correspondence_distance, double kernel) { 95 | Sophus::SE3d initial_guess(T_guess); 96 | return self 97 | .AlignPointsToMap(points, voxel_map, initial_guess, max_correspondence_distance, 98 | kernel) 99 | .matrix(); 100 | }, 101 | "points"_a, "voxel_map"_a, "initial_guess"_a, "max_correspondance_distance"_a, 102 | "kernel"_a); 103 | } 104 | } // namespace mapmos 105 | -------------------------------------------------------------------------------- /src/mapmos/pybind/stl_vector_eigen.h: -------------------------------------------------------------------------------- 1 | // ---------------------------------------------------------------------------- 2 | // NOTE: This fily has been adapted from the Open3D project, but copyright 3 | // still belongs to Open3D. All rights reserved 4 | // ---------------------------------------------------------------------------- 5 | // - Open3D: www.open3d.org - 6 | // ---------------------------------------------------------------------------- 7 | // The MIT License (MIT) 8 | // 9 | // Copyright (c) 2018-2021 www.open3d.org 10 | // 11 | // Permission is hereby granted, free of charge, to any person obtaining a copy 12 | // of this software and associated documentation files (the "Software"), to deal 13 | // in the Software without restriction, including without limitation the rights 14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | // copies of the Software, and to permit persons to whom the Software is 16 | // furnished to do so, subject to the following conditions: 17 | // 18 | // The above copyright notice and this permission notice shall be included in 19 | // all copies or substantial portions of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 26 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 27 | // IN THE SOFTWARE. 28 | // ---------------------------------------------------------------------------- 29 | #pragma once 30 | #include 31 | #include 32 | 33 | // pollute namespace with py 34 | #include 35 | namespace py = pybind11; 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace pybind11 { 43 | 44 | template , typename... Args> 45 | py::class_ bind_vector_without_repr(py::module &m, 46 | std::string const &name, 47 | Args &&...args) { 48 | // hack function to disable __repr__ for the convenient function 49 | // bind_vector() 50 | using Class_ = py::class_; 51 | Class_ cl(m, name.c_str(), std::forward(args)...); 52 | cl.def(py::init<>()); 53 | cl.def( 54 | "__bool__", [](const Vector &v) -> bool { return !v.empty(); }, 55 | "Check whether the list is nonempty"); 56 | cl.def("__len__", &Vector::size); 57 | return cl; 58 | } 59 | 60 | // - This function is used by Pybind for std::vector constructor. 61 | // This optional constructor is added to avoid too many Python <-> C++ API 62 | // calls when the vector size is large using the default biding method. 63 | // Pybind matches np.float64 array to py::array_t buffer. 64 | // - Directly using templates for the py::array_t and py::array_t 65 | // and etc. doesn't work. The current solution is to explicitly implement 66 | // bindings for each py array types. 67 | template 68 | std::vector py_array_to_vectors_double( 69 | py::array_t array) { 70 | int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; 71 | if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { 72 | throw py::cast_error(); 73 | } 74 | std::vector eigen_vectors(array.shape(0)); 75 | auto array_unchecked = array.mutable_unchecked<2>(); 76 | for (auto i = 0; i < array_unchecked.shape(0); ++i) { 77 | eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); 78 | } 79 | return eigen_vectors; 80 | } 81 | 82 | } // namespace pybind11 83 | 84 | template , 86 | typename holder_type = std::unique_ptr, 87 | typename InitFunc> 88 | py::class_ pybind_eigen_vector_of_vector(py::module &m, 89 | const std::string &bind_name, 90 | const std::string &repr_name, 91 | InitFunc init_func) { 92 | using Scalar = typename EigenVector::Scalar; 93 | auto vec = py::bind_vector_without_repr>( 94 | m, bind_name, py::buffer_protocol(), py::module_local()); 95 | vec.def(py::init(init_func)); 96 | vec.def_buffer([](std::vector &v) -> py::buffer_info { 97 | size_t rows = EigenVector::RowsAtCompileTime; 98 | return py::buffer_info(v.data(), sizeof(Scalar), py::format_descriptor::format(), 2, 99 | {v.size(), rows}, {sizeof(EigenVector), sizeof(Scalar)}); 100 | }); 101 | vec.def("__repr__", [repr_name](const std::vector &v) { 102 | return repr_name + std::string(" with ") + std::to_string(v.size()) + 103 | std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); 104 | }); 105 | vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); 106 | vec.def("__deepcopy__", 107 | [](std::vector &v) { return std::vector(v); }); 108 | 109 | // py::detail must be after custom constructor 110 | using Class_ = py::class_>; 111 | py::detail::vector_if_copy_constructible(vec); 112 | py::detail::vector_if_equal_operator(vec); 113 | py::detail::vector_modifiers(vec); 114 | py::detail::vector_accessor(vec); 115 | 116 | return vec; 117 | } 118 | -------------------------------------------------------------------------------- /src/mapmos/registration.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | import numpy as np 24 | from kiss_icp.config.parser import KISSConfig 25 | 26 | from mapmos.mapping import VoxelHashMap 27 | from mapmos.pybind import mapmos_pybind 28 | 29 | 30 | def get_registration(config: KISSConfig): 31 | return Registration( 32 | max_num_iterations=config.registration.max_num_iterations, 33 | convergence_criterion=config.registration.convergence_criterion, 34 | max_num_threads=config.registration.max_num_threads, 35 | ) 36 | 37 | 38 | class Registration: 39 | def __init__( 40 | self, 41 | max_num_iterations: int, 42 | convergence_criterion: float, 43 | max_num_threads: int = 0, 44 | ): 45 | self._registration = mapmos_pybind._Registration( 46 | max_num_iterations=max_num_iterations, 47 | convergence_criterion=convergence_criterion, 48 | max_num_threads=max_num_threads, 49 | ) 50 | 51 | def align_points_to_map( 52 | self, 53 | points: np.ndarray, 54 | voxel_map: VoxelHashMap, 55 | initial_guess: np.ndarray, 56 | max_correspondance_distance: float, 57 | kernel: float, 58 | ) -> np.ndarray: 59 | return self._registration._align_points_to_map( 60 | points=mapmos_pybind._Vector3dVector(points), 61 | voxel_map=voxel_map._internal_map, 62 | initial_guess=initial_guess, 63 | max_correspondance_distance=max_correspondance_distance, 64 | kernel=kernel, 65 | ) 66 | -------------------------------------------------------------------------------- /src/mapmos/training_module.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | import torch 25 | from pytorch_lightning import LightningModule 26 | 27 | from mapmos.config import MapMOSConfig 28 | from mapmos.mapmos_net import MapMOSNet 29 | from mapmos.metrics import get_confusion_matrix, get_iou, get_precision, get_recall 30 | from mapmos.utils.augmentation import ( 31 | random_flip_point_cloud, 32 | random_scale_point_cloud, 33 | rotate_perturbation_point_cloud, 34 | rotate_point_cloud, 35 | ) 36 | 37 | 38 | class TrainingModule(LightningModule): 39 | def __init__(self, config: MapMOSConfig): 40 | super().__init__() 41 | self.save_hyperparameters(dict(config)) 42 | self.lr = config.training.lr 43 | self.lr_epoch = config.training.lr_epoch 44 | self.lr_decay = config.training.lr_decay 45 | self.weight_decay = config.training.weight_decay 46 | self.mos = MapMOSNet(config.mos.voxel_size_mos) 47 | self.loss = torch.nn.BCEWithLogitsLoss() 48 | self.train_reset() 49 | self.val_reset() 50 | 51 | def train_reset(self): 52 | self.train_confusion_matrix_scan = torch.zeros(2, 2) 53 | self.train_confusion_matrix_map = torch.zeros(2, 2) 54 | 55 | def val_reset(self): 56 | self.val_confusion_matrix_scan = torch.zeros(2, 2) 57 | self.val_confusion_matrix_map = torch.zeros(2, 2) 58 | 59 | def training_step(self, batch: torch.Tensor, batch_idx, dataloader_index=0): 60 | # Batch is [batch,x,y,z,t,scan_idx,label] 61 | 62 | # Skip step if too few moving points 63 | batch_scan = batch[self.mask_scan(batch)] 64 | num_moving_points = len(batch_scan[batch_scan[:, -1] == 1.0]) 65 | num_points = len(batch_scan) 66 | if num_points == 0 or num_moving_points / num_points < 0.001: 67 | return None 68 | 69 | batch = self.augmentation(batch) 70 | 71 | # Only train if enough points are left 72 | if len(batch) < 100: 73 | return None 74 | 75 | coordinates = batch[:, :5].reshape(-1, 5) 76 | features = batch[:, 5].reshape(-1, 1) 77 | gt_labels = batch[:, 6].reshape(-1) 78 | logits = self.mos.forward(coordinates, features) 79 | 80 | loss = self.loss(logits, gt_labels) 81 | self.log("train_loss", loss.item(), on_step=True) 82 | 83 | logits = logits.detach().cpu() 84 | gt_labels = gt_labels.detach().cpu() 85 | 86 | pred_labels = self.mos.to_label(logits) 87 | 88 | # Logging metrics 89 | mask_scan = self.mask_scan(batch).detach().cpu() 90 | self.train_confusion_matrix_scan += get_confusion_matrix( 91 | pred_labels[mask_scan], gt_labels[mask_scan] 92 | ) 93 | 94 | self.train_confusion_matrix_map += get_confusion_matrix( 95 | pred_labels[~mask_scan], gt_labels[~mask_scan] 96 | ) 97 | torch.cuda.empty_cache() 98 | return loss 99 | 100 | def on_train_epoch_end(self): 101 | iou_scan = get_iou(self.train_confusion_matrix_scan) 102 | recall_scan = get_recall(self.train_confusion_matrix_scan) 103 | precision_scan = get_precision(self.train_confusion_matrix_scan) 104 | self.log("train_moving_iou_scan", iou_scan[1].item()) 105 | self.log("train_moving_recall_scan", recall_scan[1].item()) 106 | self.log("train_moving_precision_scan", precision_scan[1].item()) 107 | 108 | iou_map = get_iou(self.train_confusion_matrix_map) 109 | recall_map = get_recall(self.train_confusion_matrix_map) 110 | precision_map = get_precision(self.train_confusion_matrix_map) 111 | self.log("train_moving_iou_map", iou_map[1].item()) 112 | self.log("train_moving_recall_map", recall_map[1].item()) 113 | self.log("train_moving_precision_map", precision_map[1].item()) 114 | self.train_reset() 115 | torch.cuda.empty_cache() 116 | 117 | def validation_step(self, batch: torch.Tensor, batch_idx): 118 | # Batch is [batch,x,y,z,t,scan_idx,label] 119 | if len(batch) < 100: 120 | return None 121 | 122 | coordinates = batch[:, :5].reshape(-1, 5) 123 | features = batch[:, 5].reshape(-1, 1) 124 | gt_labels = batch[:, 6].reshape(-1) 125 | 126 | logits = self.mos.forward(coordinates, features) 127 | 128 | loss = self.loss(logits, gt_labels) 129 | self.log("val_loss", loss.item(), batch_size=len(batch), prog_bar=True, on_epoch=True) 130 | 131 | logits = logits.detach().cpu() 132 | gt_labels = gt_labels.detach().cpu() 133 | 134 | pred_labels = self.mos.to_label(logits) 135 | 136 | # Logging metrics 137 | mask_scan = self.mask_scan(batch).detach().cpu() 138 | self.val_confusion_matrix_scan += get_confusion_matrix( 139 | pred_labels[mask_scan], gt_labels[mask_scan] 140 | ) 141 | self.val_confusion_matrix_map += get_confusion_matrix( 142 | pred_labels[~mask_scan], gt_labels[~mask_scan] 143 | ) 144 | torch.cuda.empty_cache() 145 | return loss 146 | 147 | def on_validation_epoch_end(self): 148 | iou_scan = get_iou(self.val_confusion_matrix_scan) 149 | recall_scan = get_recall(self.val_confusion_matrix_scan) 150 | precision_scan = get_precision(self.val_confusion_matrix_scan) 151 | self.log("val_moving_iou_scan", iou_scan[1].item()) 152 | self.log("val_moving_recall_scan", recall_scan[1].item()) 153 | self.log("val_moving_precision_scan", precision_scan[1].item()) 154 | 155 | iou_map = get_iou(self.val_confusion_matrix_map) 156 | recall_map = get_recall(self.val_confusion_matrix_map) 157 | precision_map = get_precision(self.val_confusion_matrix_map) 158 | self.log("val_moving_iou_map", iou_map[1].item()) 159 | self.log("val_moving_recall_map", recall_map[1].item()) 160 | self.log("val_moving_precision_map", precision_map[1].item()) 161 | self.val_reset() 162 | torch.cuda.empty_cache() 163 | 164 | def configure_optimizers(self): 165 | optimizer = torch.optim.Adam(self.parameters(), lr=self.lr, weight_decay=self.weight_decay) 166 | scheduler = torch.optim.lr_scheduler.StepLR( 167 | optimizer, step_size=self.lr_epoch, gamma=self.lr_decay 168 | ) 169 | return [optimizer], [scheduler] 170 | 171 | def mask_scan(self, batch): 172 | return batch[:, 4] == 0.0 173 | 174 | def augmentation(self, batch): 175 | batch = self.crop(batch) 176 | batch[:, 1:4] = rotate_point_cloud(batch[:, 1:4]) 177 | batch[:, 1:4] = rotate_perturbation_point_cloud(batch[:, 1:4]) 178 | batch[:, 1:4] = random_flip_point_cloud(batch[:, 1:4]) 179 | batch[:, 1:4] = random_scale_point_cloud(batch[:, 1:4]) 180 | batch = self.subsample(batch) 181 | return batch 182 | 183 | def crop(self, batch): 184 | mask_scan = self.mask_scan(batch) 185 | sample_point = batch[mask_scan][np.random.choice(range(len(batch[mask_scan]))), 1:4] 186 | crop_x = np.random.normal(15, 2) 187 | crop_y = np.random.normal(15, 2) 188 | 189 | dist = torch.abs(batch[:, 1:4] - sample_point).reshape(-1, 3) 190 | mask = dist[:, 0] < crop_x 191 | mask = torch.logical_and(mask, dist[:, 1] < crop_y) 192 | return batch[mask] 193 | 194 | def subsample(self, batch, max_dropout_ratio=0.5): 195 | dropout = (1 - max_dropout_ratio) * torch.rand(1) + max_dropout_ratio 196 | keep = torch.rand(len(batch)) < dropout 197 | return batch[keep] 198 | -------------------------------------------------------------------------------- /src/mapmos/utils/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | -------------------------------------------------------------------------------- /src/mapmos/utils/augmentation.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import torch 24 | 25 | 26 | def rotate_point_cloud(points): 27 | """Randomly rotate the point clouds to augment the dataset""" 28 | rotation_angle = torch.rand(1) * 2 * torch.pi 29 | cosval = torch.cos(rotation_angle) 30 | sinval = torch.sin(rotation_angle) 31 | rotation_matrix = torch.Tensor([[cosval, -sinval, 0], [sinval, cosval, 0], [0, 0, 1]]) 32 | points = points @ rotation_matrix.type_as(points) 33 | return points 34 | 35 | 36 | def rotate_perturbation_point_cloud(points, angle_sigma=0.2, angle_clip=0.5): 37 | """Randomly perturb the point clouds by small rotations""" 38 | angles = torch.clip(angle_sigma * torch.randn(3), -angle_clip, angle_clip) 39 | Rx = torch.Tensor( 40 | [ 41 | [1, 0, 0], 42 | [0, torch.cos(angles[0]), -torch.sin(angles[0])], 43 | [0, torch.sin(angles[0]), torch.cos(angles[0])], 44 | ] 45 | ) 46 | Ry = torch.Tensor( 47 | [ 48 | [torch.cos(angles[1]), 0, torch.sin(angles[1])], 49 | [0, 1, 0], 50 | [-torch.sin(angles[1]), 0, torch.cos(angles[1])], 51 | ] 52 | ) 53 | Rz = torch.Tensor( 54 | [ 55 | [torch.cos(angles[2]), -torch.sin(angles[2]), 0], 56 | [torch.sin(angles[2]), torch.cos(angles[2]), 0], 57 | [0, 0, 1], 58 | ] 59 | ) 60 | rotation_matrix = Rz @ Ry @ Rx 61 | points = points @ rotation_matrix.type_as(points) 62 | return points 63 | 64 | 65 | def random_flip_point_cloud(points): 66 | """Randomly flip the point cloud. Flip is per points.""" 67 | if torch.rand(1).item() > 0.5: 68 | points = torch.multiply(points, torch.tensor([-1, 1, 1]).type_as(points)) 69 | if torch.rand(1).item() > 0.5: 70 | points = torch.multiply(points, torch.tensor([1, -1, 1]).type_as(points)) 71 | return points 72 | 73 | 74 | def random_scale_point_cloud(points, scale_low=0.8, scale_high=1.2): 75 | """Randomly scale the points.""" 76 | scales = (scale_low - scale_high) * torch.rand(1, 3) + scale_high 77 | points = torch.multiply(points, scales.type_as(points)) 78 | return points 79 | -------------------------------------------------------------------------------- /src/mapmos/utils/cache.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import functools as ft 24 | 25 | from diskcache import Cache 26 | from diskcache.core import ENOVAL, args_to_key, full_name 27 | 28 | 29 | def get_cache(directory, size_limit: int = 500): 30 | __GiB__ = 2**30 31 | return Cache(directory, size_limit=size_limit * __GiB__) 32 | 33 | 34 | def memoize(name=None, typed=False, expire=None, tag=None): 35 | """Same as DiskCache.memoize but ignoring the first argument(self) for the keys.""" 36 | # Caution: Nearly identical code exists in DjangoCache.memoize 37 | if callable(name): 38 | raise TypeError(f"name {name} cannot be callable") 39 | 40 | def decorator(func): 41 | """Decorator created by memoize() for callable `func`.""" 42 | base = (full_name(func),) if name is None else (name,) 43 | 44 | @ft.wraps(func) 45 | def wrapper(*args, **kwargs): 46 | cls = args[0] 47 | if not cls.use_cache: 48 | return func(*args, **kwargs) 49 | key = wrapper.__cache_key__(*args, **kwargs) 50 | result = cls.cache.get(key, default=ENOVAL, retry=True) 51 | 52 | if result is ENOVAL: 53 | result = func(*args, **kwargs) 54 | if expire is None or expire > 0: 55 | cls.cache.set(key, result, expire, tag=tag, retry=True) 56 | 57 | return result 58 | 59 | def __cache_key__(*args, **kwargs): 60 | """Make key for cache given function arguments.""" 61 | return args_to_key(base, args, kwargs, typed, ignore={0, "self"}) 62 | 63 | wrapper.__cache_key__ = __cache_key__ 64 | return wrapper 65 | 66 | return decorator 67 | -------------------------------------------------------------------------------- /src/mapmos/utils/pipeline_results.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import numpy as np 24 | from kiss_icp.metrics import absolute_trajectory_error, sequence_error 25 | from kiss_icp.tools.pipeline_results import PipelineResults 26 | 27 | from mapmos.metrics import get_f1, get_iou, get_precision, get_recall, get_stats 28 | 29 | 30 | class MOSPipelineResults(PipelineResults): 31 | def __init__(self) -> None: 32 | super().__init__() 33 | 34 | def eval_odometry(self, gt_poses, poses): 35 | avg_tra, avg_rot = sequence_error(gt_poses, poses) 36 | ate_rot, ate_trans = absolute_trajectory_error(gt_poses, poses) 37 | self.append(desc="Average Translation Error", units="%", value=avg_tra) 38 | self.append(desc="Average Rotational Error", units="deg/m", value=avg_rot) 39 | self.append(desc="Absoulte Trajectory Error (ATE)", units="m", value=ate_trans) 40 | self.append(desc="Absoulte Rotational Error (ARE)\n", units="rad", value=ate_rot) 41 | 42 | def eval_mos(self, confusion_matrix, desc=""): 43 | iou = get_iou(confusion_matrix) 44 | tp, fp, fn = get_stats(confusion_matrix) 45 | recall = get_recall(confusion_matrix) 46 | precision = get_precision(confusion_matrix) 47 | f1 = get_f1(confusion_matrix) 48 | self.append(desc=desc, units="", value="") 49 | self.append(desc="Moving IoU", units="%", value=iou[1].item() * 100) 50 | self.append(desc="Moving Recall", units="%", value=recall[1].item() * 100) 51 | self.append(desc="Moving Precision", units="%", value=precision[1].item() * 100) 52 | self.append(desc="Moving F1", units="%", value=f1[1].item() * 100) 53 | self.append(desc="Moving TP", units="points", value=int(tp[1].item())) 54 | self.append(desc="Moving FP", units="points", value=int(fp[1].item())) 55 | self.append(desc="Moving FN\n", units="points", value=int(fn[1].item())) 56 | 57 | def eval_fps(self, times, desc="Average Frequency"): 58 | def _get_fps(times): 59 | total_time_s = sum(times) * 1e-9 60 | return float(len(times) / total_time_s) 61 | 62 | avg_fps_mos = int(np.floor(_get_fps(times))) 63 | self.append( 64 | desc=f"{desc}", 65 | units="Hz", 66 | value=avg_fps_mos, 67 | trunc=True, 68 | ) 69 | -------------------------------------------------------------------------------- /src/mapmos/utils/save.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import importlib 24 | import os 25 | from pathlib import Path 26 | 27 | import numpy as np 28 | 29 | 30 | class StubWriter: 31 | def __init__(self) -> None: 32 | pass 33 | 34 | def write(self, *_, **__): 35 | pass 36 | 37 | 38 | class PlyWriter: 39 | def __init__(self) -> None: 40 | try: 41 | self.o3d = importlib.import_module("open3d") 42 | except ModuleNotFoundError as err: 43 | print(f'open3d is not installed on your system, run "pip install open3d"') 44 | exit(1) 45 | 46 | def write( 47 | self, scan_points: np.ndarray, pred_labels: np.ndarray, gt_labels: np.ndarray, filename: str 48 | ): 49 | os.makedirs(Path(filename).parent, exist_ok=True) 50 | pcd_current_scan = self.o3d.geometry.PointCloud( 51 | self.o3d.utility.Vector3dVector(scan_points) 52 | ).paint_uniform_color([0, 0, 0]) 53 | 54 | scan_colors = np.array(pcd_current_scan.colors) 55 | 56 | tp = (pred_labels == 1) & (gt_labels == 1) 57 | fp = (pred_labels == 1) & (gt_labels != 1) 58 | fn = (pred_labels != 1) & (gt_labels == 1) 59 | 60 | scan_colors[tp] = [0, 1, 0] 61 | scan_colors[fp] = [1, 0, 0] 62 | scan_colors[fn] = [0, 0, 1] 63 | 64 | pcd_current_scan.colors = self.o3d.utility.Vector3dVector(scan_colors) 65 | self.o3d.io.write_point_cloud(filename, pcd_current_scan) 66 | 67 | 68 | class KITTIWriter: 69 | def __init__(self) -> None: 70 | pass 71 | 72 | def write(self, pred_labels: np.ndarray, filename: str): 73 | os.makedirs(Path(filename).parent, exist_ok=True) 74 | kitti_labels = np.copy(pred_labels) 75 | kitti_labels[pred_labels == 0] = 9 76 | kitti_labels[pred_labels == 1] = 251 77 | kitti_labels = kitti_labels.reshape(-1).astype(np.int32) 78 | kitti_labels.tofile(filename) 79 | -------------------------------------------------------------------------------- /src/mapmos/utils/seed.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2023 Benedikt Mersch, Tiziano Guadagnino, Ignacio Vizzo, Cyrill 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 | 23 | import os 24 | import random 25 | 26 | import numpy as np 27 | import torch 28 | import torch.backends.cudnn 29 | 30 | 31 | def set_seed(seed): 32 | os.environ["CUBLAS_WORKSPACE_CONFIG"] = ":4096:8" 33 | torch.use_deterministic_algorithms(True) 34 | torch.manual_seed(seed) 35 | random.seed(seed) 36 | np.random.seed(seed) 37 | torch.cuda.manual_seed(seed) 38 | torch.cuda.manual_seed_all(seed) 39 | torch.backends.cudnn.enabled = False 40 | torch.backends.cudnn.deterministic = True 41 | torch.backends.cudnn.benchmark = False 42 | os.environ["PYTHONHASHSEED"] = str(seed) 43 | --------------------------------------------------------------------------------