├── .clang-format ├── .cmake-format.yaml ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── cpp ├── COLCON_IGNORE └── sage_icp │ ├── 3rdparty │ ├── eigen │ │ ├── LICENSE │ │ └── eigen.cmake │ ├── find_dependencies.cmake │ ├── sophus │ │ ├── LICENSE │ │ └── sophus.cmake │ ├── tbb │ │ ├── LICENSE │ │ └── tbb.cmake │ └── tsl_robin │ │ ├── LICENSE │ │ └── tsl_robin.cmake │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake │ └── CompilerOptions.cmake │ ├── core │ ├── CMakeLists.txt │ ├── Deskew.cpp │ ├── Deskew.hpp │ ├── Preprocessing.cpp │ ├── Preprocessing.hpp │ ├── Registration.cpp │ ├── Registration.hpp │ ├── Threshold.cpp │ ├── Threshold.hpp │ ├── VoxelHashMap.cpp │ ├── VoxelHashMap.hpp │ ├── ikd_Tree.cpp │ └── ikd_Tree.h │ ├── metrics │ ├── CMakeLists.txt │ ├── Metrics.cpp │ └── Metrics.hpp │ └── pipeline │ ├── CMakeLists.txt │ ├── sageICP.cpp │ └── sageICP.hpp ├── eval ├── kitti360_image_pub copy.py ├── kitti360_oxts_pub_wogt.py ├── kitti360_pub.py ├── kitti_pub.py ├── kittiraw_image_pub.py └── kittiraw_pub.py ├── figure └── pipeline.png └── ros ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── odometry.launch.py ├── odometry_360.launch.py ├── odometry_gt.launch.py ├── odometry_raw.launch.py ├── range_odom.launch.py ├── range_odom_360.launch.py ├── range_odom_raw.launch.py ├── sem_odom.launch.py ├── sem_odom_360.launch.py ├── sem_odom_raw.launch.py └── semantic-kitti.yaml ├── package.xml ├── ros2 ├── OdometryServer.cpp ├── OdometryServer.hpp └── Utils.hpp └── rviz ├── kiss_icp_record.rviz └── sage_icp_ros2.rviz /.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 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | install/ 2 | log/ 3 | results/* 4 | wheelhouse/ 5 | _skbuild/ 6 | .gitlab-ci-local/ 7 | 8 | # Created by https://www.toptal.com/developers/gitignore/api/python,c++ 9 | # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ 10 | ### C++ ### 11 | # Prerequisites 12 | *.d 13 | 14 | # Compiled Object files 15 | *.slo 16 | *.lo 17 | *.o 18 | *.obj 19 | 20 | # Precompiled Headers 21 | *.gch 22 | *.pch 23 | 24 | # Compiled Dynamic libraries 25 | *.so 26 | *.dylib 27 | *.dll 28 | 29 | # Fortran module files 30 | *.mod 31 | *.smod 32 | 33 | # Compiled Static libraries 34 | *.lai 35 | *.la 36 | *.a 37 | *.lib 38 | 39 | # Executables 40 | *.exe 41 | *.out 42 | *.app 43 | 44 | ### Python ### 45 | # Byte-compiled / optimized / DLL files 46 | __pycache__/ 47 | *.py[cod] 48 | *$py.class 49 | 50 | # C extensions 51 | 52 | # Distribution / packaging 53 | .Python 54 | build/ 55 | develop-eggs/ 56 | dist/ 57 | downloads/ 58 | eggs/ 59 | .eggs/ 60 | lib/ 61 | lib64/ 62 | parts/ 63 | sdist/ 64 | var/ 65 | wheels/ 66 | share/python-wheels/ 67 | *.egg-info/ 68 | .installed.cfg 69 | *.egg 70 | MANIFEST 71 | 72 | # PyInstaller 73 | # Usually these files are written by a python script from a template 74 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 75 | *.manifest 76 | *.spec 77 | 78 | # Installer logs 79 | pip-log.txt 80 | pip-delete-this-directory.txt 81 | 82 | # Unit test / coverage reports 83 | htmlcov/ 84 | .tox/ 85 | .nox/ 86 | .coverage 87 | .coverage.* 88 | .cache 89 | nosetests.xml 90 | coverage.xml 91 | *.cover 92 | *.py,cover 93 | .hypothesis/ 94 | .pytest_cache/ 95 | cover/ 96 | 97 | # Translations 98 | *.mo 99 | *.pot 100 | 101 | # Django stuff: 102 | *.log 103 | local_settings.py 104 | db.sqlite3 105 | db.sqlite3-journal 106 | 107 | # Flask stuff: 108 | instance/ 109 | .webassets-cache 110 | 111 | # Scrapy stuff: 112 | .scrapy 113 | 114 | # Sphinx documentation 115 | docs/_build/ 116 | 117 | # PyBuilder 118 | .pybuilder/ 119 | target/ 120 | 121 | # Jupyter Notebook 122 | .ipynb_checkpoints 123 | 124 | # IPython 125 | profile_default/ 126 | ipython_config.py 127 | 128 | # pyenv 129 | # For a library or package, you might want to ignore these files since the code is 130 | # intended to run in multiple environments; otherwise, check them in: 131 | # .python-version 132 | 133 | # pipenv 134 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 135 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 136 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 137 | # install all needed dependencies. 138 | #Pipfile.lock 139 | 140 | # poetry 141 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 142 | # This is especially recommended for binary packages to ensure reproducibility, and is more 143 | # commonly ignored for libraries. 144 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 145 | #poetry.lock 146 | 147 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 148 | __pypackages__/ 149 | 150 | # Celery stuff 151 | celerybeat-schedule 152 | celerybeat.pid 153 | 154 | # SageMath parsed files 155 | *.sage.py 156 | 157 | # Environments 158 | .env 159 | .venv 160 | env/ 161 | venv/ 162 | ENV/ 163 | env.bak/ 164 | venv.bak/ 165 | 166 | # Spyder project settings 167 | .spyderproject 168 | .spyproject 169 | 170 | # Rope project settings 171 | .ropeproject 172 | 173 | # mkdocs documentation 174 | /site 175 | 176 | # mypy 177 | .mypy_cache/ 178 | .dmypy.json 179 | dmypy.json 180 | 181 | # Pyre type checker 182 | .pyre/ 183 | 184 | # pytype static type analyzer 185 | .pytype/ 186 | 187 | # Cython debug symbols 188 | cython_debug/ 189 | 190 | # PyCharm 191 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 192 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 193 | # and can be added to the global gitignore or merged into this file. For a more nuclear 194 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 195 | #.idea/ 196 | 197 | # End of https://www.toptal.com/developers/gitignore/api/python,c++ 198 | *.orig 199 | # Created by https://www.toptal.com/developers/gitignore/api/python 200 | # Edit at https://www.toptal.com/developers/gitignore?templates=python 201 | 202 | ### Python ### 203 | # Byte-compiled / optimized / DLL files 204 | __pycache__/ 205 | *.py[cod] 206 | *$py.class 207 | 208 | # C extensions 209 | *.so 210 | 211 | # Distribution / packaging 212 | .Python 213 | build/ 214 | develop-eggs/ 215 | dist/ 216 | downloads/ 217 | eggs/ 218 | .eggs/ 219 | lib/ 220 | lib64/ 221 | parts/ 222 | sdist/ 223 | var/ 224 | wheels/ 225 | share/python-wheels/ 226 | *.egg-info/ 227 | .installed.cfg 228 | *.egg 229 | MANIFEST 230 | 231 | # PyInstaller 232 | # Usually these files are written by a python script from a template 233 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 234 | *.manifest 235 | *.spec 236 | 237 | # Installer logs 238 | pip-log.txt 239 | pip-delete-this-directory.txt 240 | 241 | # Unit test / coverage reports 242 | htmlcov/ 243 | .tox/ 244 | .nox/ 245 | .coverage 246 | .coverage.* 247 | .cache 248 | nosetests.xml 249 | coverage.xml 250 | *.cover 251 | *.py,cover 252 | .hypothesis/ 253 | .pytest_cache/ 254 | cover/ 255 | 256 | # Translations 257 | *.mo 258 | *.pot 259 | 260 | # Django stuff: 261 | *.log 262 | local_settings.py 263 | db.sqlite3 264 | db.sqlite3-journal 265 | 266 | # Flask stuff: 267 | instance/ 268 | .webassets-cache 269 | 270 | # Scrapy stuff: 271 | .scrapy 272 | 273 | # Sphinx documentation 274 | docs/_build/ 275 | 276 | # PyBuilder 277 | .pybuilder/ 278 | target/ 279 | 280 | # Jupyter Notebook 281 | .ipynb_checkpoints 282 | 283 | # IPython 284 | profile_default/ 285 | ipython_config.py 286 | 287 | # pyenv 288 | # For a library or package, you might want to ignore these files since the code is 289 | # intended to run in multiple environments; otherwise, check them in: 290 | # .python-version 291 | 292 | # pipenv 293 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 294 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 295 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 296 | # install all needed dependencies. 297 | #Pipfile.lock 298 | 299 | # poetry 300 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 301 | # This is especially recommended for binary packages to ensure reproducibility, and is more 302 | # commonly ignored for libraries. 303 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 304 | #poetry.lock 305 | 306 | # pdm 307 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 308 | #pdm.lock 309 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 310 | # in version control. 311 | # https://pdm.fming.dev/#use-with-ide 312 | .pdm.toml 313 | 314 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 315 | __pypackages__/ 316 | 317 | # Celery stuff 318 | celerybeat-schedule 319 | celerybeat.pid 320 | 321 | # SageMath parsed files 322 | *.sage.py 323 | 324 | # Environments 325 | .env 326 | .venv 327 | env/ 328 | venv/ 329 | ENV/ 330 | env.bak/ 331 | venv.bak/ 332 | 333 | # Spyder project settings 334 | .spyderproject 335 | .spyproject 336 | 337 | # Rope project settings 338 | .ropeproject 339 | 340 | # mkdocs documentation 341 | /site 342 | 343 | # mypy 344 | .mypy_cache/ 345 | .dmypy.json 346 | dmypy.json 347 | 348 | # Pyre type checker 349 | .pyre/ 350 | 351 | # pytype static type analyzer 352 | .pytype/ 353 | 354 | # Cython debug symbols 355 | cython_debug/ 356 | 357 | # PyCharm 358 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 359 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 360 | # and can be added to the global gitignore or merged into this file. For a more nuclear 361 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 362 | #.idea/ 363 | 364 | # End of https://www.toptal.com/developers/gitignore/api/python 365 | nn 366 | 367 | # VSCode 368 | .vscode 369 | -------------------------------------------------------------------------------- /.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 | - id: end-of-file-fixer 7 | - id: check-yaml 8 | - id: check-added-large-files 9 | - repo: https://github.com/pre-commit/mirrors-clang-format 10 | rev: v14.0.0 11 | hooks: 12 | - id: clang-format 13 | - repo: https://github.com/psf/black 14 | rev: 23.1.0 15 | hooks: 16 | - id: black 17 | args: [--config=python/pyproject.toml] 18 | - repo: https://github.com/ahans/cmake-format-precommit 19 | rev: 8e52fb6506f169dddfaa87f88600d765fca48386 20 | hooks: 21 | - id: cmake-format 22 | - repo: https://github.com/pycqa/isort 23 | rev: 5.12.0 24 | hooks: 25 | - id: isort 26 | args: ["--settings-path=python/pyproject.toml"] 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 NeSC-IV 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

SAGE-ICP

3 | Paper 4 |   •   5 | Arxiv 6 |   •   7 | YouTube 8 |   •   9 | bilibili 10 |
11 |
12 | 13 | [SAGE-ICP](https://ieeexplore.ieee.org/document/10610280) is a semantic information-assisted point-to-point ICP ([ICRA 2024](https://2024.ieee-icra.org/), accepted). 14 |
15 | [Jiaming Cui](https://github.com/shashenyiguang), Jiming Chen, [Liang Li](https://github.com/liangli1990) 16 | 17 |

18 | SAGE-ICP Demo 19 |

20 |
21 |
22 | 23 | ## Prerequisites 24 | 1. [ROS2](https://www.ros.org/blog/getting-started/) 25 | 2. Dependencies 26 | ```sh 27 | sudo apt-get install ros--tf-transformations 28 | pip install transforms3d kiss-icp lark-parser 29 | ``` 30 | 3. Semantic segmentation models for LiDAR point clouds, could select one installation. 31 | [Cylinder3D](https://github.com/NeSC-IV/cylinder3d_ros2), [RangeNet++](https://github.com/NeSC-IV/RangeNetTrt8) 32 | 33 | ## Install 34 | ```sh 35 | mkdir -p ~/sage-icp/src && cd ~/sage-icp/src 36 | git clone https://github.com/NeSC-IV/sage-icp.git 37 | cd .. && colcon build --symlink-install 38 | ``` 39 | 40 | ## Evaluation 41 | To evaluate cylinder3D on KITTI Odometry, please run: 42 | ```sh 43 | source install/setup.bash 44 | ros2 launch sage_icp sem_odom.launch.py 45 | ``` 46 | To evaluate RangeNet++ on KITTI Odometry, please run: 47 | ```sh 48 | source install/setup.bash 49 | ros2 launch sage_icp range_odom.launch.py 50 | ``` 51 | 52 | ## Citation 53 | 54 | If you use this library for any academic work, please cite our original [Paper](https://ieeexplore.ieee.org/document/10610280). 55 | ```sh 56 | @INPROCEEDINGS{10610280, 57 | author={Cui, Jiaming and Chen, Jiming and Li, Liang}, 58 | booktitle={2024 IEEE International Conference on Robotics and Automation (ICRA)}, 59 | title={SAGE-ICP: Semantic Information-Assisted ICP}, 60 | year={2024}, 61 | volume={}, 62 | number={}, 63 | pages={8537-8543}, 64 | keywords={Location awareness;Training;Laser radar;Accuracy;Semantic segmentation;Semantics;Pose estimation}, 65 | doi={10.1109/ICRA57147.2024.10610280}} 66 | ``` 67 | 68 | 69 | 70 | ## Acknowledgements 71 | This repo is built upon [KISS-ICP](https://github.com/PRBonn/kiss-icp), [Cylinder3D](https://github.com/xinge008/Cylinder3D) and [RangeNet++](https://github.com/PRBonn/rangenet_lib). 72 | This project is free software made available under the MIT License. For details see the LICENSE file. -------------------------------------------------------------------------------- /cpp/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NeSC-IV/sage-icp/69d46b8e52a4a0cae6227e6569a623b0fb0d905b/cpp/COLCON_IGNORE -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/eigen/LICENSE: -------------------------------------------------------------------------------- 1 | Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: 2 | http://www.mozilla.org/MPL/2.0/ 3 | http://www.mozilla.org/MPL/2.0/FAQ.html 4 | 5 | Some files contain third-party code under BSD or LGPL licenses, whence the other 6 | COPYING.* files here. 7 | 8 | All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. 9 | For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. 10 | 11 | If you want to guarantee that the Eigen code that you are #including is licensed 12 | under the MPL2 and possibly more permissive licenses (like BSD), #define this 13 | preprocessor symbol: 14 | EIGEN_MPL2_ONLY 15 | For example, with most compilers, you could add this to your project CXXFLAGS: 16 | -DEIGEN_MPL2_ONLY 17 | This will cause a compilation error to be generated if you #include any code that is 18 | LGPL licensed. 19 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | # 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 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 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 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/sophus/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2015 Jesse Beder. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | THE SOFTWAR 20 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/sophus/sophus.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # # Copyright (c) 2023 Saurabh Gupta, Ignacio Vizzo, Cyrill Stachniss, University of Bonn 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | include(FetchContent) 23 | 24 | set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") 25 | set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") 26 | set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") 27 | 28 | # TODO: after https://github.com/strasdat/Sophus/pull/502 gets merged go back to mainstream 29 | FetchContent_Declare(sophus SYSTEM URL https://github.com/nachovizzo/Sophus/archive/refs/tags/1.22.11.tar.gz) 30 | FetchContent_MakeAvailable(sophus) 31 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/tbb/LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/tbb/tbb.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # option(BUILD_SHARED_LIBS ON) 24 | option(BUILD_SHARED_LIBS OFF) 25 | option(TBBMALLOC_BUILD OFF) 26 | option(TBB_EXAMPLES OFF) 27 | option(TBB_STRICT OFF) 28 | option(TBB_TEST OFF) 29 | 30 | include(FetchContent) 31 | FetchContent_Declare(tbb SYSTEM URL https://github.com/oneapi-src/oneTBB/archive/refs/tags/v2021.8.0.tar.gz) 32 | FetchContent_MakeAvailable(tbb) 33 | 34 | if(${CMAKE_VERSION} VERSION_LESS 3.25) 35 | get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) 36 | set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") 37 | endif() 38 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/tsl_robin/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Thibaut Goetghebuer-Planchon 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /cpp/sage_icp/3rdparty/tsl_robin/tsl_robin.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | include(FetchContent) 24 | FetchContent_Declare(tessil SYSTEM URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.0.1.tar.gz) 25 | FetchContent_MakeAvailable(tessil) 26 | -------------------------------------------------------------------------------- /cpp/sage_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 NeSC-IV 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | cmake_minimum_required(VERSION 3.16...3.26) 23 | project(sage_icp_cpp VERSION 0.2.9 LANGUAGES CXX) 24 | 25 | # Setup build options 26 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 27 | option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) 28 | option(USE_SYSTEM_TSLMAP "Use system pre-installed tsl_robin" ON) 29 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 30 | 31 | # Set build type (repeat here for C++ only consumers) 32 | set(CMAKE_BUILD_TYPE Release) 33 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 34 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 35 | include(3rdparty/find_dependencies.cmake) 36 | include(cmake/CompilerOptions.cmake) 37 | add_subdirectory(core) 38 | add_subdirectory(metrics) 39 | add_subdirectory(pipeline) 40 | -------------------------------------------------------------------------------- /cpp/sage_icp/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 NeSC-IV 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 | -------------------------------------------------------------------------------- /cpp/sage_icp/README.md: -------------------------------------------------------------------------------- 1 | # SAGE-ICP C++ Library 2 | 3 | ## How to build 4 | 5 | ```sh 6 | cmake -Bbuild 7 | cmake --build build -j$(nproc --all) 8 | ``` 9 | 10 | ## Dependencies 11 | 12 | The cmake build system should handle all dependencies for you. In case you have some particular 13 | requirements for the library dependencies, by default the build system will attempt to use the 14 | ones you have installed on your local system. 15 | -------------------------------------------------------------------------------- /cpp/sage_icp/cmake/CompilerOptions.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | function(set_global_target_properties target) 24 | target_compile_features(${target} PUBLIC cxx_std_17) 25 | target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) 26 | target_compile_options( 27 | ${target} 28 | PRIVATE # MSVC 29 | $<$:/W4> 30 | $<$:/WX> 31 | # Clang/AppleClang 32 | $<$:-fcolor-diagnostics> 33 | $<$:-Werror> 34 | $<$:-Wall> 35 | $<$:-Wextra> 36 | $<$:-Wconversion> 37 | $<$:-Wno-sign-conversion> 38 | # GNU 39 | $<$:-fdiagnostics-color=always> 40 | $<$:-Werror> 41 | $<$:-Wall> 42 | $<$:-Wextra> 43 | $<$:-pedantic> 44 | $<$:-Wcast-align> 45 | $<$:-Wcast-qual> 46 | $<$:-Wconversion> 47 | $<$:-Wdisabled-optimization> 48 | $<$:-Woverloaded-virtual>) 49 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 50 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 51 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 52 | PUBLIC $ $) 53 | endfunction() 54 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 NeSC-IV 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 | find_package(PCL REQUIRED COMPONENTS common io registration filters keypoints features visualization range_image surface kdtree segmentation) 23 | link_directories(${PCL_LIBRARY_DIRS}) 24 | add_definitions(${PCL_DEFINITIONS}) 25 | find_package(OpenMP REQUIRED) 26 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 27 | add_compile_options(-std=c++17) 28 | set(CMAKE_CXX_FLAGS "-std=c++17 -pthread -O3 ${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 29 | add_library(core STATIC) 30 | add_library(sage_icp::core ALIAS core) 31 | target_sources(core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp) 32 | target_link_libraries(core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus OpenMP::OpenMP_CXX ${PCL_LIBRARIES}) 33 | set_global_target_properties(core) 34 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Deskew.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 "Deskew.hpp" 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | namespace { 31 | /// TODO(Nacho) Explain what is the very important meaning of this param 32 | constexpr double mid_pose_timestamp{0.5}; 33 | } // namespace 34 | 35 | namespace sage_icp { 36 | std::vector DeSkewScan(const std::vector &frame, 37 | const std::vector ×tamps, 38 | const Sophus::SE3d &start_pose, 39 | const Sophus::SE3d &finish_pose) { 40 | const auto delta_pose = (start_pose.inverse() * finish_pose).log(); 41 | std::vector corrected_frame(frame.size()); 42 | tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { 43 | const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose); 44 | Eigen::Vector3d v3point(frame[i][0], frame[i][1], frame[i][2]); 45 | Eigen::Vector3d transformed_v3point = motion * v3point; 46 | Eigen::Vector4d v4point(transformed_v3point[0], transformed_v3point[1], transformed_v3point[2], frame[i][3]); 47 | corrected_frame[i] = v4point; 48 | }); 49 | return corrected_frame; 50 | } 51 | } // namespace sage_icp 52 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Deskew.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | // #include 28 | // #include 29 | namespace sage_icp { 30 | 31 | /// Compensate the frame by estimatng the velocity between the given poses 32 | std::vector DeSkewScan(const std::vector &frame, 33 | const std::vector ×tamps, 34 | const Sophus::SE3d &start_pose, 35 | const Sophus::SE3d &finish_pose); 36 | 37 | } // namespace sage_icp 38 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Preprocessing.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 "Preprocessing.hpp" 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace { 34 | using Voxel = Eigen::Vector3i; 35 | struct VoxelHash { 36 | size_t operator()(const Voxel &voxel) const { 37 | const uint32_t *vec = reinterpret_cast(voxel.data()); 38 | return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791); 39 | } 40 | }; 41 | } // namespace 42 | 43 | namespace sage_icp { 44 | std::vector VoxelDownsample(const std::vector &frame, 45 | const std::vector> &voxel_labels, 46 | const std::vector &voxel_size, 47 | double vox_scale) { 48 | int voxel_size_len = static_cast(voxel_size.size()); 49 | // init grid group 50 | std::vector> grid_group(voxel_size_len); 51 | for (int i = 0; i < voxel_size_len; i++) 52 | { 53 | tsl::robin_map grid; 54 | grid.reserve(frame.size()); 55 | grid_group.emplace_back(grid); 56 | } 57 | // insert points into grid 58 | for (const auto &point : frame) { 59 | int label = static_cast(point[3]); 60 | int group = -1; 61 | for (int i = 0; i < voxel_size_len; i++) 62 | { 63 | if (std::find(voxel_labels[i].begin(), voxel_labels[i].end(), label) != voxel_labels[i].end()) 64 | { 65 | group = i; 66 | break; 67 | } 68 | } 69 | if (group == -1) continue; 70 | const auto voxel = Voxel((point.head<3>() / (voxel_size[group] * vox_scale)).cast()); //获取体素坐标 71 | if (grid_group[group].contains(voxel)) continue; 72 | grid_group[group].insert({voxel, point}); 73 | } 74 | std::vector frame_dowsampled; 75 | frame_dowsampled.reserve(frame.size()); 76 | for (int i = 0; i < voxel_size_len; i++) 77 | { 78 | for (const auto &[voxel, point] : grid_group[i]) { 79 | (void)voxel; 80 | frame_dowsampled.emplace_back(point); 81 | } 82 | } 83 | return frame_dowsampled; 84 | } 85 | 86 | std::vector Preprocess(const std::vector &frame, 87 | double max_range, 88 | double min_range, 89 | double label_max_range, 90 | bool dynamic_vehicle_filter, 91 | double dy_th, 92 | const std::vector &dynamic_labels, 93 | const std::vector &lankmark) { 94 | std::vector inliers; 95 | if(dynamic_vehicle_filter){ 96 | pcl::PointCloud::Ptr map_all(new pcl::PointCloud); 97 | pcl::PointCloud::Ptr map_vehicle(new pcl::PointCloud); 98 | std::vector vehicle_inliers; 99 | for (const auto &point : frame) { 100 | Eigen::Vector4d point_new = point; 101 | const double norm = point.head<3>().norm(); // Euclidean norm 102 | if (norm < max_range && norm > min_range){ 103 | if (norm > label_max_range) point_new[3] = 0.0; 104 | pcl::PointXYZL point_temp; 105 | point_temp.x = static_cast(point[0]); 106 | point_temp.y = static_cast(point[1]); 107 | point_temp.z = static_cast(point[2]); 108 | point_temp.label = static_cast(point_new[3]); 109 | map_all->points.emplace_back(point_temp); 110 | if (std::find(dynamic_labels.begin(), dynamic_labels.end(), point_temp.label) != dynamic_labels.end()){ 111 | map_vehicle->points.emplace_back(point_temp); 112 | vehicle_inliers.emplace_back(point_new); 113 | } 114 | // if(point_temp.label > 9 && point_temp.label < 21){ 115 | // map_vehicle->points.emplace_back(point_temp); 116 | // vehicle_inliers.emplace_back(point_new); 117 | // } 118 | else{ 119 | inliers.emplace_back(point_new); 120 | } 121 | } 122 | } 123 | /// 创建map树 124 | pcl::KdTreeFLANN maptree; // 创建一个KdTreeFLANN对象，输入点云。 125 | maptree.setInputCloud(map_all); 126 | 127 | /// 创建kd树 128 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 129 | tree->setInputCloud(map_vehicle); 130 | /// 设置分割参数 131 | std::vector cluster_indices; 132 | pcl::EuclideanClusterExtraction ec; 133 | ec.setClusterTolerance(0.5); //设置近邻搜索的半径 134 | ec.setMinClusterSize(5); //设置最小聚类点数 135 | ec.setMaxClusterSize(static_cast(map_vehicle->size())); //设置最大聚类点数 136 | ec.setSearchMethod(tree); 137 | ec.setInputCloud(map_vehicle); 138 | ec.extract(cluster_indices); //从点云中提取聚类，并将点云索引保存在cluster_indices中 139 | 140 | for (std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); it++){ 141 | bool is_static_vehicle = false; 142 | int cluster_size = static_cast(it->indices.size()); 143 | int count_size = 0; 144 | for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++){ 145 | // R半径搜索：在半径r内搜索近邻。 146 | std::vector pointIdxRadiusSearch; // 存储近邻索引 147 | std::vector pointRadiusSquaredDistance; // 存储近邻对应的平均距离 148 | int b = maptree.radiusSearch(map_vehicle->points[*pit], 0.5f, pointIdxRadiusSearch, pointRadiusSquaredDistance); 149 | if (b > 0) 150 | { 151 | for (size_t i = 0; i < pointIdxRadiusSearch.size(); i++) 152 | { 153 | if (std::find(lankmark.begin(), lankmark.end(), map_all->points[pointIdxRadiusSearch[i]].label) != lankmark.end()) 154 | // if (map_all->points[pointIdxRadiusSearch[i]].label == 44 || map_all->points[pointIdxRadiusSearch[i]].label == 48) 155 | { 156 | count_size++; 157 | if (count_size > static_cast(dy_th * static_cast(cluster_size))) 158 | { 159 | is_static_vehicle = true; 160 | break; 161 | } 162 | } 163 | } 164 | } 165 | } 166 | if(is_static_vehicle){ 167 | for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++){ 168 | inliers.emplace_back(vehicle_inliers[*pit]); 169 | } 170 | } 171 | } 172 | } 173 | else{ 174 | for (const auto &point : frame) { 175 | Eigen::Vector4d point_new = point; 176 | const double norm = point.head<3>().norm(); // Euclidean norm 177 | if (norm < max_range && norm > min_range){ 178 | if (norm > label_max_range) point_new[3] = 0.0; 179 | inliers.emplace_back(point_new); 180 | } 181 | } 182 | // std::copy_if(frame.cbegin(), frame.cend(), std::back_inserter(inliers), [&](const auto &pt) { 183 | // Eigen::Vector3d v3point(pt[0], pt[1], pt[2]); 184 | // const double norm = v3point.norm(); // Euclidean norm 185 | // return norm < max_range && norm > min_range; 186 | // }); 187 | } 188 | return inliers; 189 | } 190 | 191 | } // namespace sage_icp 192 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Preprocessing.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace sage_icp { 34 | 35 | /// Voxelize point cloud keeping the original coordinates 36 | std::vector VoxelDownsample(const std::vector &frame, 37 | const std::vector> &voxel_labels, 38 | const std::vector &voxel_size, 39 | double vox_scale 40 | ); 41 | std::vector Preprocess(const std::vector &frame, 42 | double max_range, 43 | double min_range, 44 | double label_max_range, 45 | bool dynamic_vehicle_filter, 46 | double dy_th, 47 | const std::vector &dynamic_labels, 48 | const std::vector &lankmark); 49 | } // namespace sage_icp 50 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Registration.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 "Registration.hpp" 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace Eigen { 34 | using Matrix6d = Eigen::Matrix; 35 | using Matrix3_6d = Eigen::Matrix; 36 | using Vector6d = Eigen::Matrix; 37 | } // namespace Eigen 38 | 39 | namespace { 40 | 41 | inline double square(double x) { return x * x; } 42 | 43 | struct ResultTuple { 44 | ResultTuple() { 45 | JTJ.setZero(); 46 | JTr.setZero(); 47 | } 48 | 49 | ResultTuple operator+(const ResultTuple &other) { 50 | this->JTJ += other.JTJ; 51 | this->JTr += other.JTr; 52 | return *this; 53 | } 54 | 55 | Eigen::Matrix6d JTJ; 56 | Eigen::Vector6d JTr; 57 | }; 58 | 59 | Sophus::SE3d AlignClouds(const std::vector &source, 60 | const std::vector &target, 61 | double th) { 62 | auto compute_jacobian_and_residual = [&](auto i) { 63 | Eigen::Vector3d source3d(source[i][0], source[i][1], source[i][2]); 64 | Eigen::Vector3d target3d(target[i][0], target[i][1], target[i][2]); 65 | const Eigen::Vector3d residual = source3d - target3d; 66 | Eigen::Matrix3_6d J_r; 67 | J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 68 | J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source3d); 69 | return std::make_tuple(J_r, residual); 70 | }; 71 | 72 | const auto &[JTJ, JTr] = tbb::parallel_reduce( 73 | // Range 74 | tbb::blocked_range{0, source.size()}, 75 | // Identity 76 | ResultTuple(), 77 | // 1st Lambda: Parallel computation 78 | [&](const tbb::blocked_range &r, ResultTuple J) -> ResultTuple { 79 | auto Weight = [&](double residual2) { return square(th) / square(th + residual2); }; 80 | auto &[JTJ_private, JTr_private] = J; 81 | for (auto i = r.begin(); i < r.end(); ++i) { 82 | const auto &[J_r, residual] = compute_jacobian_and_residual(i); 83 | const double w = Weight(residual.squaredNorm()); 84 | JTJ_private.noalias() += J_r.transpose() * w * J_r; 85 | JTr_private.noalias() += J_r.transpose() * w * residual; 86 | } 87 | return J; 88 | }, 89 | // 2nd Lambda: Parallel reduction of the private Jacboians 90 | [&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; }); 91 | 92 | const Eigen::Vector6d x = JTJ.ldlt().solve(-JTr); 93 | return Sophus::SE3d::exp(x); 94 | } 95 | 96 | constexpr int MAX_NUM_ITERATIONS_ = 500; 97 | constexpr double ESTIMATION_THRESHOLD_ = 0.0001; 98 | 99 | } // namespace 100 | 101 | namespace sage_icp { 102 | 103 | void TransformPoints(const Sophus::SE3d &T, std::vector &points) { 104 | std::transform(points.cbegin(), points.cend(), points.begin(), 105 | [&](const auto &point) { 106 | Eigen::Vector3d v3point(point[0],point[1],point[2]); 107 | Eigen::Vector3d transformed_v3point = T * v3point; 108 | Eigen::Vector4d v4point(transformed_v3point[0], transformed_v3point[1], transformed_v3point[2], point[3]); 109 | return v4point; 110 | }); 111 | } 112 | 113 | Sophus::SE3d RegisterFrame(const std::vector &frame, 114 | const VoxelHashMap &voxel_map, 115 | const Sophus::SE3d &initial_guess, 116 | double max_correspondence_distance, 117 | double kernel, 118 | double sem_th) { 119 | if (voxel_map.Empty()) return initial_guess; 120 | 121 | // Transform the points to the initial guess 122 | std::vector source = frame; 123 | TransformPoints(initial_guess, source); 124 | 125 | // ICP-loop 126 | Sophus::SE3d T_icp = Sophus::SE3d(); 127 | for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) { 128 | // get correspondences 129 | const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance, sem_th); 130 | // calculate the transformation 131 | auto estimation = AlignClouds(src, tgt, kernel); 132 | // Transform the points 133 | TransformPoints(estimation, source); 134 | // Update iterations 135 | T_icp = estimation * T_icp; 136 | // Termination criteria 137 | if (estimation.log().norm() < ESTIMATION_THRESHOLD_) break; 138 | } 139 | // Spit the final transformation 140 | return T_icp * initial_guess; 141 | } 142 | 143 | } // namespace sage_icp 144 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Registration.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "VoxelHashMap.hpp" 29 | 30 | namespace sage_icp { 31 | 32 | void TransformPoints(const Sophus::SE3d &T, std::vector &points); 33 | 34 | Sophus::SE3d RegisterFrame(const std::vector &frame, 35 | const VoxelHashMap &voxel_map, 36 | const Sophus::SE3d &initial_guess, 37 | double max_correspondence_distance, 38 | double kernel, 39 | double sem_th); 40 | } // namespace sage_icp 41 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Threshold.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Threshold.hpp" 24 | 25 | #include 26 | #include 27 | 28 | namespace { 29 | double ComputeModelError(const Sophus::SE3d &model_deviation, double max_range) { 30 | const double theta = Eigen::AngleAxisd(model_deviation.rotationMatrix()).angle(); 31 | const double delta_rot = 2.0 * max_range * std::sin(theta / 2.0); 32 | const double delta_trans = model_deviation.translation().norm(); 33 | return delta_trans + delta_rot; 34 | } 35 | } // namespace 36 | 37 | namespace sage_icp { 38 | 39 | double AdaptiveThreshold::ComputeThreshold() { 40 | double model_error = ComputeModelError(model_deviation_, max_range_); 41 | if (model_error > min_motion_th_) { 42 | model_error_sse2_ += model_error * model_error; 43 | num_samples_++; 44 | } 45 | 46 | if (num_samples_ < 1) { 47 | return initial_threshold_; 48 | } 49 | return std::sqrt(model_error_sse2_ / num_samples_); 50 | } 51 | 52 | } // namespace sage_icp 53 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/Threshold.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace sage_icp { 28 | 29 | struct AdaptiveThreshold { 30 | explicit AdaptiveThreshold(double initial_threshold, double min_motion_th, double max_range) 31 | : initial_threshold_(initial_threshold), 32 | min_motion_th_(min_motion_th), 33 | max_range_(max_range) {} 34 | 35 | /// Update the current belief of the deviation from the prediction model 36 | inline void UpdateModelDeviation(const Sophus::SE3d ¤t_deviation) { 37 | model_deviation_ = current_deviation; 38 | } 39 | 40 | /// Returns the KISS-ICP adaptive threshold used in registration 41 | double ComputeThreshold(); 42 | 43 | // configurable parameters 44 | double initial_threshold_; 45 | double min_motion_th_; 46 | double max_range_; 47 | 48 | // Local cache for ccomputation 49 | double model_error_sse2_ = 0; 50 | int num_samples_ = 0; 51 | Sophus::SE3d model_deviation_ = Sophus::SE3d(); 52 | }; 53 | 54 | } // namespace sage_icp 55 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/VoxelHashMap.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 "VoxelHashMap.hpp" 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | // This parameters are not intended to be changed, therefore we do not expose it 35 | namespace { 36 | struct ResultTuple { 37 | ResultTuple(std::size_t n) { 38 | source.reserve(n); 39 | target.reserve(n); 40 | } 41 | std::vector source; 42 | std::vector target; 43 | }; 44 | } // namespace 45 | 46 | namespace sage_icp { 47 | 48 | VoxelHashMap::Vector4dVectorTuple VoxelHashMap::GetCorrespondences( 49 | const Vector4dVector &points, double max_correspondance_distance, double th) const { 50 | // Lambda Function to obtain the KNN of one point, maybe refactor 51 | auto GetClosestNeighboor = [&](const Eigen::Vector4d &point) { 52 | auto kx = static_cast(point[0] / voxel_size_); 53 | auto ky = static_cast(point[1] / voxel_size_); 54 | auto kz = static_cast(point[2] / voxel_size_); 55 | std::vector voxels; 56 | voxels.reserve(27); 57 | for (int i = kx - 1; i < kx + 1 + 1; ++i) { 58 | for (int j = ky - 1; j < ky + 1 + 1; ++j) { 59 | for (int k = kz - 1; k < kz + 1 + 1; ++k) { 60 | voxels.emplace_back(i, j, k); 61 | } 62 | } 63 | } 64 | 65 | using Vector4dVector = std::vector; 66 | Vector4dVector neighboors; 67 | neighboors.reserve(27 * (basic_points_per_voxel_ + critical_points_per_voxel_)); 68 | std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) { 69 | auto search = map_.find(voxel); 70 | if (search != map_.end()) { 71 | const auto &points = search->second.points; 72 | if (!points.empty()) { 73 | for (const auto &pointn : points) { 74 | neighboors.emplace_back(pointn); 75 | } 76 | } 77 | } 78 | }); 79 | 80 | Eigen::Vector4d closest_neighbor; 81 | double closest_distance2 = std::numeric_limits::max(); 82 | std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) { 83 | 84 | Eigen::Vector3d v3neighbor(neighbor[0], neighbor[1], neighbor[2]); 85 | Eigen::Vector3d v3point(point[0], point[1], point[2]); 86 | 87 | double distance = (v3neighbor - v3point).squaredNorm(); 88 | if(static_cast(neighbor[3]) == static_cast(point[3]) || static_cast(neighbor[3] * point[3]) == 0) distance = distance * th; 89 | if (distance < closest_distance2) { 90 | closest_neighbor = neighbor; 91 | closest_distance2 = distance; 92 | } 93 | }); 94 | 95 | return closest_neighbor; 96 | }; 97 | using points_iterator = std::vector::const_iterator; 98 | const auto [source, target] = tbb::parallel_reduce( 99 | // Range 100 | tbb::blocked_range{points.cbegin(), points.cend()}, 101 | // Identity 102 | ResultTuple(points.size()), 103 | // 1st lambda: Parallel computation 104 | [max_correspondance_distance, &GetClosestNeighboor]( 105 | const tbb::blocked_range &r, ResultTuple res) -> ResultTuple { 106 | auto &[src, tgt] = res; 107 | src.reserve(r.size()); 108 | tgt.reserve(r.size()); 109 | for (const auto &point : r) { 110 | Eigen::Vector4d closest_neighboors = GetClosestNeighboor(point); 111 | if ((closest_neighboors - point).head<3>().norm() < max_correspondance_distance) { 112 | src.emplace_back(point); 113 | tgt.emplace_back(closest_neighboors); 114 | } 115 | } 116 | return res; 117 | }, 118 | // 2nd lambda: Parallel reduction 119 | [](ResultTuple a, const ResultTuple &b) -> ResultTuple { 120 | auto &[src, tgt] = a; 121 | const auto &[srcp, tgtp] = b; 122 | src.insert(src.end(), // 123 | std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end())); 124 | tgt.insert(tgt.end(), // 125 | std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end())); 126 | return a; 127 | }); 128 | 129 | return std::make_tuple(source, target); 130 | } 131 | 132 | std::vector VoxelHashMap::Pointcloud() const { 133 | std::vector points; 134 | points.reserve((basic_points_per_voxel_+critical_points_per_voxel_) * map_.size()); 135 | for (const auto &[voxel, voxel_block] : map_) { 136 | (void)voxel; 137 | for (const auto &point : voxel_block.points) { 138 | points.push_back(point); 139 | } 140 | } 141 | return points; 142 | } 143 | 144 | void VoxelHashMap::Update(const Vector4dVector &points, const Eigen::Vector3d &origin) { 145 | AddPoints(points); 146 | RemovePointsFarFromLocation(origin); 147 | } 148 | 149 | void VoxelHashMap::Update(const Vector4dVector &points, const Sophus::SE3d &pose) { 150 | Vector4dVector points_transformed(points.size()); 151 | std::transform(points.cbegin(), points.cend(), points_transformed.begin(), 152 | [&](const auto &point) { 153 | Eigen::Vector3d v3point(point[0], point[1], point[2]); 154 | Eigen::Vector3d transformed_v3point = pose * v3point; 155 | Eigen::Vector4d v4point(transformed_v3point[0], transformed_v3point[1], transformed_v3point[2], point[3]); 156 | return v4point; 157 | }); 158 | const Eigen::Vector3d &origin = pose.translation(); 159 | Update(points_transformed, origin); 160 | } 161 | 162 | void VoxelHashMap::AddPoints(const std::vector &points) { 163 | std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { 164 | Eigen::Vector3d v3point(point[0], point[1], point[2]); 165 | auto voxel = Voxel((v3point / voxel_size_).template cast()); 166 | auto search = map_.find(voxel); 167 | if (search != map_.end()) { 168 | auto &voxel_block = search.value(); 169 | voxel_block.AddPoint(point); 170 | } else { 171 | map_.insert({voxel, VoxelBlock{{point}, basic_points_per_voxel_, critical_points_per_voxel_, basic_parts_labels_}}); 172 | } 173 | }); 174 | } 175 | 176 | void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { 177 | for (const auto &[voxel, voxel_block] : map_) { 178 | const auto &pt = voxel_block.points.front().head<3>(); 179 | const auto max_distance2 = max_distance_ * max_distance_; 180 | if ((pt - origin).squaredNorm() > (max_distance2)) { 181 | map_.erase(voxel); 182 | } 183 | } 184 | } 185 | } // namespace sage_icp 186 | -------------------------------------------------------------------------------- /cpp/sage_icp/core/VoxelHashMap.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 | // NOTE: This implementation is heavily inspired in the original CT-ICP VoxelHashMap implementation, 24 | // although it was heavily modifed and drastically simplified, but if you are using this module you 25 | // should at least acknoowledge the work from CT-ICP by giving a star on GitHub 26 | #pragma once 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace sage_icp { 35 | struct VoxelHashMap { 36 | using Vector4dVector = std::vector; 37 | using Vector4dVectorTuple = std::tuple; 38 | using Voxel = Eigen::Vector3i; 39 | struct VoxelBlock { 40 | // buffer of points with a max limit of n_points 41 | std::vector points; 42 | int basic_part_; 43 | int critical_part_; 44 | std::vector basic_parts_labels_; 45 | inline void AddPoint(const Eigen::Vector4d &point) { 46 | if (points.size() < static_cast(basic_part_)) points.emplace_back(point); 47 | else{ 48 | int label = static_cast(point[3]); 49 | if(label == 0); 50 | else if (std::find(basic_parts_labels_.begin(), basic_parts_labels_.end(), label) != basic_parts_labels_.end()) 51 | { 52 | for (auto &p : points) { 53 | if (static_cast(p[3]) == 0) { 54 | p = point; 55 | break; 56 | } 57 | } 58 | } 59 | else{ 60 | if (points.size() < static_cast(basic_part_+critical_part_)) points.emplace_back(point); 61 | else 62 | for (auto &p : points) { 63 | if (static_cast(p[3]) == 0) { 64 | p = point; 65 | break; 66 | } 67 | } 68 | } 69 | } 70 | } 71 | }; 72 | struct VoxelHash { 73 | size_t operator()(const Voxel &voxel) const { 74 | const uint32_t *vec = reinterpret_cast(voxel.data()); 75 | return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791); 76 | } 77 | }; 78 | 79 | explicit VoxelHashMap(double voxel_size, 80 | double max_distance, 81 | int basic_points_per_voxel, 82 | int critical_points_per_voxel, 83 | std::vector basic_parts_labels) 84 | : voxel_size_(voxel_size), 85 | max_distance_(max_distance), 86 | basic_points_per_voxel_(basic_points_per_voxel), 87 | critical_points_per_voxel_{critical_points_per_voxel}, 88 | basic_parts_labels_{basic_parts_labels} {} 89 | 90 | Vector4dVectorTuple GetCorrespondences(const Vector4dVector &points, 91 | double max_correspondance_distance, 92 | double th) const; 93 | inline void Clear() { map_.clear(); } 94 | inline bool Empty() const { return map_.empty(); } 95 | void Update(const std::vector &points, const Eigen::Vector3d &origin); 96 | void Update(const std::vector &points, const Sophus::SE3d &pose); 97 | void AddPoints(const std::vector &points); 98 | void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); 99 | std::vector Pointcloud() const; 100 | 101 | double voxel_size_; 102 | double max_distance_; 103 | int basic_points_per_voxel_; 104 | int critical_points_per_voxel_; 105 | std::vector basic_parts_labels_; 106 | tsl::robin_map map_; 107 | }; 108 | } // namespace sage_icp 109 | -------------------------------------------------------------------------------- /cpp/sage_icp/metrics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(metrics STATIC) 24 | add_library(sage_icp::metrics ALIAS metrics) 25 | target_sources(metrics PRIVATE Metrics.cpp) 26 | target_link_libraries(metrics PUBLIC Eigen3::Eigen) 27 | set_global_target_properties(metrics) 28 | -------------------------------------------------------------------------------- /cpp/sage_icp/metrics/Metrics.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 "Metrics.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // All this not so beatifull C++ functions are taken from kitti-dev-kit 33 | namespace { 34 | double lengths[] = {100, 200, 300, 400, 500, 600, 700, 800}; 35 | int32_t num_lengths = 8; 36 | 37 | struct errors { 38 | int32_t first_frame; 39 | double r_err; 40 | double t_err; 41 | double len; 42 | double speed; 43 | errors(int32_t first_frame, double r_err, double t_err, double len, double speed) 44 | : first_frame(first_frame), r_err(r_err), t_err(t_err), len(len), speed(speed) {} 45 | }; 46 | 47 | std::vector TrajectoryDistances(const std::vector &poses) { 48 | std::vector dist; 49 | dist.push_back(0); 50 | for (uint32_t i = 1; i < poses.size(); i++) { 51 | const Eigen::Matrix4d &P1 = poses[i - 1]; 52 | const Eigen::Matrix4d &P2 = poses[i]; 53 | 54 | double dx = P1(0, 3) - P2(0, 3); 55 | double dy = P1(1, 3) - P2(1, 3); 56 | double dz = P1(2, 3) - P2(2, 3); 57 | 58 | dist.push_back(dist[i - 1] + std::sqrt(dx * dx + dy * dy + dz * dz)); 59 | } 60 | 61 | return dist; 62 | } 63 | 64 | int32_t LastFrameFromSegmentLength(const std::vector &dist, 65 | int32_t first_frame, 66 | double len) { 67 | for (uint32_t i = first_frame; i < dist.size(); i++) { 68 | if (dist[i] > dist[first_frame] + len) { 69 | return i; 70 | } 71 | } 72 | return -1; 73 | } 74 | 75 | double RotationError(const Eigen::Matrix4d &pose_error) { 76 | double a = pose_error(0, 0); 77 | double b = pose_error(1, 1); 78 | double c = pose_error(2, 2); 79 | double d = 0.5 * (a + b + c - 1.0); 80 | return std::acos(std::max(std::min(d, 1.0), -1.0)); 81 | } 82 | 83 | double TranslationError(const Eigen::Matrix4d &pose_error) { 84 | double dx = pose_error(0, 3); 85 | double dy = pose_error(1, 3); 86 | double dz = pose_error(2, 3); 87 | return std::sqrt(dx * dx + dy * dy + dz * dz); 88 | } 89 | 90 | std::vector CalcSequenceErrors(const std::vector &poses_gt, 91 | const std::vector &poses_result) { 92 | // error vector 93 | std::vector err; 94 | 95 | // parameters 96 | int32_t step_size = 10; // every second 97 | 98 | // pre-compute distances (from ground truth as reference) 99 | std::vector dist = TrajectoryDistances(poses_gt); 100 | 101 | // for all start positions do 102 | for (uint32_t first_frame = 0; first_frame < poses_gt.size(); first_frame += step_size) { 103 | // for all segment lengths do 104 | for (int32_t i = 0; i < num_lengths; i++) { 105 | // current length 106 | double len = lengths[i]; 107 | 108 | // compute last frame 109 | int32_t last_frame = LastFrameFromSegmentLength(dist, first_frame, len); 110 | 111 | // continue, if sequence not long enough 112 | if (last_frame == -1) { 113 | continue; 114 | } 115 | 116 | // compute rotational and translational errors 117 | Eigen::Matrix4d pose_delta_gt = poses_gt[first_frame].inverse() * poses_gt[last_frame]; 118 | Eigen::Matrix4d pose_delta_result = 119 | poses_result[first_frame].inverse() * poses_result[last_frame]; 120 | Eigen::Matrix4d pose_error = pose_delta_result.inverse() * pose_delta_gt; 121 | double r_err = RotationError(pose_error); 122 | double t_err = TranslationError(pose_error); 123 | 124 | // compute speed 125 | auto num_frames = static_cast(last_frame - first_frame + 1); 126 | double speed = len / (0.1 * num_frames); 127 | 128 | // write to file 129 | err.emplace_back(first_frame, r_err / len, t_err / len, len, speed); 130 | } 131 | } 132 | 133 | // return error vector 134 | return err; 135 | } 136 | } // namespace 137 | 138 | namespace sage_icp::metrics { 139 | 140 | std::tuple SeqError(const std::vector &poses_gt, 141 | const std::vector &poses_result) { 142 | std::vector err = CalcSequenceErrors(poses_gt, poses_result); 143 | double t_err = 0; 144 | double r_err = 0; 145 | 146 | for (const auto &it : err) { 147 | t_err += it.t_err; 148 | r_err += it.r_err; 149 | } 150 | 151 | double avg_trans_error = 100.0 * (t_err / static_cast(err.size())); 152 | double avg_rot_error = 100.0 * (r_err / static_cast(err.size())) / 3.14 * 180.0; 153 | 154 | return std::make_tuple(avg_trans_error, avg_rot_error); 155 | } 156 | 157 | std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, 158 | const std::vector &poses_result) { 159 | assert(poses_gt.size() == poses_result.size() && 160 | "AbsoluteTrajectoryError| Different number of poses in ground truth and estimate"); 161 | Eigen::MatrixXd source(3, poses_gt.size()); 162 | Eigen::MatrixXd target(3, poses_gt.size()); 163 | const size_t num_poses = poses_gt.size(); 164 | // Align the two trajectories using SVD-ICP (Umeyama algorithm) 165 | for (size_t i = 0; i < num_poses; ++i) { 166 | source.block<3, 1>(0, i) = poses_result[i].block<3, 1>(0, 3); 167 | target.block<3, 1>(0, i) = poses_gt[i].block<3, 1>(0, 3); 168 | } 169 | const Eigen::Matrix4d T_align_trajectories = Eigen::umeyama(source, target, false); 170 | // ATE computation 171 | double ATE_rot = 0, ATE_trans = 0; 172 | for (size_t j = 0; j < num_poses; ++j) { 173 | // Apply alignement matrix 174 | const Eigen::Matrix4d T_estimate = T_align_trajectories * poses_result[j]; 175 | const Eigen::Matrix4d &T_ground_truth = poses_gt[j]; 176 | // Compute error in translation and rotation matrix (ungly) 177 | const Eigen::Matrix3d delta_R = 178 | T_ground_truth.block<3, 3>(0, 0) * T_estimate.block<3, 3>(0, 0).transpose(); 179 | const Eigen::Vector3d delta_t = 180 | T_ground_truth.block<3, 1>(0, 3) - delta_R * T_estimate.block<3, 1>(0, 3); 181 | // Get angular error 182 | const double theta = Eigen::AngleAxisd(delta_R).angle(); 183 | // Sum of Squares 184 | ATE_rot += theta * theta; 185 | ATE_trans += delta_t.squaredNorm(); 186 | } 187 | // Get the RMSE 188 | ATE_rot /= static_cast(num_poses); 189 | ATE_trans /= static_cast(num_poses); 190 | return std::make_tuple(std::sqrt(ATE_rot), std::sqrt(ATE_trans)); 191 | } 192 | } // namespace sage_icp::metrics 193 | -------------------------------------------------------------------------------- /cpp/sage_icp/metrics/Metrics.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | 24 | #ifndef KITTI_UTILS_H_ 25 | #define KITTI_UTILS_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace sage_icp::metrics { 32 | 33 | std::tuple SeqError(const std::vector &poses_gt, 34 | const std::vector &poses_result); 35 | 36 | std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, 37 | const std::vector &poses_result); 38 | } // namespace sage_icp::metrics 39 | 40 | #endif // KITTI_UTILS_H_ 41 | -------------------------------------------------------------------------------- /cpp/sage_icp/pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 NeSC-IV 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 | add_library(pipeline STATIC) 23 | add_library(sage_icp::pipeline ALIAS pipeline) 24 | target_sources(pipeline PRIVATE sageICP.cpp) 25 | target_link_libraries(pipeline PUBLIC sage_icp::core) 26 | set_global_target_properties(pipeline) 27 | -------------------------------------------------------------------------------- /cpp/sage_icp/pipeline/sageICP.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 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 | #include "sageICP.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include "sage_icp/core/Deskew.hpp" 30 | #include "sage_icp/core/Preprocessing.hpp" 31 | #include "sage_icp/core/Registration.hpp" 32 | #include "sage_icp/core/VoxelHashMap.hpp" 33 | 34 | namespace sage_icp::pipeline { 35 | 36 | sageICP::Vector4dVectorTuple sageICP::RegisterFrame(const std::vector &frame, 37 | const std::vector ×tamps) { 38 | const auto &deskew_frame = [&]() -> std::vector { 39 | if (!config_.deskew) return frame; 40 | // TODO(Nacho) Add some asserts here to sanitize the timestamps 41 | 42 | // If not enough poses for the estimation, do not de-skew 43 | const size_t N = poses().size(); 44 | if (N <= 2) return frame; 45 | 46 | // Estimate linear and angular velocities 47 | const auto &start_pose = poses_[N - 2]; 48 | const auto &finish_pose = poses_[N - 1]; 49 | return DeSkewScan(frame, timestamps, start_pose, finish_pose); 50 | }(); 51 | return RegisterFrame(deskew_frame); 52 | } 53 | 54 | sageICP::Vector4dVectorTuple sageICP::RegisterFrame(const std::vector &frame) { 55 | // Preprocess the input cloud 56 | auto preprocess_start = std::chrono::high_resolution_clock::now(); 57 | const auto &cropped_frame = Preprocess(frame, 58 | config_.max_range, 59 | config_.min_range, 60 | config_.label_max_range, 61 | config_.dynamic_vehicle_filter, 62 | config_.dynamic_vehicle_filter_th, 63 | config_.voxel_labels[config_.dynamic_vehicle_voxid], 64 | config_.dynamic_remove_lankmark 65 | ); 66 | 67 | // Voxelize 68 | const auto &[source, frame_downsample] = Voxelize(cropped_frame); // source is double downsample pc，frame_downsample is single downsample pc 69 | 70 | // Get motion prediction and adaptive_threshold 71 | const double sigma = GetAdaptiveThreshold(); 72 | 73 | // Compute initial_guess for ICP 74 | const auto prediction = GetPredictionModel(); // according to the last two poses to predict the current pose 75 | const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d(); // last pose 76 | const auto initial_guess = last_pose * prediction; // initial guess 77 | 78 | // Run icp 79 | auto start = std::chrono::high_resolution_clock::now(); 80 | const Sophus::SE3d new_pose = sage_icp::RegisterFrame(source, // 81 | sem_map_, // 82 | initial_guess, // 83 | 3.0 * sigma, // max_correspondence_distance 84 | sigma / 3.0, 85 | config_.sem_th); // kernel 86 | auto end = std::chrono::high_resolution_clock::now(); 87 | std::chrono::duration elapsed = end - start; 88 | std::chrono::duration elapsed_all = end - preprocess_start; 89 | 90 | const auto model_deviation = initial_guess.inverse() * new_pose; // deviation between initial guess and new pose 91 | adaptive_threshold_.UpdateModelDeviation(model_deviation); 92 | sem_map_.Update(frame_downsample, new_pose); // update map 93 | poses_.push_back(new_pose); // update poses 94 | return {source, elapsed.count(), elapsed_all.count()}; 95 | } 96 | 97 | sageICP::Vector4dVectorTuple2 sageICP::Voxelize(const std::vector &frame) const { 98 | const auto frame_downsample = sage_icp::VoxelDownsample(frame, config_.voxel_labels, config_.voxel_size, 0.5); 99 | const auto source = sage_icp::VoxelDownsample(frame_downsample, config_.voxel_labels, config_.voxel_size, 1.5); 100 | return {source, frame_downsample}; 101 | } 102 | 103 | double sageICP::GetAdaptiveThreshold() { 104 | if (!HasMoved()) { 105 | return config_.initial_threshold; 106 | } 107 | return adaptive_threshold_.ComputeThreshold(); 108 | } 109 | 110 | Sophus::SE3d sageICP::GetPredictionModel() const { 111 | Sophus::SE3d pred = Sophus::SE3d(); 112 | const size_t N = poses_.size(); 113 | if (N < 2) return pred; 114 | return poses_[N - 2].inverse() * poses_[N - 1]; 115 | } 116 | 117 | bool sageICP::HasMoved() { 118 | if (poses_.empty()) return false; 119 | const double motion = (poses_.front().inverse() * poses_.back()).translation().norm(); 120 | return motion > 5.0 * config_.min_motion_th; 121 | } 122 | 123 | std::vector sageICP::TransformToLastFrame(const Sophus::SE3d &last_pose, 124 | const Sophus::SE3d ¤t_pose, 125 | const std::vector &points){ 126 | std::vector trans_points = points; 127 | sage_icp::TransformPoints(last_pose.inverse() * current_pose, trans_points); 128 | return trans_points; 129 | } 130 | 131 | } // namespace sage_icp::pipeline 132 | -------------------------------------------------------------------------------- /cpp/sage_icp/pipeline/sageICP.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include "sage_icp/core/Deskew.hpp" 29 | #include "sage_icp/core/Threshold.hpp" 30 | #include "sage_icp/core/VoxelHashMap.hpp" 31 | // #include 32 | // #include 33 | // #include 34 | // #include 35 | // #include 36 | #include 37 | 38 | namespace sage_icp::pipeline { 39 | struct sageConfig { 40 | std::vector> voxel_labels; 41 | std::vector voxel_size; 42 | // map params 43 | double voxel_size_map = 1.0; 44 | 45 | double max_range = 100.0; 46 | double min_range = 5.0; 47 | double label_max_range = 50.0; 48 | double local_map_range = 100.0; 49 | int basic_points_per_voxel = 20; 50 | int critical_points_per_voxel = 20; 51 | std::vector basic_parts_labels; 52 | 53 | // th parms 54 | double min_motion_th = 0.1; 55 | double initial_threshold = 2.0; 56 | double sem_th = 0.4; 57 | 58 | // Motion compensation 59 | bool deskew = false; 60 | 61 | bool dynamic_vehicle_filter = false; 62 | double dynamic_vehicle_filter_th = 0.5; 63 | int dynamic_vehicle_voxid = 5; 64 | std::vector dynamic_remove_lankmark; 65 | }; 66 | 67 | class sageICP { 68 | public: 69 | using Vector4dVector = std::vector; 70 | using Vector4dVectorTuple = std::tuple; 71 | using Vector4dVectorTuple2 = std::tuple; 72 | 73 | explicit sageICP(const sageConfig &config) 74 | : config_(config), 75 | sem_map_(config.voxel_size_map, config_.local_map_range, config_.basic_points_per_voxel, config_.critical_points_per_voxel, config_.basic_parts_labels), 76 | adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} 77 | 78 | sageICP() : sageICP(sageConfig{}) {} 79 | 80 | Vector4dVectorTuple RegisterFrame(const std::vector &frame); 81 | Vector4dVectorTuple RegisterFrame(const std::vector &frame, 82 | const std::vector ×tamps); 83 | Vector4dVectorTuple2 Voxelize(const std::vector &frame) const; 84 | double GetAdaptiveThreshold(); 85 | Sophus::SE3d GetPredictionModel() const; 86 | bool HasMoved(); 87 | std::vector TransformToLastFrame(const Sophus::SE3d &last_pose, 88 | const Sophus::SE3d ¤t_pose, 89 | const std::vector &points); 90 | 91 | // Extra C++ API to facilitate ROS debugging 92 | std::vector LocalMap() const { return sem_map_.Pointcloud(); }; 93 | std::vector poses() const { return poses_; }; 94 | bool reinitialize() { 95 | poses_.clear(); 96 | adaptive_threshold_ = AdaptiveThreshold(config_.initial_threshold, config_.min_motion_th, config_.max_range); 97 | sem_map_.Clear(); 98 | return true; 99 | }; 100 | 101 | private: 102 | // SAGE-ICP pipeline modules 103 | std::vector poses_; 104 | sageConfig config_; 105 | // local map 106 | VoxelHashMap sem_map_; 107 | AdaptiveThreshold adaptive_threshold_; 108 | 109 | }; 110 | 111 | } // namespace sage_icp::pipeline 112 | -------------------------------------------------------------------------------- /eval/kittiraw_pub.py: -------------------------------------------------------------------------------- 1 | import pykitti 2 | import os 3 | import sys 4 | import argparse 5 | import rclpy 6 | from rclpy.clock import Clock 7 | from rclpy.node import Node 8 | from rclpy.exceptions import ParameterNotDeclaredException 9 | from rcl_interfaces.msg import ParameterType, ParameterDescriptor 10 | from sensor_msgs.msg import PointCloud2, Imu, NavSatFix, CameraInfo, PointField 11 | from std_msgs.msg import Header 12 | from ament_index_python import get_package_share_directory,get_package_prefix 13 | import time 14 | import open3d as o3d 15 | from std_srvs.srv import Empty 16 | from rclpy.duration import Duration 17 | from rclpy.serialization import serialize_message 18 | from example_interfaces.msg import Int32 19 | import rclpy 20 | from rclpy.time import Time 21 | 22 | import rosbag2_py 23 | import sys 24 | sys.dont_write_bytecode = True 25 | import math 26 | from numpy.linalg import inv 27 | import tf_transformations 28 | import cv2 29 | # from cv_bridge import CvBridge 30 | import progressbar 31 | from tf2_msgs.msg import TFMessage 32 | from datetime import datetime 33 | from std_msgs.msg import Header 34 | from sensor_msgs_py import point_cloud2 as pcl2 # point_cloud2.create_cloud() 函数是sensor_msgs.msg.PointCloud2消息的一个帮助函数，它将一系列点的x、y、z坐标和其他属性打包到点云消息中。 35 | from geometry_msgs.msg import TransformStamped, TwistStamped, Transform, PoseStamped 36 | from nav_msgs.msg import Odometry 37 | import numpy as np 38 | import glob 39 | from tqdm import tqdm 40 | import matplotlib.pyplot as plt 41 | from example_interfaces.srv import AddTwoInts 42 | import pandas as pd 43 | 44 | def takeSort(elem): 45 | return int(elem) 46 | 47 | from kiss_icp.pybind import kiss_icp_pybind 48 | correct_kitti_scan = lambda frame: np.asarray( 49 | kiss_icp_pybind._correct_kitti_scan(kiss_icp_pybind._Vector3dVector(frame)) 50 | ) 51 | 52 | 53 | def inv_t(transform): 54 | 55 | R = transform[0:3, 0:3] 56 | t = transform[0:3, 3] 57 | t_inv = -1*R.T.dot(t) 58 | transform_inv = np.eye(4) 59 | transform_inv[0:3, 0:3] = R.T 60 | transform_inv[0:3, 3] = t_inv 61 | 62 | return transform_inv 63 | 64 | 65 | def get_velo_data(kitti, velo_frame_id, iter): 66 | # velo_data_dir = os.path.join(kitti.data_path, 'data') 67 | # velo_filenames = sorted(os.listdir(velo_data_dir)) 68 | 69 | # datatimes = kitti.timestamps 70 | # dt = datatimes[iter] 71 | # veloname = kitti.velo_files[iter] 72 | # if dt is None: 73 | # continue 74 | 75 | # velo_filename = os.path.join(velo_data_dir, veloname) 76 | 77 | # veloscan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4) 78 | # points = veloscan.astype(np.float64) 79 | 80 | points = np.array(kitti.get_velo(iter)).reshape(-1, 4) 81 | veloscan3 = points[:, :3].astype(np.float64) 82 | velodyne_correct = correct_kitti_scan(veloscan3) 83 | points[:, :3] = velodyne_correct 84 | 85 | header = Header() 86 | header.frame_id = velo_frame_id 87 | time = Clock().now() 88 | header.stamp = time.to_msg() 89 | 90 | fields =[PointField(name='x', offset=0, datatype=PointField.FLOAT32, count = 1), 91 | PointField(name='y', offset=4, datatype=PointField.FLOAT32, count = 1), 92 | PointField(name='z', offset=8, datatype=PointField.FLOAT32, count = 1), 93 | PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count = 1)] 94 | 95 | pcl_msg = pcl2.create_cloud(header, fields, points) 96 | return pcl_msg 97 | 98 | def get_pose_msg(kitti, master_frame_id, iter): 99 | # posestamp只存储位姿信息 100 | pose = kitti.oxts[iter].T_w_imu 101 | # 将imu坐标系下的位姿转换到lidar坐标系下 102 | pose = np.matmul(np.linalg.inv(kitti.oxts[0].T_w_imu), pose) 103 | p = PoseStamped() 104 | p.header.frame_id = master_frame_id 105 | time = Clock().now() 106 | p.header.stamp = time.to_msg() 107 | 108 | t = pose[0:3, 3] 109 | q = tf_transformations.quaternion_from_matrix(pose) 110 | 111 | p.pose.position.x = t[0] 112 | p.pose.position.y = t[1] 113 | p.pose.position.z = t[2] 114 | 115 | q_n = q / np.linalg.norm(q) # 四元数归一化 116 | 117 | p.pose.orientation.x = q_n[0] 118 | p.pose.orientation.y = q_n[1] 119 | p.pose.orientation.z = q_n[2] 120 | p.pose.orientation.w = q_n[3] 121 | return p 122 | 123 | class Listener(Node): 124 | def __init__(self,name,seq,basedir,date,drive): 125 | super().__init__(name) 126 | self.iter = 0 127 | self.get_pose = True 128 | self.odom_group = [] 129 | self.gt_group = [] 130 | self.sequence_number = str(seq) 131 | self.scanlabel_bool = 0 132 | 133 | self.data = pykitti.raw(basedir, date, drive) 134 | 135 | self.world_frame_id = 'map' 136 | self.velo_frame_id = 'velodyne' 137 | # calibration = read_calib_file(os.path.join(self.kitti.data_path, 'calib.txt')) 138 | # ground_truth_file_name = "{}.txt".format(self.sequence_number) 139 | 140 | # self.ground_truth = read_poses_file(os.path.join("/media/oliver/Elements SE/dataset/kitti_360/data_poses", self.kitti.seq, "poses.txt")) 141 | self.lenth = self.data.timestamps.__len__() 142 | self.odomx = [] # 定义一个 x 轴的空列表用来接收动态的数据 143 | self.odomy = [] # 定义一个 y 轴的空列表用来接收动态的数据 144 | self.gtx = [] # 定义一个 x 轴的空列表用来接收动态的数据 145 | self.gty = [] # 定义一个 y 轴的空列表用来接收动态的数据 146 | plt.ion() # 开启一个画图的窗口 147 | self.subscriber = self.create_subscription(Odometry, 148 | "odometry", 149 | self.sub_callback, 150 | 10) 151 | self.lidar_pub = self.create_publisher(PointCloud2, '/velodyne_points',1) 152 | self.pose_pub = self.create_publisher(PoseStamped, '/ground_truth', 1) 153 | # 创建service,用于更新seq 154 | self.cli = self.create_client(AddTwoInts, 'reinit') 155 | while not self.cli.wait_for_service(timeout_sec=1.0): 156 | self.get_logger().info('service not available, waiting again...') 157 | self.req = AddTwoInts.Request() 158 | 159 | # self.first_pose_inv = inv(self.ground_truth[int(self.kitti.velo_files[0].split(".")[0])]) 160 | 161 | timer_period = 0.2 # 定时器周期为0.1s 162 | self.timer = self.create_timer(timer_period,self.timer_callback) #创建定时器 163 | self.get_logger().info("Publisher is started, publishing kitti %s to %s" % (seq,"/velodyne_points")) 164 | self.pbar = tqdm(total=self.lenth) 165 | 166 | def send_request(self, a, b): 167 | self.req.a = a 168 | self.req.b = b 169 | self.future = self.cli.call_async(self.req) 170 | try: 171 | rclpy.spin_until_future_complete(self, self.future, timeout_sec=10.0) # 设置超时时间为10秒 172 | except Exception as e: 173 | self.get_logger().error('Service call failed %r' % (e,)) 174 | return None 175 | if self.future is not None and self.future.done(): 176 | if (self.future.result().sum == 1): 177 | self.get_logger().info('Finish cleaning!') 178 | return self.future.result() 179 | 180 | def sub_callback(self,msg): 181 | odom = [msg.header.stamp.sec, msg.header.stamp.nanosec, msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w] 182 | self.odom_group.append(odom) 183 | self.odomx.append(msg.pose.pose.position.x) 184 | self.odomy.append(msg.pose.pose.position.y) 185 | plt.clf() # 清除之前画的图 186 | plt.plot(self.odomx,self.odomy,color = "g") # 画出当前 ax 列表和 ay 列表中的值的图形 187 | plt.plot(self.gtx, self.gty, color='r') 188 | plt.xlabel('x') 189 | plt.ylabel('y') 190 | plt.pause(0.01) # 暂停一秒 191 | plt.ioff() # 关闭画图的窗口 192 | self.get_pose = True 193 | 194 | 195 | def timer_callback(self): 196 | if self.get_pose: 197 | if(self.iter == self.lenth): 198 | print('Convertion is done, start saving pose ...') 199 | plt.clf() # 清除之前画的图 200 | plt.plot(self.odomx,self.odomy,color = "g") # 画出当前 ax 列表和 ay 列表中的值的图形 201 | plt.plot(self.gtx, self.gty, color='r') 202 | plt.xlabel('x') 203 | plt.ylabel('y') 204 | saving_path = "/home/oliver/catkin_ros2/src/kiss-icp/results/seq" + str(int(self.sequence_number)) 205 | if not os.path.exists(saving_path): # 判断是否存在文件夹如果不存在则创建为文件夹 206 | os.makedirs(saving_path) 207 | plt.savefig(saving_path + "/sequence" + self.sequence_number + '.png', bbox_inches='tight') 208 | with open(saving_path + "/path.txt", 'w') as f: 209 | for odom in self.odom_group: 210 | f.writelines(str(odom[0]) + '.' + str(odom[1]) + ' ' + str(odom[2]) + ' ' + str(odom[3]) + ' ' + str(odom[4]) + ' ' + str(odom[5]) + ' ' + str(odom[6]) + ' ' + str(odom[7]) + ' ' + str(odom[8]) + '\n') 211 | with open(saving_path + "/gt_path.txt", 'w') as f: 212 | for gt in self.gt_group: 213 | f.writelines(str(gt[0]) + '.' + str(gt[1]) + ' ' + str(gt[2]) + ' ' + str(gt[3]) + ' ' + str(gt[4]) + ' ' + str(gt[5]) + ' ' + str(gt[6]) + ' ' + str(gt[7]) + ' ' + str(gt[8]) + '\n') 214 | response = self.send_request(int(self.sequence_number), 0) 215 | # plt.ioff() # 关闭画图的窗口 216 | # 结束进程 217 | raise SystemExit # <--- here is we exit the node 218 | 219 | # if self.scanlabel_bool == 1: 220 | # pointcloud = get_velo_data_with_label(self.kitti, self.velo_frame_id, self.iter) 221 | 222 | # elif self.scanlabel_bool == 0: 223 | pointcloud = get_velo_data(self.data, self.velo_frame_id, self.iter) 224 | pose = get_pose_msg(self.data, self.world_frame_id, self.iter) # posestamped: ground_truth to map, odom: ground_truth to map 225 | 226 | self.lidar_pub.publish(pointcloud) 227 | self.pose_pub.publish(pose) 228 | 229 | gt_pose = [pose.header.stamp.sec, pose.header.stamp.nanosec, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] 230 | self.gt_group.append(gt_pose) 231 | self.gtx.append(pose.pose.position.x) 232 | self.gty.append(pose.pose.position.y) 233 | self.iter +=1 234 | self.pbar.update(1) 235 | self.get_pose = False 236 | 237 | 238 | 239 | def main(args=None): 240 | rclpy.init() 241 | basedir = "/media/oliver/Elements SE/dataset/KITTI" 242 | if not os.path.exists(basedir): 243 | print('KITTI dataset not found') 244 | return 245 | date_seq = [] 246 | drive_seq = [] 247 | for name in os.listdir(basedir): 248 | # 获取name前三位字符 249 | if (name[0:4] != "2011"): continue 250 | 251 | if os.path.isdir(os.path.join(basedir, name)): 252 | drive_seq_temp = [] 253 | for drive in os.listdir(os.path.join(basedir, name)): 254 | if os.path.isdir(os.path.join(basedir, name, drive)): 255 | drive_name = drive[-9:-5] 256 | date_seq.append(name) 257 | drive_seq_temp.append(drive_name) 258 | # 排序drive_seq_temp 259 | drive_seq_temp.sort(key=takeSort) 260 | drive_seq = drive_seq + drive_seq_temp 261 | 262 | for seq in range(30, 42): #[30,41],37 263 | if seq == 35: 264 | continue 265 | print('Start publishing sequence %s' % seq) 266 | dataset_pub = Listener('dataset_publisher',seq,basedir,date_seq[seq-30], drive_seq[seq-30]) 267 | try: 268 | rclpy.spin(dataset_pub) 269 | except SystemExit: # <--- process the exception 270 | rclpy.logging.get_logger("Quitting").info('Done') 271 | dataset_pub.destroy_node() 272 | time.sleep(5) 273 | rclpy.shutdown() # 停止节点 274 | 275 | if __name__ =='__main__': 276 | # talker() 277 | main() 278 | # os.killpg(os.getpgid(os.getpid()), signal.SIGKILL) 279 | 280 | 281 | -------------------------------------------------------------------------------- /figure/pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NeSC-IV/sage-icp/69d46b8e52a4a0cae6227e6569a623b0fb0d905b/figure/pipeline.png -------------------------------------------------------------------------------- /ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 NeSC-IV 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 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(sage_icp VERSION 1.0.0 LANGUAGES CXX) 25 | 26 | set(ignore ${CATKIN_INSTALL_INTO_PREFIX_ROOT}) 27 | set(CMAKE_BUILD_TYPE Release) 28 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 29 | set(CMAKE_CXX_STANDARD 17) 30 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 31 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/sage_icp/) 32 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/sage_icp ${CMAKE_CURRENT_BINARY_DIR}/sage_icp) 33 | else() 34 | message(FATAL_ERROR "SAGE-ICP not found, please clone the repository with --recursive flag") 35 | endif() 36 | 37 | if("$ENV{ROS_VERSION}" STREQUAL "1") 38 | message(FATAL_ERROR "SAGE-ICP-ROS1 not support!") 39 | 40 | elseif("$ENV{ROS_VERSION}" STREQUAL "2") 41 | message(STATUS "SAGE-ICP-ROS2 wrapper will be compiled") 42 | 43 | find_package(ament_cmake REQUIRED) 44 | find_package(nav_msgs REQUIRED) 45 | find_package(rclcpp REQUIRED) 46 | find_package(sensor_msgs REQUIRED) 47 | find_package(visualization_msgs REQUIRED) 48 | find_package(tf2_ros REQUIRED) 49 | find_package(example_interfaces REQUIRED) 50 | 51 | # ROS2 node 52 | add_executable(odometry_node ros2/OdometryServer.cpp) 53 | target_include_directories(odometry_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) 54 | target_link_libraries(odometry_node sage_icp::pipeline) 55 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -msse3 -std=c++17") # debug 56 | install(TARGETS odometry_node RUNTIME DESTINATION lib/${PROJECT_NAME}) 57 | install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) 58 | ament_target_dependencies(odometry_node rclcpp nav_msgs sensor_msgs visualization_msgs tf2_ros example_interfaces) 59 | ament_package() 60 | else() 61 | message(FATAL_ERROR "catkin or colcon not found SAGE-ICP-ROS disabled") 62 | endif() 63 | -------------------------------------------------------------------------------- /ros/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 NeSC-IV 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /ros/README.md: -------------------------------------------------------------------------------- 1 | # KISS-ICP ROS wrappers 2 | 3 | This node is just an application example on how to use the KISS-ICP C++ API. It's still considered 4 | work in progress and we are very happy to receive any contribution from the comunity 👼 5 | 6 | https://user-images.githubusercontent.com/21349875/214578180-b1d2431c-8fff-440e-aa6e-99a1d85989b5.mp4 7 | 8 | ## ROS2 9 | 10 | ### How to build 11 | 12 | You should not need any extra dependency, just clone and build: 13 | 14 | ```sh 15 | git clone https://github.com/PRBonn/kiss-icp 16 | colcon build 17 | source ./install/setup.bash 18 | ``` 19 | 20 | ### How to run 21 | 22 | The only required argument to provide is the **topic name** so KISS-ICP knows which PointCloud2 to proces: 23 | 24 | ```sh 25 | ros2 launch kiss_icp odometry.launch.py bagfile:= topic:= 26 | ``` 27 | 28 | You can optionally launch the node with any bagfile, and play the bagfiles on a different shell: 29 | 30 | ```sh 31 | ros2 launch kiss_icp odometry.launch.py topic:= 32 | ``` 33 | 34 | and then, 35 | 36 | ```sh 37 | ros2 bag play *.bag 38 | ``` 39 | 40 | ## ROS1 41 | 42 | ### How to build 43 | 44 | You should not need any extra dependency, just clone and build: 45 | 46 | ```sh 47 | cd ~/catkin_ws/ 48 | git clone https://github.com/PRBonn/kiss-icp 49 | catkin build 50 | source devel/setup.bash 51 | ``` 52 | 53 | ### How to run 54 | 55 | The only required argument to provide is the **topic name** so KISS-ICP knows which PointCloud2 to proces: 56 | 57 | ```sh 58 | roslaunch kiss_icp odometry.launch bagfile:= topic:= 59 | ``` 60 | 61 | You can optionally launch the node with any bagfile, and play the bagfiles on a different shell: 62 | 63 | ```sh 64 | roslaunch kiss_icp odometry.launch topic:= 65 | ``` 66 | 67 | and then, 68 | 69 | ```sh 70 | rosbag play *.bag 71 | ``` 72 | 73 | ## Out of source builds 74 | 75 | Good news! If you don't have git or you don't need to change the core KISS-ICP library, you can just 76 | copy paste this folder into your ROS1/ROS2 workspace and build as usual. The build system will fetch 77 | the latest stable release for you. 78 | 79 | ## Looking how to run KITTI on ROS along with KISS-ICP? 80 | 81 | I believe you could use the python API instead, to avoid converting all the data to a format that is 82 | not the original one. If you still insist in doing it, I've created a separate repository for this, 83 | just clone and build [this package](https://github.com/nachovizzo/kiss_icp_kitti) 84 | -------------------------------------------------------------------------------- /ros/launch/odometry.launch.py: -------------------------------------------------------------------------------- 1 | 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 4 | from launch.conditions import IfCondition 5 | from launch.substitutions import ( 6 | LaunchConfiguration, 7 | PathJoinSubstitution, 8 | PythonExpression, 9 | ) 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions import FindPackageShare 12 | import os 13 | import yaml 14 | class Basic_config(): 15 | def __init__(self, color_yaml="semantic-kitti.yaml"): 16 | # ROS2 parameters 17 | self.pc_topic: str = "/sem_points" # input pointcloud topic 18 | self.base_frame: str = "base_link" 19 | self.odom_frame: str = "odom" 20 | self.odom_topic: str = "/sage_icp/odometry" 21 | self.trajectory_topic: str = "/sage_icp/trajectory" 22 | 23 | self.publish_frame: bool = True # publish frame in odom and map for visualization 24 | self.frame_topic: str = "/sage_icp/frame" 25 | self.local_map_topic: str = "/sage_icp/local_map" 26 | 27 | self.sub_ground_truth: bool = True 28 | self.gt_topic: str = "/ground_truth" # input gt_topic 29 | self.gt_trajectory_topic: str = "/sage_icp/gt_trajectory" 30 | 31 | # Pointcloud pre-process 32 | self.deskew: bool = False # Point cloud deskew 33 | self.max_range: float = 100.0 # pointcloud max range 34 | self.min_range: float = 5.0 # pointcloud min range 35 | self.label_max_range: float = 50.0 # label max range 36 | 37 | # Voxel grid filter 38 | self.voxel_labels = [ 39 | [40, 44, 48, 49], # road 40 | [50, 51, 52], # building 41 | [70, 72], # plant 42 | [60, 71, 80, 81, 99], # object 43 | [0], # unlabelled 44 | [10, 11, 13, 15, 16, 18, 20], # vehicle 45 | ] 46 | self.voxel_labels_str: str = self.pack_2d_array(self.voxel_labels) # pack 2d array to string 47 | self.voxel_size: list = [0.6, 1.0, 0.9, 0.8, 1.0, 0.6] 48 | 49 | # Dynamic cars remove 50 | self.dynamic_vehicle_filter: bool = True 51 | self.dynamic_vehicle_filter_th: float = 0.5 52 | self.dynamic_vehicle_voxid: int = 5 # voxid in voxel_labels 53 | self.dynamic_remove_lankmark: list = [44, 48] # landmark labels for dynamic remove 54 | 55 | # Map 56 | self.voxel_size_map: float = 0.8 57 | self.local_map_range: float = 100.0 58 | self.basic_points_per_voxel: int = 20 # basic part 59 | self.critical_points_per_voxel: int = 20 # critical part 60 | self.basic_parts_labels: list = [40, 44, 48, 49, 50, 70, 72] # basic parts labels, others are critical parts 61 | 62 | # Semantic assisted association 63 | self.sem_th: float = 0.4 64 | 65 | # KISS-ICP Adaptive threshold 66 | self.initial_threshold: float = 2.0 67 | self.min_motion_th: float = 0.1 68 | 69 | # color map 70 | self.current_pkg = FindPackageShare("sage_icp") 71 | current_pkg_path_str = self.current_pkg.find("sage_icp") 72 | label_mapping = os.path.join(current_pkg_path_str, "launch", color_yaml) 73 | with open(label_mapping, 'r') as stream: 74 | semkittiyaml = yaml.safe_load(stream) 75 | color_map_bgr = semkittiyaml['color_map'] 76 | self.color_list = [] 77 | for key, value in color_map_bgr.items(): 78 | b, g, r = value 79 | rgb = (int(r) << 16) | (int(g) << 8) | int(b) 80 | self.color_list.append([key,rgb]) 81 | self.color_list_str: str = self.pack_2d_array(self.color_list) 82 | 83 | # Key Frames extract 84 | self.publish_key_frame: bool = False # publish key frame 85 | self.key_frame_topic: str = "/sage_icp/key_frame" 86 | self.key_marker_topic: str = "/sage_icp/key_marker" 87 | self.key_frame_overlap: float = 0.5 # map sample overlap 88 | self.key_frame_bounds: list = [[-51.2, 51.2], [-51.2, 51.2], [-4, 2.4]] # Point Cloud Boundaries, used for generate occupancy map 89 | self.key_frame_bounds_str: str = self.pack_2d_array(self.key_frame_bounds) 90 | self.key_frame_occ_size: list = [128, 128] # H*W, occ resolution 91 | 92 | # RVIZ2 and ROS2 bag play 93 | self.visualize: str = "true" # must be string 94 | self.bagfile: str = "" 95 | 96 | def pack_2d_array(self, array_2d): 97 | return ';'.join([','.join(map(str, row)) for row in array_2d]) 98 | 99 | 100 | def generate_launch_description(): 101 | 102 | sage_icp_config = Basic_config() 103 | # SAGE-ICP Node 104 | sage_icp_node = Node( 105 | package="sage_icp", 106 | executable="odometry_node", 107 | name="odometry_node", 108 | output="screen", 109 | # remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], 110 | parameters=[ 111 | { 112 | "pc_topic": sage_icp_config.pc_topic, 113 | "base_frame": sage_icp_config.base_frame, 114 | "odom_frame": sage_icp_config.odom_frame, 115 | "odom_topic": sage_icp_config.odom_topic, 116 | "trajectory_topic": sage_icp_config.trajectory_topic, 117 | "publish_frame": sage_icp_config.publish_frame, 118 | "frame_topic": sage_icp_config.frame_topic, 119 | "local_map_topic": sage_icp_config.local_map_topic, 120 | "sub_ground_truth": sage_icp_config.sub_ground_truth, 121 | "gt_topic": sage_icp_config.gt_topic, 122 | "gt_trajectory_topic_": sage_icp_config.gt_trajectory_topic, 123 | "deskew": sage_icp_config.deskew, 124 | "max_range": sage_icp_config.max_range, 125 | "min_range": sage_icp_config.min_range, 126 | "label_max_range": sage_icp_config.label_max_range, 127 | "voxel_labels_str": sage_icp_config.voxel_labels_str, 128 | "voxel_size": sage_icp_config.voxel_size, 129 | "dynamic_vehicle_filter": sage_icp_config.dynamic_vehicle_filter, 130 | "dynamic_vehicle_filter_th": sage_icp_config.dynamic_vehicle_filter_th, 131 | "dynamic_vehicle_voxid": sage_icp_config.dynamic_vehicle_voxid, 132 | "dynamic_remove_lankmark": sage_icp_config.dynamic_remove_lankmark, 133 | "voxel_size_map": sage_icp_config.voxel_size_map, 134 | "local_map_range": sage_icp_config.local_map_range, 135 | "basic_points_per_voxel": sage_icp_config.basic_points_per_voxel, 136 | "critical_points_per_voxel": sage_icp_config.critical_points_per_voxel, 137 | "basic_parts_labels": sage_icp_config.basic_parts_labels, 138 | "sem_th": sage_icp_config.sem_th, 139 | "initial_threshold": sage_icp_config.initial_threshold, 140 | "min_motion_th": sage_icp_config.min_motion_th, 141 | "color_list_str": sage_icp_config.color_list_str, 142 | "publish_key_frame": sage_icp_config.publish_key_frame, 143 | "key_frame_topic": sage_icp_config.key_frame_topic, 144 | "key_marker_topic": sage_icp_config.key_marker_topic, 145 | "key_frame_overlap": sage_icp_config.key_frame_overlap, 146 | "key_frame_bounds_str": sage_icp_config.key_frame_bounds_str, 147 | "key_frame_occ_size": sage_icp_config.key_frame_occ_size, 148 | } 149 | ], 150 | ) 151 | # RVIZ2 152 | rviz2_node = Node( 153 | package="rviz2", 154 | executable="rviz2", 155 | output={"both": "log"}, 156 | arguments=["-d", PathJoinSubstitution([sage_icp_config.current_pkg, "rviz", "sage_icp_ros2.rviz"])], 157 | condition=IfCondition(sage_icp_config.visualize), 158 | ) 159 | # ROS2 bag play 160 | bag_play = ExecuteProcess( 161 | cmd=["ros2", "bag", "play", sage_icp_config.bagfile], 162 | output="screen", 163 | condition=IfCondition( 164 | PythonExpression(["'", sage_icp_config.bagfile, "' != ''"]) 165 | ), 166 | ) 167 | return LaunchDescription( 168 | [ 169 | sage_icp_node, 170 | rviz2_node, 171 | bag_play, 172 | ] 173 | ) 174 | 175 | 176 | if __name__ == "__main__": 177 | generate_launch_description() -------------------------------------------------------------------------------- /ros/launch/odometry_360.launch.py: -------------------------------------------------------------------------------- 1 | 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 4 | from launch.conditions import IfCondition 5 | from launch.substitutions import ( 6 | LaunchConfiguration, 7 | PathJoinSubstitution, 8 | PythonExpression, 9 | ) 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions import FindPackageShare 12 | import os 13 | import yaml 14 | class Basic_config(): 15 | def __init__(self, color_yaml="semantic-kitti.yaml"): 16 | # ROS2 parameters 17 | self.pc_topic: str = "/sem_points" # input pointcloud topic 18 | self.base_frame: str = "base_link" 19 | self.odom_frame: str = "odom" 20 | self.odom_topic: str = "/sage_icp/odometry" 21 | self.trajectory_topic: str = "/sage_icp/trajectory" 22 | 23 | self.publish_frame: bool = True # publish frame in odom and map for visualization 24 | self.frame_topic: str = "/sage_icp/frame" 25 | self.local_map_topic: str = "/sage_icp/local_map" 26 | 27 | self.sub_ground_truth: bool = True 28 | self.gt_topic: str = "/ground_truth" # input gt_topic 29 | self.gt_trajectory_topic: str = "/sage_icp/gt_trajectory" 30 | 31 | # Pointcloud pre-process 32 | self.deskew: bool = False # Point cloud deskew 33 | self.max_range: float = 100.0 # pointcloud max range 34 | self.min_range: float = 5.0 # pointcloud min range 35 | self.label_max_range: float = 50.0 # label max range 36 | 37 | # Voxel grid filter 38 | self.voxel_labels = [ 39 | [40, 44, 48, 49], # road 40 | [50, 51, 52], # building 41 | [70, 72], # plant 42 | [60, 71, 80, 81, 99], # object 43 | [0], # unlabelled 44 | [10, 11, 13, 15, 16, 18, 20], # vehicle 45 | ] 46 | self.voxel_labels_str: str = self.pack_2d_array(self.voxel_labels) # pack 2d array to string 47 | self.voxel_size: list = [1.0, 0.5, 1.0, 0.5, 1.0, 0.5] 48 | 49 | # Dynamic cars remove 50 | self.dynamic_vehicle_filter: bool = True 51 | self.dynamic_vehicle_filter_th: float = 0.5 52 | self.dynamic_vehicle_voxid: int = 5 # voxid in voxel_labels 53 | self.dynamic_remove_lankmark: list = [44, 48] # landmark labels for dynamic remove 54 | 55 | # Map 56 | self.voxel_size_map: float = 1.0 57 | self.local_map_range: float = 100.0 58 | self.basic_points_per_voxel: int = 20 # basic part 59 | self.critical_points_per_voxel: int = 20 # critical part 60 | self.basic_parts_labels: list = [40, 44, 48, 49, 50, 70, 72] # basic parts labels, others are critical parts 61 | 62 | # Semantic assisted association 63 | self.sem_th: float = 0.8 64 | 65 | # KISS-ICP Adaptive threshold 66 | self.initial_threshold: float = 2.0 67 | self.min_motion_th: float = 0.1 68 | 69 | # color map 70 | self.current_pkg = FindPackageShare("sage_icp") 71 | current_pkg_path_str = self.current_pkg.find("sage_icp") 72 | label_mapping = os.path.join(current_pkg_path_str, "launch", color_yaml) 73 | with open(label_mapping, 'r') as stream: 74 | semkittiyaml = yaml.safe_load(stream) 75 | color_map_bgr = semkittiyaml['color_map'] 76 | self.color_list = [] 77 | for key, value in color_map_bgr.items(): 78 | b, g, r = value 79 | rgb = (int(r) << 16) | (int(g) << 8) | int(b) 80 | self.color_list.append([key,rgb]) 81 | self.color_list_str: str = self.pack_2d_array(self.color_list) 82 | 83 | # Key Frames extract 84 | self.publish_key_frame: bool = False # publish key frame 85 | self.key_frame_topic: str = "/sage_icp/key_frame" 86 | self.key_marker_topic: str = "/sage_icp/key_marker" 87 | self.key_frame_overlap: float = 0.5 # map sample overlap 88 | self.key_frame_bounds: list = [[-51.2, 51.2], [-51.2, 51.2], [-4, 2.4]] # Point Cloud Boundaries, used for generate occupancy map 89 | self.key_frame_bounds_str: str = self.pack_2d_array(self.key_frame_bounds) 90 | self.key_frame_occ_size: list = [128, 128] # H*W, occ resolution 91 | 92 | # RVIZ2 and ROS2 bag play 93 | self.visualize: str = "true" # must be string 94 | self.bagfile: str = "" 95 | 96 | def pack_2d_array(self, array_2d): 97 | return ';'.join([','.join(map(str, row)) for row in array_2d]) 98 | 99 | 100 | def generate_launch_description(): 101 | 102 | sage_icp_config = Basic_config() 103 | # SAGE-ICP Node 104 | sage_icp_node = Node( 105 | package="sage_icp", 106 | executable="odometry_node", 107 | name="odometry_node", 108 | output="screen", 109 | # remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], 110 | parameters=[ 111 | { 112 | "pc_topic": sage_icp_config.pc_topic, 113 | "base_frame": sage_icp_config.base_frame, 114 | "odom_frame": sage_icp_config.odom_frame, 115 | "odom_topic": sage_icp_config.odom_topic, 116 | "trajectory_topic": sage_icp_config.trajectory_topic, 117 | "publish_frame": sage_icp_config.publish_frame, 118 | "frame_topic": sage_icp_config.frame_topic, 119 | "local_map_topic": sage_icp_config.local_map_topic, 120 | "sub_ground_truth": sage_icp_config.sub_ground_truth, 121 | "gt_topic": sage_icp_config.gt_topic, 122 | "gt_trajectory_topic_": sage_icp_config.gt_trajectory_topic, 123 | "deskew": sage_icp_config.deskew, 124 | "max_range": sage_icp_config.max_range, 125 | "min_range": sage_icp_config.min_range, 126 | "label_max_range": sage_icp_config.label_max_range, 127 | "voxel_labels_str": sage_icp_config.voxel_labels_str, 128 | "voxel_size": sage_icp_config.voxel_size, 129 | "dynamic_vehicle_filter": sage_icp_config.dynamic_vehicle_filter, 130 | "dynamic_vehicle_filter_th": sage_icp_config.dynamic_vehicle_filter_th, 131 | "dynamic_vehicle_voxid": sage_icp_config.dynamic_vehicle_voxid, 132 | "dynamic_remove_lankmark": sage_icp_config.dynamic_remove_lankmark, 133 | "voxel_size_map": sage_icp_config.voxel_size_map, 134 | "local_map_range": sage_icp_config.local_map_range, 135 | "basic_points_per_voxel": sage_icp_config.basic_points_per_voxel, 136 | "critical_points_per_voxel": sage_icp_config.critical_points_per_voxel, 137 | "basic_parts_labels": sage_icp_config.basic_parts_labels, 138 | "sem_th": sage_icp_config.sem_th, 139 | "initial_threshold": sage_icp_config.initial_threshold, 140 | "min_motion_th": sage_icp_config.min_motion_th, 141 | "color_list_str": sage_icp_config.color_list_str, 142 | "publish_key_frame": sage_icp_config.publish_key_frame, 143 | "key_frame_topic": sage_icp_config.key_frame_topic, 144 | "key_marker_topic": sage_icp_config.key_marker_topic, 145 | "key_frame_overlap": sage_icp_config.key_frame_overlap, 146 | "key_frame_bounds_str": sage_icp_config.key_frame_bounds_str, 147 | "key_frame_occ_size": sage_icp_config.key_frame_occ_size, 148 | } 149 | ], 150 | ) 151 | # RVIZ2 152 | rviz2_node = Node( 153 | package="rviz2", 154 | executable="rviz2", 155 | output={"both": "log"}, 156 | arguments=["-d", PathJoinSubstitution([sage_icp_config.current_pkg, "rviz", "sage_icp_ros2.rviz"])], 157 | condition=IfCondition(sage_icp_config.visualize), 158 | ) 159 | # ROS2 bag play 160 | bag_play = ExecuteProcess( 161 | cmd=["ros2", "bag", "play", sage_icp_config.bagfile], 162 | output="screen", 163 | condition=IfCondition( 164 | PythonExpression(["'", sage_icp_config.bagfile, "' != ''"]) 165 | ), 166 | ) 167 | return LaunchDescription( 168 | [ 169 | sage_icp_node, 170 | rviz2_node, 171 | bag_play, 172 | ] 173 | ) 174 | 175 | 176 | if __name__ == "__main__": 177 | generate_launch_description() -------------------------------------------------------------------------------- /ros/launch/odometry_gt.launch.py: -------------------------------------------------------------------------------- 1 | 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 4 | from launch.conditions import IfCondition 5 | from launch.substitutions import ( 6 | LaunchConfiguration, 7 | PathJoinSubstitution, 8 | PythonExpression, 9 | ) 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions import FindPackageShare 12 | import os 13 | import yaml 14 | class Basic_config(): 15 | def __init__(self, color_yaml="semantic-kitti.yaml"): 16 | # ROS2 parameters 17 | self.pc_topic: str = "/label_points" # input pointcloud topic 18 | self.base_frame: str = "base_link" 19 | self.odom_frame: str = "odom" 20 | self.odom_topic: str = "/sage_icp/odometry" 21 | self.trajectory_topic: str = "/sage_icp/trajectory" 22 | 23 | self.publish_frame: bool = True # publish frame in odom and map for visualization 24 | self.frame_topic: str = "/sage_icp/frame" 25 | self.local_map_topic: str = "/sage_icp/local_map" 26 | 27 | self.sub_ground_truth: bool = True 28 | self.gt_topic: str = "/ground_truth" # input gt_topic 29 | self.gt_trajectory_topic: str = "/sage_icp/gt_trajectory" 30 | 31 | # Pointcloud pre-process 32 | self.deskew: bool = False # Point cloud deskew 33 | self.max_range: float = 100.0 # pointcloud max range 34 | self.min_range: float = 5.0 # pointcloud min range 35 | self.label_max_range: float = 50.0 # label max range 36 | 37 | # Voxel grid filter 38 | self.voxel_labels = [ 39 | [40, 44, 48, 49], # road 40 | [50, 51, 52], # building 41 | [70, 72], # plant 42 | [60, 71, 80, 81, 99], # object 43 | [0], # unlabelled 44 | [10, 11, 13, 15, 16, 18, 20], # vehicle 45 | ] 46 | self.voxel_labels_str: str = self.pack_2d_array(self.voxel_labels) # pack 2d array to string 47 | self.voxel_size: list = [0.6, 1.0, 0.9, 0.8, 1.0, 0.6] 48 | 49 | # Dynamic cars remove 50 | self.dynamic_vehicle_filter: bool = False 51 | self.dynamic_vehicle_filter_th: float = 0.1 52 | self.dynamic_vehicle_voxid: int = 5 # voxid in voxel_labels 53 | self.dynamic_remove_lankmark: list = [44, 48] # landmark labels for dynamic remove 54 | 55 | # Map 56 | self.voxel_size_map: float = 0.8 57 | self.local_map_range: float = 100.0 58 | self.basic_points_per_voxel: int = 20 # basic part 59 | self.critical_points_per_voxel: int = 20 # critical part 60 | self.basic_parts_labels: list = [40, 44, 48, 49, 50, 70, 72] # basic parts labels, others are critical parts 61 | 62 | # Semantic assisted association 63 | self.sem_th: float = 0.05 64 | 65 | # KISS-ICP Adaptive threshold 66 | self.initial_threshold: float = 2.0 67 | self.min_motion_th: float = 0.1 68 | 69 | # color map 70 | self.current_pkg = FindPackageShare("sage_icp") 71 | current_pkg_path_str = self.current_pkg.find("sage_icp") 72 | label_mapping = os.path.join(current_pkg_path_str, "launch", color_yaml) 73 | with open(label_mapping, 'r') as stream: 74 | semkittiyaml = yaml.safe_load(stream) 75 | color_map_bgr = semkittiyaml['color_map'] 76 | self.color_list = [] 77 | for key, value in color_map_bgr.items(): 78 | b, g, r = value 79 | rgb = (int(r) << 16) | (int(g) << 8) | int(b) 80 | self.color_list.append([key,rgb]) 81 | self.color_list_str: str = self.pack_2d_array(self.color_list) 82 | 83 | # Key Frames extract 84 | self.publish_key_frame: bool = False # publish key frame 85 | self.key_frame_topic: str = "/sage_icp/key_frame" 86 | self.key_marker_topic: str = "/sage_icp/key_marker" 87 | self.key_frame_overlap: float = 0.5 # map sample overlap 88 | self.key_frame_bounds: list = [[-51.2, 51.2], [-51.2, 51.2], [-4, 2.4]] # Point Cloud Boundaries, used for generate occupancy map 89 | self.key_frame_bounds_str: str = self.pack_2d_array(self.key_frame_bounds) 90 | self.key_frame_occ_size: list = [128, 128] # H*W, occ resolution 91 | 92 | # RVIZ2 and ROS2 bag play 93 | self.visualize: str = "true" # must be string 94 | self.bagfile: str = "" 95 | 96 | def pack_2d_array(self, array_2d): 97 | return ';'.join([','.join(map(str, row)) for row in array_2d]) 98 | 99 | 100 | def generate_launch_description(): 101 | 102 | sage_icp_config = Basic_config() 103 | # SAGE-ICP Node 104 | sage_icp_node = Node( 105 | package="sage_icp", 106 | executable="odometry_node", 107 | name="odometry_node", 108 | output="screen", 109 | # remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], 110 | parameters=[ 111 | { 112 | "pc_topic": sage_icp_config.pc_topic, 113 | "base_frame": sage_icp_config.base_frame, 114 | "odom_frame": sage_icp_config.odom_frame, 115 | "odom_topic": sage_icp_config.odom_topic, 116 | "trajectory_topic": sage_icp_config.trajectory_topic, 117 | "publish_frame": sage_icp_config.publish_frame, 118 | "frame_topic": sage_icp_config.frame_topic, 119 | "local_map_topic": sage_icp_config.local_map_topic, 120 | "sub_ground_truth": sage_icp_config.sub_ground_truth, 121 | "gt_topic": sage_icp_config.gt_topic, 122 | "gt_trajectory_topic_": sage_icp_config.gt_trajectory_topic, 123 | "deskew": sage_icp_config.deskew, 124 | "max_range": sage_icp_config.max_range, 125 | "min_range": sage_icp_config.min_range, 126 | "label_max_range": sage_icp_config.label_max_range, 127 | "voxel_labels_str": sage_icp_config.voxel_labels_str, 128 | "voxel_size": sage_icp_config.voxel_size, 129 | "dynamic_vehicle_filter": sage_icp_config.dynamic_vehicle_filter, 130 | "dynamic_vehicle_filter_th": sage_icp_config.dynamic_vehicle_filter_th, 131 | "dynamic_vehicle_voxid": sage_icp_config.dynamic_vehicle_voxid, 132 | "dynamic_remove_lankmark": sage_icp_config.dynamic_remove_lankmark, 133 | "voxel_size_map": sage_icp_config.voxel_size_map, 134 | "local_map_range": sage_icp_config.local_map_range, 135 | "basic_points_per_voxel": sage_icp_config.basic_points_per_voxel, 136 | "critical_points_per_voxel": sage_icp_config.critical_points_per_voxel, 137 | "basic_parts_labels": sage_icp_config.basic_parts_labels, 138 | "sem_th": sage_icp_config.sem_th, 139 | "initial_threshold": sage_icp_config.initial_threshold, 140 | "min_motion_th": sage_icp_config.min_motion_th, 141 | "color_list_str": sage_icp_config.color_list_str, 142 | "publish_key_frame": sage_icp_config.publish_key_frame, 143 | "key_frame_topic": sage_icp_config.key_frame_topic, 144 | "key_marker_topic": sage_icp_config.key_marker_topic, 145 | "key_frame_overlap": sage_icp_config.key_frame_overlap, 146 | "key_frame_bounds_str": sage_icp_config.key_frame_bounds_str, 147 | "key_frame_occ_size": sage_icp_config.key_frame_occ_size, 148 | } 149 | ], 150 | ) 151 | # RVIZ2 152 | rviz2_node = Node( 153 | package="rviz2", 154 | executable="rviz2", 155 | output={"both": "log"}, 156 | arguments=["-d", PathJoinSubstitution([sage_icp_config.current_pkg, "rviz", "sage_icp_ros2.rviz"])], 157 | condition=IfCondition(sage_icp_config.visualize), 158 | ) 159 | # ROS2 bag play 160 | bag_play = ExecuteProcess( 161 | cmd=["ros2", "bag", "play", sage_icp_config.bagfile], 162 | output="screen", 163 | condition=IfCondition( 164 | PythonExpression(["'", sage_icp_config.bagfile, "' != ''"]) 165 | ), 166 | ) 167 | return LaunchDescription( 168 | [ 169 | sage_icp_node, 170 | rviz2_node, 171 | bag_play, 172 | ] 173 | ) 174 | 175 | 176 | if __name__ == "__main__": 177 | generate_launch_description() -------------------------------------------------------------------------------- /ros/launch/odometry_raw.launch.py: -------------------------------------------------------------------------------- 1 | 2 | from launch import LaunchDescription 3 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 4 | from launch.conditions import IfCondition 5 | from launch.substitutions import ( 6 | LaunchConfiguration, 7 | PathJoinSubstitution, 8 | PythonExpression, 9 | ) 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions import FindPackageShare 12 | import os 13 | import yaml 14 | class Basic_config(): 15 | def __init__(self, color_yaml="semantic-kitti.yaml"): 16 | # ROS2 parameters 17 | self.pc_topic: str = "/sem_points" # input pointcloud topic 18 | self.base_frame: str = "base_link" 19 | self.odom_frame: str = "odom" 20 | self.odom_topic: str = "/sage_icp/odometry" 21 | self.trajectory_topic: str = "/sage_icp/trajectory" 22 | 23 | self.publish_frame: bool = True # publish frame in odom and map for visualization 24 | self.frame_topic: str = "/sage_icp/frame" 25 | self.local_map_topic: str = "/sage_icp/local_map" 26 | 27 | self.sub_ground_truth: bool = True 28 | self.gt_topic: str = "/ground_truth" # input gt_topic 29 | self.gt_trajectory_topic: str = "/sage_icp/gt_trajectory" 30 | 31 | # Pointcloud pre-process 32 | self.deskew: bool = False # Point cloud deskew 33 | self.max_range: float = 100.0 # pointcloud max range 34 | self.min_range: float = 5.0 # pointcloud min range 35 | self.label_max_range: float = 50.0 # label max range 36 | 37 | # Voxel grid filter 38 | self.voxel_labels = [ 39 | [40, 44, 48, 49], # road 40 | [50, 51, 52], # building 41 | [70, 72], # plant 42 | [60, 71, 80, 81, 99], # object 43 | [0], # unlabelled 44 | [10, 11, 13, 15, 16, 18, 20], # vehicle 45 | ] 46 | self.voxel_labels_str: str = self.pack_2d_array(self.voxel_labels) # pack 2d array to string 47 | self.voxel_size: list = [1.2, 1.0, 1.2, 0.2, 1.0, 0.5] 48 | 49 | # Dynamic cars remove 50 | self.dynamic_vehicle_filter: bool = True 51 | self.dynamic_vehicle_filter_th: float = 0.5 52 | self.dynamic_vehicle_voxid: int = 5 # voxid in voxel_labels 53 | self.dynamic_remove_lankmark: list = [44, 48] # landmark labels for dynamic remove 54 | 55 | # Map 56 | self.voxel_size_map: float = 1.0 57 | self.local_map_range: float = 100.0 58 | self.basic_points_per_voxel: int = 20 # basic part 59 | self.critical_points_per_voxel: int = 20 # critical part 60 | self.basic_parts_labels: list = [40, 44, 48, 49, 50, 70, 72] # basic parts labels, others are critical parts 61 | 62 | # Semantic assisted association 63 | self.sem_th: float = 0.2 64 | 65 | # KISS-ICP Adaptive threshold 66 | self.initial_threshold: float = 2.0 67 | self.min_motion_th: float = 0.1 68 | 69 | # color map 70 | self.current_pkg = FindPackageShare("sage_icp") 71 | current_pkg_path_str = self.current_pkg.find("sage_icp") 72 | label_mapping = os.path.join(current_pkg_path_str, "launch", color_yaml) 73 | with open(label_mapping, 'r') as stream: 74 | semkittiyaml = yaml.safe_load(stream) 75 | color_map_bgr = semkittiyaml['color_map'] 76 | self.color_list = [] 77 | for key, value in color_map_bgr.items(): 78 | b, g, r = value 79 | rgb = (int(r) << 16) | (int(g) << 8) | int(b) 80 | self.color_list.append([key,rgb]) 81 | self.color_list_str: str = self.pack_2d_array(self.color_list) 82 | 83 | # Key Frames extract 84 | self.publish_key_frame: bool = False # publish key frame 85 | self.key_frame_topic: str = "/sage_icp/key_frame" 86 | self.key_marker_topic: str = "/sage_icp/key_marker" 87 | self.key_frame_overlap: float = 0.5 # map sample overlap 88 | self.key_frame_bounds: list = [[-51.2, 51.2], [-51.2, 51.2], [-4, 2.4]] # Point Cloud Boundaries, used for generate occupancy map 89 | self.key_frame_bounds_str: str = self.pack_2d_array(self.key_frame_bounds) 90 | self.key_frame_occ_size: list = [128, 128] # H*W, occ resolution 91 | 92 | # RVIZ2 and ROS2 bag play 93 | self.visualize: str = "true" # must be string 94 | self.bagfile: str = "" 95 | 96 | def pack_2d_array(self, array_2d): 97 | return ';'.join([','.join(map(str, row)) for row in array_2d]) 98 | 99 | 100 | def generate_launch_description(): 101 | 102 | sage_icp_config = Basic_config() 103 | # SAGE-ICP Node 104 | sage_icp_node = Node( 105 | package="sage_icp", 106 | executable="odometry_node", 107 | name="odometry_node", 108 | output="screen", 109 | # remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], 110 | parameters=[ 111 | { 112 | "pc_topic": sage_icp_config.pc_topic, 113 | "base_frame": sage_icp_config.base_frame, 114 | "odom_frame": sage_icp_config.odom_frame, 115 | "odom_topic": sage_icp_config.odom_topic, 116 | "trajectory_topic": sage_icp_config.trajectory_topic, 117 | "publish_frame": sage_icp_config.publish_frame, 118 | "frame_topic": sage_icp_config.frame_topic, 119 | "local_map_topic": sage_icp_config.local_map_topic, 120 | "sub_ground_truth": sage_icp_config.sub_ground_truth, 121 | "gt_topic": sage_icp_config.gt_topic, 122 | "gt_trajectory_topic_": sage_icp_config.gt_trajectory_topic, 123 | "deskew": sage_icp_config.deskew, 124 | "max_range": sage_icp_config.max_range, 125 | "min_range": sage_icp_config.min_range, 126 | "label_max_range": sage_icp_config.label_max_range, 127 | "voxel_labels_str": sage_icp_config.voxel_labels_str, 128 | "voxel_size": sage_icp_config.voxel_size, 129 | "dynamic_vehicle_filter": sage_icp_config.dynamic_vehicle_filter, 130 | "dynamic_vehicle_filter_th": sage_icp_config.dynamic_vehicle_filter_th, 131 | "dynamic_vehicle_voxid": sage_icp_config.dynamic_vehicle_voxid, 132 | "dynamic_remove_lankmark": sage_icp_config.dynamic_remove_lankmark, 133 | "voxel_size_map": sage_icp_config.voxel_size_map, 134 | "local_map_range": sage_icp_config.local_map_range, 135 | "basic_points_per_voxel": sage_icp_config.basic_points_per_voxel, 136 | "critical_points_per_voxel": sage_icp_config.critical_points_per_voxel, 137 | "basic_parts_labels": sage_icp_config.basic_parts_labels, 138 | "sem_th": sage_icp_config.sem_th, 139 | "initial_threshold": sage_icp_config.initial_threshold, 140 | "min_motion_th": sage_icp_config.min_motion_th, 141 | "color_list_str": sage_icp_config.color_list_str, 142 | "publish_key_frame": sage_icp_config.publish_key_frame, 143 | "key_frame_topic": sage_icp_config.key_frame_topic, 144 | "key_marker_topic": sage_icp_config.key_marker_topic, 145 | "key_frame_overlap": sage_icp_config.key_frame_overlap, 146 | "key_frame_bounds_str": sage_icp_config.key_frame_bounds_str, 147 | "key_frame_occ_size": sage_icp_config.key_frame_occ_size, 148 | } 149 | ], 150 | ) 151 | # RVIZ2 152 | rviz2_node = Node( 153 | package="rviz2", 154 | executable="rviz2", 155 | output={"both": "log"}, 156 | arguments=["-d", PathJoinSubstitution([sage_icp_config.current_pkg, "rviz", "sage_icp_ros2.rviz"])], 157 | condition=IfCondition(sage_icp_config.visualize), 158 | ) 159 | # ROS2 bag play 160 | bag_play = ExecuteProcess( 161 | cmd=["ros2", "bag", "play", sage_icp_config.bagfile], 162 | output="screen", 163 | condition=IfCondition( 164 | PythonExpression(["'", sage_icp_config.bagfile, "' != ''"]) 165 | ), 166 | ) 167 | return LaunchDescription( 168 | [ 169 | sage_icp_node, 170 | rviz2_node, 171 | bag_play, 172 | ] 173 | ) 174 | 175 | 176 | if __name__ == "__main__": 177 | generate_launch_description() -------------------------------------------------------------------------------- /ros/launch/range_odom.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('rangenet_pp'), 20 | 'launch/ros2_rangenet.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /ros/launch/range_odom_360.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('rangenet_pp'), 20 | 'launch/ros2_rangenet.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry_360.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /ros/launch/range_odom_raw.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('rangenet_pp'), 20 | 'launch/ros2_rangenet.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry_raw.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) -------------------------------------------------------------------------------- /ros/launch/sem_odom.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('cylinder3d_ros2'), 20 | 'launch/cylinder3d_ros2.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /ros/launch/sem_odom_360.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('cylinder3d_ros2'), 20 | 'launch/cylinder3d_ros2.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry_360.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /ros/launch/sem_odom_raw.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python import get_package_share_directory,get_package_prefix 3 | from launch import LaunchDescription 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.actions import GroupAction,ExecuteProcess 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import TextSubstitution 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import PushRosNamespace 12 | 13 | 14 | def generate_launch_description(): 15 | # include another launch file 16 | sem_launch_include = IncludeLaunchDescription( 17 | PythonLaunchDescriptionSource( 18 | os.path.join( 19 | get_package_share_directory('cylinder3d_ros2'), 20 | 'launch/cylinder3d_ros2.launch.py')) 21 | ) 22 | 23 | odom_launch_include = IncludeLaunchDescription( 24 | PythonLaunchDescriptionSource( 25 | os.path.join( 26 | get_package_share_directory('sage_icp'), 27 | 'launch/odometry_raw.launch.py')) 28 | ) 29 | 30 | 31 | return LaunchDescription( 32 | [ 33 | sem_launch_include, 34 | odom_launch_include, 35 | ] 36 | ) -------------------------------------------------------------------------------- /ros/launch/semantic-kitti.yaml: -------------------------------------------------------------------------------- 1 | # This file is covered by the LICENSE file in the root of this project. 2 | labels: 3 | 0 : "unlabeled" 4 | 1 : "outlier" 5 | 10: "car" 6 | 11: "bicycle" 7 | 13: "bus" 8 | 15: "motorcycle" 9 | 16: "on-rails" 10 | 18: "truck" 11 | 20: "other-vehicle" 12 | 30: "person" 13 | 31: "bicyclist" 14 | 32: "motorcyclist" 15 | 40: "road" 16 | 44: "parking" 17 | 48: "sidewalk" 18 | 49: "other-ground" 19 | 50: "building" 20 | 51: "fence" 21 | 52: "other-structure" 22 | 60: "lane-marking" 23 | 70: "vegetation" 24 | 71: "trunk" 25 | 72: "terrain" 26 | 80: "pole" 27 | 81: "traffic-sign" 28 | 99: "other-object" 29 | 252: "moving-car" 30 | 253: "moving-bicyclist" 31 | 254: "moving-person" 32 | 255: "moving-motorcyclist" 33 | 256: "moving-on-rails" 34 | 257: "moving-bus" 35 | 258: "moving-truck" 36 | 259: "moving-other-vehicle" 37 | color_map: # bgr 38 | 0 : [0, 0, 0] 39 | 1 : [0, 0, 255] 40 | 10: [245, 150, 100] 41 | 11: [245, 230, 100] 42 | 13: [250, 80, 100] 43 | 15: [150, 60, 30] 44 | 16: [255, 0, 0] 45 | 18: [180, 30, 80] 46 | 20: [255, 0, 0] 47 | 30: [30, 30, 255] 48 | 31: [200, 40, 255] 49 | 32: [90, 30, 150] 50 | 40: [255, 0, 255] 51 | 44: [255, 150, 255] 52 | 48: [75, 0, 75] 53 | 49: [75, 0, 175] 54 | 50: [0, 200, 255] 55 | 51: [50, 120, 255] 56 | 52: [0, 150, 255] 57 | 60: [170, 255, 150] 58 | 70: [0, 175, 0] 59 | 71: [0, 60, 135] 60 | 72: [80, 240, 150] 61 | 80: [150, 240, 255] 62 | 81: [0, 0, 255] 63 | 99: [255, 255, 50] 64 | 252: [245, 150, 100] 65 | 256: [255, 0, 0] 66 | 253: [200, 40, 255] 67 | 254: [30, 30, 255] 68 | 255: [90, 30, 150] 69 | 257: [250, 80, 100] 70 | 258: [180, 30, 80] 71 | 259: [255, 0, 0] 72 | content: # as a ratio with the total number of points 73 | 0: 0.018889854628292943 74 | 1: 0.0002937197336781505 75 | 10: 0.040818519255974316 76 | 11: 0.00016609538710764618 77 | 13: 2.7879693665067774e-05 78 | 15: 0.00039838616015114444 79 | 16: 0.0 80 | 18: 0.0020633612104619787 81 | 20: 0.0016218197275284021 82 | 30: 0.00017698551338515307 83 | 31: 1.1065903904919655e-08 84 | 32: 5.532951952459828e-09 85 | 40: 0.1987493871255525 86 | 44: 0.014717169549888214 87 | 48: 0.14392298360372 88 | 49: 0.0039048553037472045 89 | 50: 0.1326861944777486 90 | 51: 0.0723592229456223 91 | 52: 0.002395131480328884 92 | 60: 4.7084144280367186e-05 93 | 70: 0.26681502148037506 94 | 71: 0.006035012012626033 95 | 72: 0.07814222006271769 96 | 80: 0.002855498193863172 97 | 81: 0.0006155958086189918 98 | 99: 0.009923127583046915 99 | 252: 0.001789309418528068 100 | 253: 0.00012709999297008662 101 | 254: 0.00016059776092534436 102 | 255: 3.745553104802113e-05 103 | 256: 0.0 104 | 257: 0.00011351574470342043 105 | 258: 0.00010157861367183268 106 | 259: 4.3840131989471124e-05 107 | # classes that are indistinguishable from single scan or inconsistent in 108 | # ground truth are mapped to their closest equivalent 109 | learning_map: 110 | 0 : 0 # "unlabeled" 111 | 1 : 0 # "outlier" mapped to "unlabeled" --------------------------mapped 112 | 10: 1 # "car" 113 | 11: 2 # "bicycle" 114 | 13: 5 # "bus" mapped to "other-vehicle" --------------------------mapped 115 | 15: 3 # "motorcycle" 116 | 16: 5 # "on-rails" mapped to "other-vehicle" ---------------------mapped 117 | 18: 4 # "truck" 118 | 20: 5 # "other-vehicle" 119 | 30: 6 # "person" 120 | 31: 7 # "bicyclist" 121 | 32: 8 # "motorcyclist" 122 | 40: 9 # "road" 123 | 44: 10 # "parking" 124 | 48: 11 # "sidewalk" 125 | 49: 12 # "other-ground" 126 | 50: 13 # "building" 127 | 51: 14 # "fence" 128 | 52: 0 # "other-structure" mapped to "unlabeled" ------------------mapped 129 | 60: 9 # "lane-marking" to "road" ---------------------------------mapped 130 | 70: 15 # "vegetation" 131 | 71: 16 # "trunk" 132 | 72: 17 # "terrain" 133 | 80: 18 # "pole" 134 | 81: 19 # "traffic-sign" 135 | 99: 0 # "other-object" to "unlabeled" ----------------------------mapped 136 | 252: 1 # "moving-car" to "car" ------------------------------------mapped 137 | 253: 7 # "moving-bicyclist" to "bicyclist" ------------------------mapped 138 | 254: 6 # "moving-person" to "person" ------------------------------mapped 139 | 255: 8 # "moving-motorcyclist" to "motorcyclist" ------------------mapped 140 | 256: 5 # "moving-on-rails" mapped to "other-vehicle" --------------mapped 141 | 257: 5 # "moving-bus" mapped to "other-vehicle" -------------------mapped 142 | 258: 4 # "moving-truck" to "truck" --------------------------------mapped 143 | 259: 5 # "moving-other"-vehicle to "other-vehicle" ----------------mapped 144 | learning_map_inv: # inverse of previous map 145 | 0: 0 # "unlabeled", and others ignored 146 | 1: 10 # "car" 147 | 2: 11 # "bicycle" 148 | 3: 15 # "motorcycle" 149 | 4: 18 # "truck" 150 | 5: 20 # "other-vehicle" 151 | 6: 30 # "person" 152 | 7: 31 # "bicyclist" 153 | 8: 32 # "motorcyclist" 154 | 9: 40 # "road" 155 | 10: 44 # "parking" 156 | 11: 48 # "sidewalk" 157 | 12: 49 # "other-ground" 158 | 13: 50 # "building" 159 | 14: 51 # "fence" 160 | 15: 70 # "vegetation" 161 | 16: 71 # "trunk" 162 | 17: 72 # "terrain" 163 | 18: 80 # "pole" 164 | 19: 81 # "traffic-sign" 165 | learning_ignore: # Ignore classes 166 | 0: True # "unlabeled", and others ignored 167 | 1: False # "car" 168 | 2: False # "bicycle" 169 | 3: False # "motorcycle" 170 | 4: False # "truck" 171 | 5: False # "other-vehicle" 172 | 6: False # "person" 173 | 7: False # "bicyclist" 174 | 8: False # "motorcyclist" 175 | 9: False # "road" 176 | 10: False # "parking" 177 | 11: False # "sidewalk" 178 | 12: False # "other-ground" 179 | 13: False # "building" 180 | 14: False # "fence" 181 | 15: False # "vegetation" 182 | 16: False # "trunk" 183 | 17: False # "terrain" 184 | 18: False # "pole" 185 | 19: False # "traffic-sign" 186 | split: # sequence numbers 187 | train: 188 | - 0 189 | - 1 190 | - 2 191 | - 3 192 | - 4 193 | - 5 194 | - 6 195 | - 7 196 | - 9 197 | - 10 198 | valid: 199 | - 8 200 | test: 201 | - 11 202 | - 12 203 | - 13 204 | - 14 205 | - 15 206 | - 16 207 | - 17 208 | - 18 209 | - 19 210 | - 20 211 | - 21 212 | -------------------------------------------------------------------------------- /ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 25 | 26 | sage_icp 27 | 1.0.0 28 | SAGE-ICP ROS Wrappers 29 | jiamingcui 30 | MIT 31 | 32 | 33 | ros_environment 34 | 35 | 36 | catkin 37 | roscpp 38 | 39 | 40 | ament_cmake 41 | rclcpp 42 | rclcpp_components 43 | ros2launch 44 | 45 | 46 | geometry_msgs 47 | nav_msgs 48 | std_msgs 49 | sensor_msgs 50 | visualization_msgs 51 | tf2 52 | tf2_ros 53 | 54 | 55 | catkin 56 | ament_cmake 57 | 58 | -------------------------------------------------------------------------------- /ros/ros2/OdometryServer.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | // SAGE-ICP 25 | #include "sage_icp/pipeline/sageICP.hpp" 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | // ROS2 33 | #include "nav_msgs/msg/odometry.hpp" 34 | #include "nav_msgs/msg/path.hpp" 35 | #include "rclcpp/rclcpp.hpp" 36 | #include "sensor_msgs/msg/point_cloud2.hpp" 37 | #include "visualization_msgs/msg/marker.hpp" 38 | #include "tf2_ros/transform_broadcaster.h" 39 | #include "example_interfaces/srv/add_two_ints.hpp" 40 | namespace sage_icp_ros { 41 | 42 | class OdometryServer : public rclcpp::Node { 43 | public: 44 | /// OdometryServer constructor 45 | OdometryServer(); 46 | 47 | private: 48 | /// Register new frame 49 | void RegisterFrame(const sensor_msgs::msg::PointCloud2::SharedPtr msg_ptr); 50 | /// pre-matching 51 | // Sophus::SE3d PreMatching(pcl::PointCloud::Ptr &building_pc); 52 | /// publish groundtruth 53 | void pub_gtpath(const geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr); 54 | 55 | void ReinitService(const std::shared_ptr request_header, 56 | const std::shared_ptr request, 57 | std::shared_ptr response); 58 | 59 | private: 60 | /// Ros node stuff 61 | size_t queue_size_{1}; 62 | 63 | /// Tools for broadcasting TFs. 64 | std::unique_ptr tf_broadcaster_; 65 | 66 | /// Data subscribers. 67 | rclcpp::Subscription::SharedPtr pointcloud_sub_; 68 | rclcpp::Subscription::SharedPtr gt_sub_; 69 | 70 | /// Data publishers. 71 | rclcpp::Publisher::SharedPtr odom_publisher_; 72 | rclcpp::Publisher::SharedPtr frame_publisher_; 73 | rclcpp::Publisher::SharedPtr map_publisher_; 74 | rclcpp::Publisher::SharedPtr key_frame_publisher_; 75 | rclcpp::Publisher::SharedPtr marker_publisher_; 76 | 77 | /// service 78 | rclcpp::Service::SharedPtr reinit_service_; 79 | /// Path publisher 80 | // nav_msgs::msg::Path path_msg_; 81 | rclcpp::Publisher::SharedPtr traj_publisher_; 82 | /// Groundtruth Path publisher 83 | rclcpp::Publisher::SharedPtr GT_publisher_; 84 | 85 | /// SAGE-ICP 86 | sage_icp::pipeline::sageICP odometry_; 87 | sage_icp::pipeline::sageConfig config_; 88 | 89 | /// Global/map coordinate frame. 90 | std::string pc_topic_{"pointcloud"}; 91 | std::string base_frame_{"base_link"}; 92 | std::string odom_frame_{"odom"}; 93 | std::string odom_topic_{"odometry"}; 94 | std::string trajectory_topic_{"trajectory"}; 95 | bool publish_frame_{false}; 96 | std::string frame_topic_{"frame"}; 97 | std::string local_map_topic_{"local_map"}; 98 | bool sub_ground_truth_{false}; 99 | std::string gt_topic_{"/ground_truth"}; 100 | std::string gt_trajectory_topic_{"gt_trajectory"}; 101 | std::map color_list_; 102 | bool publish_key_frame_{false}; 103 | std::string key_frame_topic_{"key_frame"}; 104 | std::string key_marker_topic_{"key_marker"}; 105 | double key_frame_overlap_{0.5}; 106 | std::vector> key_frame_bounds_; 107 | std::vector key_frame_occ_size_; 108 | int last_marker_id_{0}; 109 | Sophus::SE3d last_pose_; 110 | std::vector> last_key_frame_occ_; 111 | }; 112 | 113 | } // namespace sage_icp_ros 114 | -------------------------------------------------------------------------------- /ros/ros2/Utils.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2023 NeSC-IV 4 | 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include "sensor_msgs/msg/point_cloud2.hpp" 32 | #include "sensor_msgs/point_cloud2_iterator.hpp" 33 | #include "sensor_msgs/msg/point_field.hpp" 34 | #include 35 | #include "visualization_msgs/msg/marker.hpp" 36 | #include "nav_msgs/msg/odometry.hpp" 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace sage_icp_ros::utils { 46 | 47 | using PointCloud2 = sensor_msgs::msg::PointCloud2; 48 | using PointField = sensor_msgs::msg::PointField; 49 | using Header = std_msgs::msg::Header; 50 | using Marker = visualization_msgs::msg::Marker; 51 | std::string FixFrameId(const std::string &frame_id) { 52 | return std::regex_replace(frame_id, std::regex("^/"), ""); 53 | } 54 | 55 | auto GetTimestampField(const PointCloud2 &msg) { 56 | PointField timestamp_field; 57 | for (const auto &field : msg.fields) { 58 | if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { 59 | timestamp_field = field; 60 | } 61 | } 62 | if (!timestamp_field.count) { 63 | throw std::runtime_error("Field 't', 'timestamp', or 'time' does not exist"); 64 | } 65 | return timestamp_field; 66 | } 67 | 68 | // Normalize timestamps from 0.0 to 1.0 69 | auto NormalizeTimestamps(const std::vector ×tamps) { 70 | const double max_timestamp = *std::max_element(timestamps.cbegin(), timestamps.cend()); 71 | // check if already normalized 72 | if (max_timestamp < 1.0) return timestamps; 73 | std::vector timestamps_normalized(timestamps.size()); 74 | std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), 75 | [&](const auto ×tamp) { return timestamp / max_timestamp; }); 76 | return timestamps_normalized; 77 | } 78 | 79 | auto ExtractTimestampsFromMsg(const PointCloud2 &msg, const PointField &field) { 80 | // Extract timestamps from cloud_msg 81 | const size_t n_points = msg.height * msg.width; 82 | std::vector timestamps; 83 | timestamps.reserve(n_points); 84 | 85 | // Option 1: Timestamps are unsigned integers -> epoch time. 86 | if (field.name == "t" || field.name == "timestamp") { 87 | sensor_msgs::PointCloud2ConstIterator msg_t(msg, field.name); 88 | for (size_t i = 0; i < n_points; ++i, ++msg_t) { 89 | timestamps.emplace_back(static_cast(*msg_t)); 90 | } 91 | // Covert to normalized time, between 0.0 and 1.0 92 | return NormalizeTimestamps(timestamps); 93 | } 94 | 95 | // Option 2: Timestamps are floating point values between 0.0 and 1.0 96 | // field.name == "timestamp" 97 | sensor_msgs::PointCloud2ConstIterator msg_t(msg, field.name); 98 | for (size_t i = 0; i < n_points; ++i, ++msg_t) { 99 | timestamps.emplace_back(*msg_t); 100 | } 101 | return timestamps; 102 | } 103 | 104 | auto CreatePointCloud2Msg(const size_t n_points, const Header &header, bool timestamp = false) { 105 | PointCloud2 cloud_msg; 106 | sensor_msgs::PointCloud2Modifier modifier(cloud_msg); 107 | cloud_msg.header = header; 108 | cloud_msg.header.frame_id = FixFrameId(cloud_msg.header.frame_id); 109 | cloud_msg.fields.clear(); 110 | int offset = 0; 111 | offset = addPointField(cloud_msg, "x", 1, PointField::FLOAT32, offset); 112 | offset = addPointField(cloud_msg, "y", 1, PointField::FLOAT32, offset); 113 | offset = addPointField(cloud_msg, "z", 1, PointField::FLOAT32, offset); 114 | offset = addPointField(cloud_msg, "label", 1, PointField::UINT8, offset); 115 | offset = addPointField(cloud_msg, "rgb", 1, PointField::UINT32, offset); 116 | offset += sizeOfPointField(PointField::FLOAT32); 117 | if (timestamp) { 118 | // asuming timestamp on a velodyne fashion for now (between 0.0 and 1.0) 119 | offset = addPointField(cloud_msg, "time", 1, PointField::FLOAT64, offset); 120 | offset += sizeOfPointField(PointField::FLOAT64); 121 | } 122 | 123 | // Resize the point cloud accordingly 124 | cloud_msg.point_step = offset; 125 | cloud_msg.row_step = cloud_msg.width * cloud_msg.point_step; 126 | cloud_msg.data.resize(cloud_msg.height * cloud_msg.row_step); 127 | modifier.resize(n_points); 128 | return cloud_msg; 129 | } 130 | 131 | void FillPointCloud2XYZlRGB(const std::vector &points, const std::map &color_list, PointCloud2 &msg) { 132 | sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); 133 | sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); 134 | sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); 135 | sensor_msgs::PointCloud2Iterator msg_label(msg, "label"); 136 | sensor_msgs::PointCloud2Iterator msg_rgb(msg, "rgb"); 137 | for (size_t i = 0; i < points.size(); i++, ++msg_x, ++msg_y, ++msg_z, ++msg_label, ++msg_rgb) { 138 | const Eigen::Vector4d &point = points[i]; 139 | *msg_x = point.x(); 140 | *msg_y = point.y(); 141 | *msg_z = point.z(); 142 | *msg_label = static_cast(point.w()); 143 | *msg_rgb = color_list.at(static_cast(point.w())); 144 | } 145 | } 146 | 147 | void FillPointCloud2Timestamp(const std::vector ×tamps, PointCloud2 &msg) { 148 | sensor_msgs::PointCloud2Iterator msg_t(msg, "time"); 149 | for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i]; 150 | } 151 | 152 | std::vector GetTimestamps(const PointCloud2 &msg) { 153 | auto timestamp_field = GetTimestampField(msg); 154 | 155 | // Extract timestamps from cloud_msg 156 | std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field); 157 | 158 | return timestamps; 159 | } 160 | 161 | std::vector PointCloud2ToEigen(const PointCloud2 &msg) { 162 | std::vector points; 163 | points.reserve(msg.height * msg.width); 164 | sensor_msgs::PointCloud2ConstIterator msg_x(msg, "x"); 165 | sensor_msgs::PointCloud2ConstIterator msg_y(msg, "y"); 166 | sensor_msgs::PointCloud2ConstIterator msg_z(msg, "z"); 167 | if (msg.fields.size() == 5){ 168 | sensor_msgs::PointCloud2ConstIterator msg_l(msg, "label"); 169 | for (size_t i = 0; i < msg.height * msg.width; ++i, ++msg_x, ++msg_y, ++msg_z, ++msg_l) { 170 | points.emplace_back(*msg_x, *msg_y, *msg_z, *msg_l); 171 | } 172 | } 173 | else{ 174 | sensor_msgs::PointCloud2ConstIterator msg_l(msg, "label"); 175 | for (size_t i = 0; i < msg.height * msg.width; ++i, ++msg_x, ++msg_y, ++msg_z, ++msg_l) { 176 | points.emplace_back(*msg_x, *msg_y, *msg_z, *msg_l); 177 | } 178 | } 179 | return points; 180 | } 181 | 182 | PointCloud2 EigenToPointCloud2(const std::vector &points, 183 | const Header &header, 184 | const std::map &color_list) { 185 | PointCloud2 msg = CreatePointCloud2Msg(points.size(), header); 186 | FillPointCloud2XYZlRGB(points, color_list, msg); 187 | return msg; 188 | } 189 | 190 | PointCloud2 EigenToPointCloud2(const std::vector &points, 191 | const Header &header, 192 | const std::map &color_list, 193 | const std::vector ×tamps) { 194 | PointCloud2 msg = CreatePointCloud2Msg(points.size(), header, true); 195 | FillPointCloud2XYZlRGB(points, color_list, msg); 196 | FillPointCloud2Timestamp(timestamps, msg); 197 | return msg; 198 | } 199 | 200 | Marker OdomToMarker(const nav_msgs::msg::Odometry &odom_msg, 201 | const std::string &key_frame_topic, 202 | int &last_marker_id){ 203 | Marker marker; 204 | marker.header = odom_msg.header; 205 | marker.ns = key_frame_topic; 206 | marker.id = last_marker_id++; 207 | marker.type = Marker::SPHERE; 208 | marker.action = Marker::ADD; 209 | marker.pose = odom_msg.pose.pose; 210 | marker.scale.x = 0.3; 211 | marker.scale.y = 0.3; 212 | marker.scale.z = 0.3; 213 | marker.color.a = 1.0; 214 | marker.color.r = 1.0; 215 | marker.color.g = 0.0; 216 | marker.color.b = 0.0; 217 | return marker; 218 | } 219 | 220 | std::vector> EigenToGridMap(const std::vector& points, 221 | const std::vector> key_frame_bounds, 222 | const std::vector& key_frame_occ_size) { 223 | std::vector> gridMap(key_frame_occ_size[0], std::vector(key_frame_occ_size[1], 0)); 224 | const double x_resolution = (key_frame_bounds[0][1] - key_frame_bounds[0][0]) / key_frame_occ_size[1]; // Width 225 | const double y_resolution = (key_frame_bounds[1][1] - key_frame_bounds[1][0]) / key_frame_occ_size[0]; // Height 226 | for (const auto& point : points) { 227 | if (point[0] < key_frame_bounds[0][0] || point[0] > key_frame_bounds[0][1] || 228 | point[1] < key_frame_bounds[1][0] || point[1] > key_frame_bounds[1][1] || 229 | point[2] < key_frame_bounds[2][0] || point[2] > key_frame_bounds[2][1]) { 230 | continue; 231 | } 232 | // move pc to occ frame 233 | int occ_x = static_cast((point[0] + key_frame_bounds[0][1])/ x_resolution); 234 | int occ_y = static_cast((point[1] + key_frame_bounds[1][1])/ y_resolution); 235 | 236 | // check if the point is within the bounds of the occupancy grid 237 | if (occ_x >= 0 && occ_x < key_frame_occ_size[1] && occ_y >= 0 && occ_y < key_frame_occ_size[0]) { 238 | gridMap[occ_y][occ_x] = 1; // update the occupancy grid 239 | } 240 | } 241 | return gridMap; 242 | } 243 | 244 | double compute_occ_overlap(const std::vector>& occ_s, const std::vector>& occ_t) { 245 | int overlap = 0; 246 | int total = 0; 247 | for (size_t i = 0; i < occ_s.size(); i++) { 248 | for (size_t j = 0; j < occ_s[0].size(); j++) { 249 | if (occ_s[i][j] == 1 && occ_t[i][j] == 1) { 250 | overlap++; 251 | } 252 | if (occ_s[i][j] == 1) { 253 | total++; 254 | } 255 | } 256 | } 257 | return static_cast(overlap) / total; 258 | } 259 | 260 | std::vector> unpack_2d_array_int(const std::string& packed_str) { 261 | std::vector> result; 262 | std::stringstream ss(packed_str); 263 | std::string row_str; 264 | 265 | while (std::getline(ss, row_str, ';')) { 266 | std::vector row; 267 | std::stringstream row_ss(row_str); 268 | std::string value_str; 269 | 270 | while (std::getline(row_ss, value_str, ',')) { 271 | row.push_back(std::stof(value_str)); 272 | } 273 | 274 | result.push_back(row); 275 | } 276 | 277 | return result; 278 | } 279 | 280 | std::vector> unpack_2d_array_double(const std::string& packed_str) { 281 | std::vector> result; 282 | std::stringstream ss(packed_str); 283 | std::string row_str; 284 | 285 | while (std::getline(ss, row_str, ';')) { 286 | std::vector row; 287 | std::stringstream row_ss(row_str); 288 | std::string value_str; 289 | 290 | while (std::getline(row_ss, value_str, ',')) { 291 | row.push_back(std::stof(value_str)); 292 | } 293 | 294 | result.push_back(row); 295 | } 296 | 297 | return result; 298 | } 299 | 300 | std::map unpack_dict(const std::string& mapAsString) { 301 | std::map result; 302 | std::stringstream ss(mapAsString); 303 | std::string item; 304 | while (std::getline(ss, item, ';')) { 305 | size_t pos = item.find(","); 306 | if (pos != std::string::npos) { 307 | int key = std::stoi(item.substr(0, pos)); 308 | int value = std::stoi(item.substr(pos + 1)); 309 | result[key] = value; 310 | } 311 | } 312 | return result; 313 | } 314 | 315 | 316 | } // namespace sage_icp_ros::utils 317 | -------------------------------------------------------------------------------- /ros/rviz/kiss_icp_record.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 87 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /local_map1 10 | - /frame_deskew1 11 | Splitter Ratio: 0.5 12 | Tree Height: 356 13 | - Class: rviz_common/Selection 14 | Name: Selection 15 | - Class: rviz_common/Tool Properties 16 | Expanded: 17 | - /2D Goal Pose1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz_common/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz_common/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 1 35 | Autocompute Intensity Bounds: true 36 | Autocompute Value Bounds: 37 | Max Value: 7.098438262939453 38 | Min Value: -27.796110153198242 39 | Value: true 40 | Axis: Z 41 | Channel Name: intensity 42 | Class: rviz_default_plugins/PointCloud2 43 | Color: 255; 255; 255 44 | Color Transformer: RGB8 45 | Decay Time: 0 46 | Enabled: true 47 | Invert Rainbow: false 48 | Max Color: 255; 255; 255 49 | Max Intensity: 4096 50 | Min Color: 0; 0; 0 51 | Min Intensity: 0 52 | Name: local_map 53 | Position Transformer: XYZ 54 | Selectable: true 55 | Size (Pixels): 3 56 | Size (m): 0.009999999776482582 57 | Style: Points 58 | Topic: 59 | Depth: 5 60 | Durability Policy: Volatile 61 | Filter size: 10 62 | History Policy: Keep Last 63 | Reliability Policy: Reliable 64 | Value: /local_map 65 | Use Fixed Frame: true 66 | Use rainbow: true 67 | Value: true 68 | - Alpha: 1 69 | Autocompute Intensity Bounds: true 70 | Autocompute Value Bounds: 71 | Max Value: 10 72 | Min Value: -10 73 | Value: true 74 | Axis: Z 75 | Channel Name: intensity 76 | Class: rviz_default_plugins/PointCloud2 77 | Color: 255; 255; 255 78 | Color Transformer: RGB8 79 | Decay Time: 0 80 | Enabled: true 81 | Invert Rainbow: false 82 | Max Color: 255; 255; 255 83 | Max Intensity: 4096 84 | Min Color: 0; 0; 0 85 | Min Intensity: 0 86 | Name: frame_deskew 87 | Position Transformer: XYZ 88 | Selectable: true 89 | Size (Pixels): 4 90 | Size (m): 0.009999999776482582 91 | Style: Points 92 | Topic: 93 | Depth: 5 94 | Durability Policy: Volatile 95 | Filter size: 10 96 | History Policy: Keep Last 97 | Reliability Policy: Reliable 98 | Value: /frame 99 | Use Fixed Frame: true 100 | Use rainbow: true 101 | Value: true 102 | - Class: rviz_default_plugins/Axes 103 | Enabled: true 104 | Length: 1 105 | Name: odom_frame 106 | Radius: 0.10000000149011612 107 | Reference Frame: odom 108 | Value: true 109 | - Class: rviz_default_plugins/Axes 110 | Enabled: true 111 | Length: 1 112 | Name: pointcloud_frame 113 | Radius: 0.10000000149011612 114 | Reference Frame: base_link 115 | Value: true 116 | - Alpha: 1 117 | Buffer Length: 1 118 | Class: rviz_default_plugins/Path 119 | Color: 0; 255; 0 120 | Enabled: true 121 | Head Diameter: 0.30000001192092896 122 | Head Length: 0.20000000298023224 123 | Length: 0.30000001192092896 124 | Line Style: Billboards 125 | Line Width: 0.20000000298023224 126 | Name: trajectory 127 | Offset: 128 | X: 0 129 | Y: 0 130 | Z: 0 131 | Pose Color: 255; 85; 255 132 | Pose Style: None 133 | Radius: 0.029999999329447746 134 | Shaft Diameter: 0.10000000149011612 135 | Shaft Length: 0.10000000149011612 136 | Topic: 137 | Depth: 5 138 | Durability Policy: Volatile 139 | Filter size: 10 140 | History Policy: Keep Last 141 | Reliability Policy: Reliable 142 | Value: /trajectory 143 | Value: true 144 | - Alpha: 1 145 | Buffer Length: 1 146 | Class: rviz_default_plugins/Path 147 | Color: 255; 0; 0 148 | Enabled: true 149 | Head Diameter: 0.30000001192092896 150 | Head Length: 0.20000000298023224 151 | Length: 0.30000001192092896 152 | Line Style: Lines 153 | Line Width: 0.029999999329447746 154 | Name: Groundtruth 155 | Offset: 156 | X: 0 157 | Y: 0 158 | Z: 0 159 | Pose Color: 255; 85; 255 160 | Pose Style: None 161 | Radius: 0.029999999329447746 162 | Shaft Diameter: 0.10000000149011612 163 | Shaft Length: 0.10000000149011612 164 | Topic: 165 | Depth: 5 166 | Durability Policy: Volatile 167 | Filter size: 10 168 | History Policy: Keep Last 169 | Reliability Policy: Reliable 170 | Value: /GT_trajectory 171 | Value: true 172 | Enabled: true 173 | Global Options: 174 | Background Color: 255; 255; 255 175 | Fixed Frame: base_link 176 | Frame Rate: 30 177 | Name: root 178 | Tools: 179 | - Class: rviz_default_plugins/Interact 180 | Hide Inactive Objects: true 181 | - Class: rviz_default_plugins/MoveCamera 182 | - Class: rviz_default_plugins/Select 183 | - Class: rviz_default_plugins/FocusCamera 184 | - Class: rviz_default_plugins/Measure 185 | Line color: 128; 128; 0 186 | - Class: rviz_default_plugins/SetInitialPose 187 | Covariance x: 0.25 188 | Covariance y: 0.25 189 | Covariance yaw: 0.06853891909122467 190 | Topic: 191 | Depth: 5 192 | Durability Policy: Volatile 193 | History Policy: Keep Last 194 | Reliability Policy: Reliable 195 | Value: /initialpose 196 | - Class: rviz_default_plugins/SetGoal 197 | Topic: 198 | Depth: 5 199 | Durability Policy: Volatile 200 | History Policy: Keep Last 201 | Reliability Policy: Reliable 202 | Value: /goal_pose 203 | - Class: rviz_default_plugins/PublishPoint 204 | Single click: true 205 | Topic: 206 | Depth: 5 207 | Durability Policy: Volatile 208 | History Policy: Keep Last 209 | Reliability Policy: Reliable 210 | Value: /clicked_point 211 | Transformation: 212 | Current: 213 | Class: rviz_default_plugins/TF 214 | Value: true 215 | Views: 216 | Current: 217 | Class: rviz_default_plugins/Orbit 218 | Distance: 67.09307861328125 219 | Enable Stereo Rendering: 220 | Stereo Eye Separation: 0.05999999865889549 221 | Stereo Focal Distance: 1 222 | Swap Stereo Eyes: false 223 | Value: false 224 | Focal Point: 225 | X: 0 226 | Y: 0 227 | Z: 0 228 | Focal Shape Fixed Size: false 229 | Focal Shape Size: 0.05000000074505806 230 | Invert Z Axis: false 231 | Name: Current View 232 | Near Clip Distance: 0.009999999776482582 233 | Pitch: 1.5697963237762451 234 | Target Frame: 235 | Value: Orbit (rviz_default_plugins) 236 | Yaw: 3.1404030323028564 237 | Saved: ~ 238 | Window Geometry: 239 | Displays: 240 | collapsed: false 241 | Height: 1417 242 | Hide Left Dock: false 243 | Hide Right Dock: false 244 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000047dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000047d000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000047dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000047d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000004cfc0100000002fb0000000800540069006d0065010000000000000a000000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 245 | Selection: 246 | collapsed: false 247 | Time: 248 | collapsed: false 249 | Tool Properties: 250 | collapsed: false 251 | Views: 252 | collapsed: false 253 | Width: 2560 254 | X: 2560 255 | Y: 0 256 | -------------------------------------------------------------------------------- /ros/rviz/sage_icp_ros2.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 131 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 1000 10 | - Class: rviz_common/Selection 11 | Name: Selection 12 | - Class: rviz_common/Tool Properties 13 | Expanded: 14 | - /2D Goal Pose1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.5886790156364441 18 | - Class: rviz_common/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz_common/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: current_frame 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Class: rviz_default_plugins/Axes 32 | Enabled: true 33 | Length: 1 34 | Name: odom_frame 35 | Radius: 0.10000000149011612 36 | Reference Frame: odom 37 | Value: true 38 | - Class: rviz_default_plugins/Axes 39 | Enabled: true 40 | Length: 1 41 | Name: pointcloud_frame 42 | Radius: 0.10000000149011612 43 | Reference Frame: base_link 44 | Value: true 45 | - Class: rviz_common/Group 46 | Displays: 47 | - Alpha: 1 48 | Autocompute Intensity Bounds: true 49 | Autocompute Value Bounds: 50 | Max Value: 10 51 | Min Value: -10 52 | Value: true 53 | Axis: Z 54 | Channel Name: intensity 55 | Class: rviz_default_plugins/PointCloud2 56 | Color: 87; 227; 137 57 | Color Transformer: FlatColor 58 | Decay Time: 0 59 | Enabled: true 60 | Invert Rainbow: false 61 | Max Color: 255; 255; 255 62 | Max Intensity: 4096 63 | Min Color: 0; 0; 0 64 | Min Intensity: 0 65 | Name: current_frame 66 | Position Transformer: XYZ 67 | Selectable: true 68 | Size (Pixels): 4 69 | Size (m): 0.009999999776482582 70 | Style: Points 71 | Topic: 72 | Depth: 5 73 | Durability Policy: Volatile 74 | History Policy: Keep Last 75 | Reliability Policy: Reliable 76 | Value: /sage_icp/frame 77 | Use Fixed Frame: true 78 | Use rainbow: true 79 | Value: true 80 | - Alpha: 1 81 | Autocompute Intensity Bounds: true 82 | Autocompute Value Bounds: 83 | Max Value: 10 84 | Min Value: -10 85 | Value: true 86 | Axis: Z 87 | Channel Name: intensity 88 | Class: rviz_default_plugins/PointCloud2 89 | Color: 255; 255; 255 90 | Color Transformer: RGB8 91 | Decay Time: 0 92 | Enabled: true 93 | Invert Rainbow: false 94 | Max Color: 255; 255; 255 95 | Max Intensity: 4096 96 | Min Color: 0; 0; 0 97 | Min Intensity: 0 98 | Name: local_map 99 | Position Transformer: XYZ 100 | Selectable: true 101 | Size (Pixels): 1 102 | Size (m): 0.009999999776482582 103 | Style: Points 104 | Topic: 105 | Depth: 5 106 | Durability Policy: Volatile 107 | History Policy: Keep Last 108 | Reliability Policy: Reliable 109 | Value: /sage_icp/local_map 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: true 113 | - Class: rviz_default_plugins/Marker 114 | Enabled: true 115 | Name: keyframe_Marker 116 | Namespaces: 117 | /sage_icp/key_marker: true 118 | Topic: 119 | Depth: 5 120 | Durability Policy: Volatile 121 | Filter size: 10 122 | History Policy: Keep Last 123 | Reliability Policy: Reliable 124 | Value: /sage_icp/key_marker 125 | Value: true 126 | - Alpha: 1 127 | Autocompute Intensity Bounds: true 128 | Autocompute Value Bounds: 129 | Max Value: 10 130 | Min Value: -10 131 | Value: true 132 | Axis: Z 133 | Channel Name: intensity 134 | Class: rviz_default_plugins/PointCloud2 135 | Color: 237; 51; 59 136 | Color Transformer: FlatColor 137 | Decay Time: 0 138 | Enabled: false 139 | Invert Rainbow: false 140 | Max Color: 255; 255; 255 141 | Max Intensity: 4096 142 | Min Color: 0; 0; 0 143 | Min Intensity: 0 144 | Name: key_frame 145 | Position Transformer: XYZ 146 | Selectable: true 147 | Size (Pixels): 2 148 | Size (m): 0.10000000149011612 149 | Style: Points 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /sage_icp/key_frame 156 | Use Fixed Frame: true 157 | Use rainbow: true 158 | Value: false 159 | - Angle Tolerance: 0.10000000149011612 160 | Class: rviz_default_plugins/Odometry 161 | Covariance: 162 | Orientation: 163 | Alpha: 0.5 164 | Color: 255; 255; 127 165 | Color Style: Unique 166 | Frame: Local 167 | Offset: 1 168 | Scale: 1 169 | Value: true 170 | Position: 171 | Alpha: 0.30000001192092896 172 | Color: 204; 51; 204 173 | Scale: 1 174 | Value: true 175 | Value: true 176 | Enabled: true 177 | Keep: 20 178 | Name: Odometry 179 | Position Tolerance: 0.10000000149011612 180 | Shape: 181 | Alpha: 1 182 | Axes Length: 1 183 | Axes Radius: 0.10000000149011612 184 | Color: 255; 25; 0 185 | Head Length: 0.30000001192092896 186 | Head Radius: 0.10000000149011612 187 | Shaft Length: 1 188 | Shaft Radius: 0.05000000074505806 189 | Value: Arrow 190 | Topic: 191 | Depth: 5 192 | Durability Policy: Volatile 193 | Filter size: 10 194 | History Policy: Keep Last 195 | Reliability Policy: Reliable 196 | Value: /sage_icp/odometry 197 | Value: true 198 | - Alpha: 1 199 | Buffer Length: 1 200 | Class: rviz_default_plugins/Path 201 | Color: 25; 255; 0 202 | Enabled: true 203 | Head Diameter: 0.30000001192092896 204 | Head Length: 0.20000000298023224 205 | Length: 0.30000001192092896 206 | Line Style: Lines 207 | Line Width: 0.029999999329447746 208 | Name: Path 209 | Offset: 210 | X: 0 211 | Y: 0 212 | Z: 0 213 | Pose Color: 255; 85; 255 214 | Pose Style: None 215 | Radius: 0.029999999329447746 216 | Shaft Diameter: 0.10000000149011612 217 | Shaft Length: 0.10000000149011612 218 | Topic: 219 | Depth: 5 220 | Durability Policy: Volatile 221 | Filter size: 10 222 | History Policy: Keep Last 223 | Reliability Policy: Reliable 224 | Value: /sage_icp/trajectory 225 | Value: true 226 | Enabled: true 227 | Name: sage-icp 228 | - Alpha: 1 229 | Buffer Length: 1 230 | Class: rviz_default_plugins/Path 231 | Color: 255; 0; 0 232 | Enabled: true 233 | Head Diameter: 0.30000001192092896 234 | Head Length: 0.20000000298023224 235 | Length: 0.30000001192092896 236 | Line Style: Lines 237 | Line Width: 0.029999999329447746 238 | Name: GT-Path 239 | Offset: 240 | X: 0 241 | Y: 0 242 | Z: 0 243 | Pose Color: 255; 85; 255 244 | Pose Style: None 245 | Radius: 0.029999999329447746 246 | Shaft Diameter: 0.10000000149011612 247 | Shaft Length: 0.10000000149011612 248 | Topic: 249 | Depth: 5 250 | Durability Policy: Volatile 251 | Filter size: 10 252 | History Policy: Keep Last 253 | Reliability Policy: Reliable 254 | Value: /gt_trajectory 255 | Value: true 256 | Enabled: true 257 | Global Options: 258 | Background Color: 48; 48; 48 259 | Fixed Frame: base_link 260 | Frame Rate: 30 261 | Name: root 262 | Tools: 263 | - Class: rviz_default_plugins/Interact 264 | Hide Inactive Objects: true 265 | - Class: rviz_default_plugins/MoveCamera 266 | - Class: rviz_default_plugins/Select 267 | - Class: rviz_default_plugins/FocusCamera 268 | - Class: rviz_default_plugins/Measure 269 | Line color: 128; 128; 0 270 | - Class: rviz_default_plugins/SetInitialPose 271 | Covariance x: 0.25 272 | Covariance y: 0.25 273 | Covariance yaw: 0.06853891909122467 274 | Topic: 275 | Depth: 5 276 | Durability Policy: Volatile 277 | History Policy: Keep Last 278 | Reliability Policy: Reliable 279 | Value: /initialpose 280 | - Class: rviz_default_plugins/SetGoal 281 | Topic: 282 | Depth: 5 283 | Durability Policy: Volatile 284 | History Policy: Keep Last 285 | Reliability Policy: Reliable 286 | Value: /goal_pose 287 | - Class: rviz_default_plugins/PublishPoint 288 | Single click: true 289 | Topic: 290 | Depth: 5 291 | Durability Policy: Volatile 292 | History Policy: Keep Last 293 | Reliability Policy: Reliable 294 | Value: /clicked_point 295 | Transformation: 296 | Current: 297 | Class: rviz_default_plugins/TF 298 | Value: true 299 | Views: 300 | Current: 301 | Class: rviz_default_plugins/Orbit 302 | Distance: 39.488563537597656 303 | Enable Stereo Rendering: 304 | Stereo Eye Separation: 0.05999999865889549 305 | Stereo Focal Distance: 1 306 | Swap Stereo Eyes: false 307 | Value: false 308 | Focal Point: 309 | X: -0.5177667140960693 310 | Y: 2.3285651206970215 311 | Z: 9.773652076721191 312 | Focal Shape Fixed Size: false 313 | Focal Shape Size: 0.05000000074505806 314 | Invert Z Axis: false 315 | Name: Current View 316 | Near Clip Distance: 0.009999999776482582 317 | Pitch: 1.4552011489868164 318 | Target Frame: 319 | Value: Orbit (rviz_default_plugins) 320 | Yaw: 3.160391092300415 321 | Saved: ~ 322 | Window Geometry: 323 | Displays: 324 | collapsed: false 325 | Height: 1371 326 | Hide Left Dock: false 327 | Hide Right Dock: false 328 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000004abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003f000004ab000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000379000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000004cfc0100000002fb0000000800540069006d0065010000000000000a000000026f00fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 329 | Selection: 330 | collapsed: false 331 | Time: 332 | collapsed: false 333 | Tool Properties: 334 | collapsed: false 335 | Views: 336 | collapsed: false 337 | Width: 2560 338 | X: 880 339 | Y: 32 340 | --------------------------------------------------------------------------------