├── .clang-format ├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── config.yml │ └── questions.md └── workflows │ └── issue_comment.yml ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── Makefile ├── README.md ├── cpp ├── examples │ ├── 3rdparty │ │ ├── download_datasets.cmake │ │ ├── find_dependencies.cmake │ │ └── tbb │ │ │ ├── LICENSE │ │ │ └── tbb.cmake │ ├── CMakeLists.txt │ ├── COLCON_IGNORE │ ├── README.md │ ├── include │ │ └── quatro │ │ │ ├── fpfh.h │ │ │ ├── matcher.h │ │ │ └── quatro_utils.h │ └── src │ │ ├── conversion_speed_comparison.cc │ │ ├── downsampling_speed_comparison.cc │ │ ├── fpfh.cc │ │ ├── matcher.cc │ │ ├── run_kiss_matcher.cc │ │ └── run_matching_using_bunny.cc └── kiss_matcher │ ├── 3rdparty │ ├── eigen │ │ ├── LICENSE │ │ ├── eigen.cmake │ │ └── eigen.patch │ ├── find_dependencies.cmake │ ├── robin │ │ └── robin.cmake │ ├── tbb │ │ ├── LICENSE │ │ └── tbb.cmake │ ├── teaserpp │ │ └── teaserpp.cmake │ └── tsl_robin │ │ ├── LICENSE │ │ └── tsl_robin.cmake │ ├── CMakeLists.txt │ ├── COLCON_IGNORE │ ├── README.md │ ├── cmake │ ├── CompilerOptions.cmake │ └── kiss_matcherConfig.cmake │ └── core │ └── kiss_matcher │ ├── FasterPFH.cpp │ ├── FasterPFH.hpp │ ├── GncSolver.cpp │ ├── GncSolver.hpp │ ├── KISSMatcher.cpp │ ├── KISSMatcher.hpp │ ├── ROBINMatching.cpp │ ├── ROBINMatching.hpp │ ├── kdtree │ ├── kdtree.hpp │ ├── kdtree_tbb.hpp │ ├── nanoflann.hpp │ ├── nanoflann_tbb.hpp │ └── traits.hpp │ ├── points │ ├── downsampling.hpp │ ├── eigen.hpp │ ├── fast_floor.hpp │ ├── point_cloud.hpp │ ├── traits.hpp │ └── vector3i_hash.hpp │ └── tsl │ ├── robin_growth_policy.h │ ├── robin_hash.h │ ├── robin_map.h │ └── robin_set.h ├── python ├── CMakeLists.txt ├── COLCON_IGNORE ├── LICENSE ├── README.md ├── examples │ └── run_kiss_matcher.py ├── kiss_matcher │ ├── __init__.py │ └── pybind │ │ ├── .gitignore │ │ ├── CMakeLists.txt │ │ ├── kiss_matcher_pybind.cpp │ │ └── stl_vector_eigen.h ├── pyproject.toml └── utils │ ├── bin2pcd.py │ └── download_datasets.py ├── ros ├── CMakeLists.txt ├── LICENSE ├── README.md ├── README_ROS2_REGISTRATION.md ├── config │ ├── alignment_config.yaml │ ├── kimera_multi │ │ ├── ouster64.yaml │ │ └── velodyne16.yaml │ ├── params.yaml │ └── slam_config.yaml ├── include │ ├── slam │ │ ├── loop_closure.h │ │ ├── loop_detector.h │ │ ├── loop_types.hpp │ │ ├── pose_graph_manager.h │ │ ├── pose_graph_node.hpp │ │ └── utils.hpp │ └── tictoc.hpp ├── launch │ ├── inter_frame_alignment.launch.yaml │ ├── run_kiss_matcher_sam.launch.yaml │ ├── slam_in_kimera_multi.launch.yaml │ └── visualizer_launch.py ├── package.xml ├── rviz │ ├── kimera_multi_initial_alignment.rviz │ ├── kiss_matcher_reg.rviz │ ├── kiss_matcher_sam.rviz │ └── slam_in_kimera_multi.rviz └── src │ ├── inter_frame_alignment.cpp │ ├── registration_visualizer.cpp │ ├── run_kiss_matcher.cpp │ └── slam │ ├── loop_closure.cpp │ ├── loop_detector.cpp │ └── pose_graph_manager.cpp └── shellscripts ├── install_robin.sh └── install_teaserpp.sh /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: true 7 | AlignConsecutiveDeclarations: false 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: All 14 | AllowShortIfStatementsOnASingleLine: true 15 | AllowShortLoopsOnASingleLine: true 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: true 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: false 21 | BinPackParameters: false 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | CommentPragmas: "^ IWYU pragma:" 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: true 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | # Spacers 52 | - Regex: "^$" 53 | Priority: 15 54 | - Regex: "^$" 55 | Priority: 25 56 | - Regex: "^$" 57 | Priority: 35 58 | - Regex: "^$" 59 | Priority: 45 60 | # C system headers 61 | - Regex: '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$' 62 | Priority: 10 63 | # C++ system headers 64 | - Regex: '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$' 65 | Priority: 20 66 | # Other library h files (with angles) 67 | - Regex: "^<" 68 | Priority: 30 69 | # Your project's h files (with angles) 70 | - Regex: "^> $GITHUB_ENV 18 | else 19 | echo "short=false" >> $GITHUB_ENV 20 | fi 21 | 22 | - name: Comment on short issue 23 | if: env.short == 'true' 24 | uses: actions/github-script@v6 25 | with: 26 | github-token: ${{ secrets.GITHUB_TOKEN }} 27 | script: | 28 | github.rest.issues.createComment({ 29 | owner: context.repo.owner, 30 | repo: context.repo.repo, 31 | issue_number: context.issue.number, 32 | body: "👋 Thanks for your issue! However, it looks too short to help you in details. Could you provide more details such as:\n\n- A clear description of the problem\n- Steps to reproduce\n- Expected vs. actual behavior\n- **Important** The result of visualizing the current situation/problems \n- Your environment (OS, software version, etc.)\n\nThis will help us investigate the issue effectively. Thanks! 🚀" 33 | }) 34 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *cmake-build-debug*/ 2 | *cmake-build-release*/ 3 | *build*/ 4 | */build/ 5 | */CMakeFiles/ 6 | 7 | .idea/ 8 | .vscode/ 9 | 10 | */install/ 11 | install/* 12 | log/ 13 | results/ 14 | wheelhouse/ 15 | _skbuild/ 16 | .gitlab-ci-local/ 17 | .polyscope.ini 18 | imgui.ini 19 | 20 | cpp/examples/cmake-build-debug*/ 21 | cpp/examples/cmake-build-release*/ 22 | cpp/examples/build*/ 23 | cpp/examples/CMakeFiles/ 24 | cpp/examples/.idea/ 25 | cpp/examples/.vscode/ 26 | 27 | cpp/kiss_matcher/cmake-build-debug*/ 28 | cpp/kiss_matcher/cmake-build-release*/ 29 | cpp/kiss_matcher/build*/ 30 | cpp/kiss_matcher/CMakeFiles/ 31 | cpp/kiss_matcher/.idea/ 32 | cpp/kiss_matcher/.vscode/ 33 | 34 | */data/ 35 | */zip_files/ 36 | 37 | compile_commands.json 38 | */compile_commands.json 39 | 40 | # Created by https://www.toptal.com/developers/gitignore/api/python,c++ 41 | # Edit at https://www.toptal.com/developers/gitignore?templates=python,c++ 42 | ### C++ ### 43 | # Prerequisites 44 | *.d 45 | 46 | # Compiled Object files 47 | *.slo 48 | *.lo 49 | *.o 50 | *.obj 51 | 52 | # Precompiled Headers 53 | *.gch 54 | *.pch 55 | 56 | # Compiled Dynamic libraries 57 | *.so 58 | *.dylib 59 | *.dll 60 | 61 | # Fortran module files 62 | *.mod 63 | *.smod 64 | 65 | # Compiled Static libraries 66 | *.lai 67 | *.la 68 | *.a 69 | *.lib 70 | 71 | # Executables 72 | *.exe 73 | *.out 74 | *.app 75 | 76 | ### Python ### 77 | # Byte-compiled / optimized / DLL files 78 | __pycache__/ 79 | *.py[cod] 80 | *$py.class 81 | 82 | # C extensions 83 | 84 | # Distribution / packaging 85 | .Python 86 | build/ 87 | develop-eggs/ 88 | dist/ 89 | downloads/ 90 | eggs/ 91 | .eggs/ 92 | lib/ 93 | lib64/ 94 | parts/ 95 | sdist/ 96 | var/ 97 | wheels/ 98 | share/python-wheels/ 99 | *.egg-info/ 100 | .installed.cfg 101 | *.egg 102 | MANIFEST 103 | 104 | # PyInstaller 105 | # Usually these files are written by a python script from a template 106 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 107 | *.manifest 108 | *.spec 109 | 110 | # Installer logs 111 | pip-log.txt 112 | pip-delete-this-directory.txt 113 | 114 | # Unit test / coverage reports 115 | htmlcov/ 116 | .tox/ 117 | .nox/ 118 | .coverage 119 | .coverage.* 120 | .cache 121 | nosetests.xml 122 | coverage.xml 123 | *.cover 124 | *.py,cover 125 | .hypothesis/ 126 | .pytest_cache/ 127 | cover/ 128 | 129 | # Translations 130 | *.mo 131 | *.pot 132 | 133 | # Django stuff: 134 | *.log 135 | local_settings.py 136 | db.sqlite3 137 | db.sqlite3-journal 138 | 139 | # Flask stuff: 140 | instance/ 141 | .webassets-cache 142 | 143 | # Scrapy stuff: 144 | .scrapy 145 | 146 | # Sphinx documentation 147 | docs/_build/ 148 | 149 | # PyBuilder 150 | .pybuilder/ 151 | target/ 152 | 153 | # Jupyter Notebook 154 | .ipynb_checkpoints 155 | 156 | # IPython 157 | profile_default/ 158 | ipython_config.py 159 | 160 | # pyenv 161 | # For a library or package, you might want to ignore these files since the code is 162 | # intended to run in multiple environments; otherwise, check them in: 163 | # .python-version 164 | 165 | # pipenv 166 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 167 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 168 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 169 | # install all needed dependencies. 170 | #Pipfile.lock 171 | 172 | # poetry 173 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 174 | # This is especially recommended for binary packages to ensure reproducibility, and is more 175 | # commonly ignored for libraries. 176 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 177 | #poetry.lock 178 | 179 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 180 | __pypackages__/ 181 | 182 | # Celery stuff 183 | celerybeat-schedule 184 | celerybeat.pid 185 | 186 | # SageMath parsed files 187 | *.sage.py 188 | 189 | # Environments 190 | .env 191 | .venv 192 | env/ 193 | venv/ 194 | ENV/ 195 | env.bak/ 196 | venv.bak/ 197 | 198 | # Spyder project settings 199 | .spyderproject 200 | .spyproject 201 | 202 | # Rope project settings 203 | .ropeproject 204 | 205 | # mkdocs documentation 206 | /site 207 | 208 | # mypy 209 | .mypy_cache/ 210 | .dmypy.json 211 | dmypy.json 212 | 213 | # Pyre type checker 214 | .pyre/ 215 | 216 | # pytype static type analyzer 217 | .pytype/ 218 | 219 | # Cython debug symbols 220 | cython_debug/ 221 | 222 | # PyCharm 223 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 224 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 225 | # and can be added to the global gitignore or merged into this file. For a more nuclear 226 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 227 | .idea/ 228 | 229 | # End of https://www.toptal.com/developers/gitignore/api/python 230 | nn 231 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | ci: 2 | autofix: true 3 | 4 | repos: 5 | - repo: https://github.com/pre-commit/pre-commit-hooks 6 | rev: v4.4.0 7 | hooks: 8 | - id: check-yaml 9 | exclude: \.clang-format$ 10 | - id: check-json 11 | - id: check-xml 12 | - id: end-of-file-fixer 13 | - id: trailing-whitespace 14 | - id: check-added-large-files 15 | - id: check-merge-conflict 16 | 17 | ###################################################################### 18 | # Python Formatting 19 | ###################################################################### 20 | # For Python import sorting 21 | - repo: https://github.com/pycqa/isort 22 | rev: "5.12.0" 23 | hooks: 24 | - id: isort 25 | 26 | # Yet Another Python Formatter 27 | # NOTE(hlim): Python 3.12 (Ubuntu 24.04) is now not supported by yapf 28 | # - repo: https://github.com/pre-commit/mirrors-yapf 29 | # rev: v0.32.0 30 | # hooks: 31 | # - id: yapf 32 | # additional_dependencies: [toml] 33 | 34 | - repo: https://github.com/psf/black 35 | rev: 23.12.1 36 | hooks: 37 | - id: black 38 | 39 | - repo: https://github.com/PyCQA/flake8 40 | rev: 5.0.4 41 | hooks: 42 | - id: flake8 43 | exclude: 'python/utils/download_datasets' 44 | # To avoid conflict between flake and black 45 | args: ["--ignore=E501"] 46 | 47 | ###################################################################### 48 | # Markdown Formatting 49 | ###################################################################### 50 | - repo: https://github.com/executablebooks/mdformat 51 | rev: 0.7.9 52 | hooks: 53 | - id: mdformat 54 | exclude: 'github/' 55 | 56 | ###################################################################### 57 | # C++ Formatting 58 | ###################################################################### 59 | - repo: https://github.com/pre-commit/mirrors-clang-format 60 | rev: "v16.0.6" 61 | hooks: 62 | - id: clang-format 63 | 64 | - repo: https://github.com/cpplint/cpplint 65 | rev: "1.6.1" 66 | hooks: 67 | - id: cpplint 68 | args: 69 | [ 70 | "--filter=-whitespace/line_length,-legal/copyright,-build/include_order,-runtime/references,-build/c++11,-build/namespaces", 71 | ] 72 | exclude: 'cpp/kiss_matcher/3rdparty|cpp/kiss_matcher/core/kiss_matcher/kdtree|cpp/kiss_matcher/core/kiss_matcher/tsl|cpp/kiss_matcher/core/kiss_matcher/points' 73 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 MIT-SPARK 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # To account for docker env 2 | # SUDO := $(shell if command -v sudo >/dev/null 2>&1 && sudo -n true 2>/dev/null; then echo "sudo"; else echo ""; fi) 3 | ifeq ($(shell test -f /.dockerenv && echo -n yes),yes) 4 | SUDO := 5 | else 6 | SUDO := "sudo" 7 | endif 8 | 9 | deps: 10 | @echo "[KISS-Matcher] SUDO is: $(SUDO)" 11 | @echo "[KISS-Matcher] Installing dependencies..." 12 | @$(SUDO) apt-get update -y 13 | @$(SUDO) apt-get install -y gcc g++ build-essential libeigen3-dev python3-pip python3-dev cmake git ninja-build libflann-dev 14 | 15 | # I used this one: 16 | # https://patorjk.com/software/taag/#p=display&f=ANSI%20Shadow 17 | ascii_art: 18 | @echo " " 19 | @echo "██╗ ██╗██╗███████╗███████╗ " 20 | @echo "██║ ██╔╝██║██╔════╝██╔════╝ " 21 | @echo "█████╔╝ ██║███████╗███████╗█████╗" 22 | @echo "██╔═██╗ ██║╚════██║╚════██║╚════╝" 23 | @echo "██║ ██╗██║███████║███████║ " 24 | @echo "╚═╝ ╚═╝╚═╝╚══════╝╚══════╝ " 25 | @echo " " 26 | @echo "███╗ ███╗ █████╗ ████████╗ ██████╗██╗ ██╗███████╗██████╗ " 27 | @echo "████╗ ████║██╔══██╗╚══██╔══╝██╔════╝██║ ██║██╔════╝██╔══██╗" 28 | @echo "██╔████╔██║███████║ ██║ ██║ ███████║█████╗ ██████╔╝" 29 | @echo "██║╚██╔╝██║██╔══██║ ██║ ██║ ██╔══██║██╔══╝ ██╔══██╗" 30 | @echo "██║ ╚═╝ ██║██║ ██║ ██║ ╚██████╗██║ ██║███████╗██║ ██║" 31 | @echo "╚═╝ ╚═╝╚═╝ ╚═╝ ╚═╝ ╚═════╝╚═╝ ╚═╝╚══════╝╚═╝ ╚═╝" 32 | @echo " " 33 | 34 | # Also install MIT-SPARK ROBIN 35 | # See https://github.com/MIT-SPARK/ROBIN 36 | # ToDo(hlim): It's not elegant, but at least it works 37 | cppinstall: deps 38 | @mkdir -p cpp/kiss_matcher/build 39 | @cmake -Bcpp/kiss_matcher/build cpp/kiss_matcher -DCMAKE_BUILD_TYPE=Release 40 | @cmake --build cpp/kiss_matcher/build -j$(nproc --all) 41 | @$(SUDO) cmake --install cpp/kiss_matcher/build 42 | @$(SUDO) cmake --install cpp/kiss_matcher/build/_deps/robin-build 43 | 44 | cppinstall_matcher_only: 45 | @mkdir -p cpp/kiss_matcher/build 46 | @cmake -Bcpp/kiss_matcher/build cpp/kiss_matcher -DCMAKE_BUILD_TYPE=Release 47 | @cmake --build cpp/kiss_matcher/build -j$(nproc --all) 48 | @$(SUDO) cmake --install cpp/kiss_matcher/build 49 | -------------------------------------------------------------------------------- /cpp/examples/3rdparty/download_datasets.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | set(DATA_DIR "${CMAKE_BINARY_DIR}/data") 4 | set(ZIP_DIR "${CMAKE_BINARY_DIR}/zip_files") 5 | 6 | set(DROPBOX_FILES 7 | "https://www.dropbox.com/scl/fi/1lsz6bxwan0sytj87ea9h/Vel16.zip?rlkey=g4jpze2j6iu6hk9ahq0m6t7o3&st=vrfpk4nv&dl=1" 8 | "https://www.dropbox.com/scl/fi/weqfi572gbi5z654d56ai/Vel64.zip?rlkey=l9upmgfjx7nhkbgl9du7igrfu&st=4q9gyous&dl=1" 9 | "https://www.dropbox.com/scl/fi/lnsbaqmbgz0qi1r8ocesd/HeLiPR-KAIST05.zip?rlkey=50jyhl180qpmf1j5jn9ru37ru&st=q0kay7o3&dl=1" 10 | "https://www.dropbox.com/scl/fi/c6c1jxrld17ywj7x2ok1q/VBR-Collosseo.zip?rlkey=b1sk0xvnntqy8kyw1apob37ob&st=5imrvvwo&dl=1" 11 | "https://www.dropbox.com/scl/fi/73l59cj5ypfvjrkrc23qx/KITTI00-to-KITTI360.zip?rlkey=pqukxxgpxaq1pugo6dhzq8xa4&st=yns7uolj&dl=1" 12 | "https://www.dropbox.com/scl/fi/lb49afp7x5ja3bfptbo68/KITTI00-to-07.zip?rlkey=qkwq99vwwlnxnxj3nhdptrusr&st=nzx8ts9j&dl=1" 13 | "https://www.dropbox.com/scl/fi/n6sypvt4rdssn172mn2jv/bun_zipper.ply?rlkey=hk2danjxt29a7ahq374s8m7ak&st=o5udnqjv&dl=1" 14 | ) 15 | 16 | set(DOWNLOADED_FILES 17 | "${ZIP_DIR}/Vel16.zip" 18 | "${ZIP_DIR}/Vel64.zip" 19 | "${ZIP_DIR}/HeLiPR-KAIST05.zip" 20 | "${ZIP_DIR}/VBR-Collosseo.zip" 21 | "${ZIP_DIR}/KITTI00-to-KITTI360.zip" 22 | "${ZIP_DIR}/KITTI00-to-07.zip" 23 | "${ZIP_DIR}/bun_zipper.ply" 24 | ) 25 | 26 | file(MAKE_DIRECTORY ${DATA_DIR}) 27 | file(MAKE_DIRECTORY ${ZIP_DIR}) 28 | 29 | list(LENGTH DROPBOX_FILES NUM_FILES) 30 | math(EXPR LAST_INDEX "${NUM_FILES} - 1") 31 | 32 | foreach(INDEX RANGE ${LAST_INDEX}) 33 | list(GET DROPBOX_FILES ${INDEX} URL) 34 | list(GET DOWNLOADED_FILES ${INDEX} DEST_FILE) 35 | 36 | message(STATUS "Downloading ${URL} -> ${DEST_FILE}") 37 | file(DOWNLOAD ${URL} ${DEST_FILE} SHOW_PROGRESS) 38 | 39 | if (${DEST_FILE} MATCHES "\\.zip$") 40 | message(STATUS "Extracting ${DEST_FILE} to ${DATA_DIR}") 41 | execute_process( 42 | COMMAND ${CMAKE_COMMAND} -E tar xzf ${DEST_FILE} 43 | WORKING_DIRECTORY ${DATA_DIR} 44 | ) 45 | else() 46 | file(COPY ${DEST_FILE} DESTINATION ${DATA_DIR}) 47 | endif() 48 | endforeach() 49 | 50 | message(STATUS "All datasets downloaded and extracted to ${DATA_DIR}") 51 | -------------------------------------------------------------------------------- /cpp/examples/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 | # CMake arguments for configuring ExternalProjects. 30 | set(ExternalProject_CMAKE_ARGS 31 | -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DCMAKE_CXX_COMPILER_LAUNCHER=${CMAKE_CXX_COMPILER_LAUNCHER} 32 | -DCMAKE_BUILD_TYPE=Release -DCMAKE_POSITION_INDEPENDENT_CODE=ON) 33 | 34 | # tbb needs to be statically linked, so, also do it always :) 35 | if(USE_SYSTEM_TBB) 36 | find_package(TBB QUIET NO_MODULE) 37 | endif() 38 | if(NOT USE_SYSTEM_TBB OR NOT TARGET TBB::tbb) 39 | set(USE_SYSTEM_TBB OFF) 40 | include(${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake) 41 | endif() 42 | -------------------------------------------------------------------------------- /cpp/examples/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 | include(ExternalProject) 24 | find_package(Threads) 25 | ExternalProject_Add( 26 | external_tbb 27 | PREFIX tbb 28 | URL https://github.com/nachovizzo/tbb/archive/refs/tags/tbbstatic.tar.gz 29 | URL_HASH SHA256=db5ede77c4bd10ad12fab11ed38b7e8cf80aba85db16a57514073c383e6c8630 30 | UPDATE_COMMAND "" 31 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= 32 | ${ExternalProject_CMAKE_ARGS} 33 | ${ExternalProject_CMAKE_CXX_FLAGS} 34 | # custom flags 35 | -DTBB_BUILD_TBBMALLOC=ON 36 | -DTBB_BUILD_SHARED=OFF 37 | -DTBB_BUILD_STATIC=ON 38 | -DTBB_BUILD_TESTS=OFF) 39 | 40 | # Simulate importing TBB::tbb for OpenVDBHelper target 41 | ExternalProject_Get_Property(external_tbb INSTALL_DIR) 42 | set(TBB_ROOT ${INSTALL_DIR} CACHE INTERNAL "TBB_ROOT Install directory") 43 | add_library(TBBHelper INTERFACE) 44 | add_dependencies(TBBHelper external_tbb) 45 | target_include_directories(TBBHelper INTERFACE ${INSTALL_DIR}/include) 46 | target_link_directories(TBBHelper INTERFACE ${INSTALL_DIR}/lib) 47 | target_link_libraries(TBBHelper INTERFACE tbb Threads::Threads) 48 | add_library(TBB::tbb ALIAS TBBHelper) 49 | -------------------------------------------------------------------------------- /cpp/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kiss_matcher_example) 3 | 4 | # Set a default build type if none is specified 5 | if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 6 | message(STATUS "Setting build type to 'Release' as none was specified.") 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build." FORCE) 8 | # Provide options for cmake-gui or ccmake 9 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 10 | endif() 11 | set(CMAKE_CXX_STANDARD 17) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | set(CMAKE_CXX_EXTENSIONS OFF) 14 | 15 | include(FetchContent) 16 | 17 | find_package(Eigen3 REQUIRED) 18 | find_package(teaserpp REQUIRED) 19 | find_package(PCL REQUIRED) 20 | ### To include ROBIN& KISS-Matcher ### 21 | find_package(robin REQUIRED) 22 | find_package(kiss_matcher REQUIRED) 23 | 24 | # The best way to set proper compiler settings for using OpenMP in all platforms 25 | include(FindOpenMP) 26 | if (OPENMP_FOUND) 27 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 28 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 29 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 30 | else (OPENMP_FOUND) 31 | message("ERROR: OpenMP could not be found.") 32 | endif (OPENMP_FOUND) 33 | 34 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 35 | include(3rdparty/find_dependencies.cmake) 36 | include(3rdparty/download_datasets.cmake) 37 | 38 | include_directories(${PCL_INCLUDE_DIRS}) 39 | link_directories(${PCL_LIBRARY_DIRS}) 40 | add_definitions(${PCL_DEFINITIONS}) 41 | 42 | include_directories( 43 | include 44 | src 45 | ) 46 | 47 | add_executable(run_matching_using_bunny src/run_matching_using_bunny.cc) 48 | target_include_directories(run_matching_using_bunny 49 | PUBLIC 50 | ${PCL_INCLUDE_DIRS} 51 | ) 52 | target_link_libraries(run_matching_using_bunny 53 | Eigen3::Eigen 54 | teaserpp::teaser_registration 55 | teaserpp::teaser_io 56 | TBB::tbb 57 | kiss_matcher::kiss_matcher_core 58 | robin::robin 59 | ${PCL_LIBRARIES} 60 | ) 61 | 62 | add_executable(run_kiss_matcher src/run_kiss_matcher.cc) 63 | target_include_directories(run_kiss_matcher 64 | PUBLIC 65 | ${PCL_INCLUDE_DIRS} 66 | ) 67 | target_link_libraries(run_kiss_matcher 68 | Eigen3::Eigen 69 | TBB::tbb 70 | kiss_matcher::kiss_matcher_core 71 | robin::robin 72 | ${PCL_LIBRARIES} 73 | ) 74 | 75 | add_executable(downsampling_speed_comparison src/downsampling_speed_comparison.cc) 76 | target_link_libraries(downsampling_speed_comparison PRIVATE ${PCL_LIBRARIES} TBB::tbb) 77 | 78 | add_executable(conversion_speed_comparison src/conversion_speed_comparison.cc) 79 | target_link_libraries(conversion_speed_comparison PRIVATE ${PCL_LIBRARIES} TBB::tbb) 80 | -------------------------------------------------------------------------------- /cpp/examples/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/KISS-Matcher/908fa6c2c8c63ec6f348d2bccf68268277d5f40f/cpp/examples/COLCON_IGNORE -------------------------------------------------------------------------------- /cpp/examples/include/quatro/fpfh.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2020, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Jingnan Shi, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "teaser/geometry.h" 16 | 17 | namespace teaser { 18 | 19 | using FPFHCloud = pcl::PointCloud; 20 | using FPFHCloudPtr = pcl::PointCloud::Ptr; 21 | 22 | class FPFHEstimation { 23 | public: 24 | FPFHEstimation() 25 | : fpfh_estimation_( 26 | new pcl::FPFHEstimationOMP) {} 27 | 28 | /** 29 | * Compute FPFH features. 30 | * 31 | * @return A shared pointer to the FPFH feature point cloud 32 | * @param input_cloud 33 | * @param normal_search_radius Radius for estimating normals 34 | * @param fpfh_search_radius Radius for calculating FPFH (needs to be at least normalSearchRadius) 35 | */ 36 | FPFHCloudPtr computeFPFHFeatures(const PointCloud& input_cloud, 37 | double normal_search_radius = 0.03, 38 | double fpfh_search_radius = 0.05); 39 | 40 | /** 41 | * Return the pointer to the underlying pcl::FPFHEstimation object 42 | * @return 43 | */ 44 | inline pcl::FPFHEstimationOMP::Ptr 45 | getImplPointer() const { 46 | return fpfh_estimation_; 47 | } 48 | 49 | private: 50 | // pcl::FPFHEstimation::Ptr fpfh_estimation_; 51 | pcl::FPFHEstimationOMP::Ptr fpfh_estimation_; 52 | 53 | /** 54 | * Wrapper function for the corresponding PCL function. 55 | * @param output_cloud 56 | */ 57 | void compute(pcl::PointCloud& output_cloud); 58 | 59 | /** 60 | * Wrapper function for the corresponding PCL function. 61 | * @param input_cloud 62 | */ 63 | void setInputCloud(pcl::PointCloud::Ptr input_cloud); 64 | 65 | /** 66 | * Wrapper function for the corresponding PCL function. 67 | * @param input_normals 68 | */ 69 | void setInputNormals(pcl::PointCloud::Ptr input_normals); 70 | 71 | /** 72 | * Wrapper function for the corresponding PCL function. 73 | * @param search_method 74 | */ 75 | void setSearchMethod(pcl::search::KdTree::Ptr search_method); 76 | 77 | /** 78 | * Wrapper function for the corresponding PCL function. 79 | */ 80 | void setRadiusSearch(double); 81 | }; 82 | 83 | } // namespace teaser 84 | -------------------------------------------------------------------------------- /cpp/examples/include/quatro/matcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2020, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Jingnan Shi, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include "quatro/fpfh.h" 19 | 20 | namespace teaser { 21 | 22 | class Matcher { 23 | public: 24 | typedef std::vector Feature; 25 | typedef flann::Index> KDTree; 26 | 27 | // New methods 28 | // Public methods: 29 | // 1. calculateCorrespondences 30 | // input: source point cloud, target point cloud 31 | // output: correspondences 32 | Matcher() = default; 33 | 34 | /** 35 | * Calculate correspondences based on given features and point clouds. 36 | * @param source_points 37 | * @param target_points 38 | * @param use_absolute_scale 39 | * @param use_crosscheck 40 | * @param use_tuple_test 41 | * @return 42 | */ 43 | std::vector> calculateCorrespondences(teaser::PointCloud& source_points, 44 | teaser::PointCloud& target_points, 45 | teaser::FPFHCloud& source_features, 46 | teaser::FPFHCloud& target_features, 47 | bool use_absolute_scale = true, 48 | bool use_crosscheck = true, 49 | bool use_tuple_test = true, 50 | float tuple_scale = 0, 51 | bool use_optimized_matching = true); 52 | 53 | float thr_dist_ = 30; // Empirically, the matching whose dist is larger than 30 is highly likely 54 | // to be an outlier 55 | float num_max_corres_ = 56 | 600; // Empirically, too many correspondences cause slow down of the system 57 | 58 | private: 59 | template 60 | void buildKDTree(const std::vector& data, KDTree* tree); 61 | 62 | template 63 | void buildKDTreeWithTBB(const std::vector& data, KDTree* tree); 64 | 65 | template 66 | void searchKDTree(KDTree* tree, 67 | const T& input, 68 | std::vector& indices, 69 | std::vector& dists, 70 | int nn); 71 | 72 | template 73 | void searchKDTreeAll(Matcher::KDTree* tree, 74 | const std::vector& inputs, 75 | std::vector& indices, 76 | std::vector& dists, 77 | int nn); 78 | 79 | void advancedMatching(bool use_crosscheck, bool use_tuple_test, float tuple_scale); 80 | 81 | void optimizedMatching(float thr_dist, int num_max_corres, float tuple_scale); 82 | 83 | void normalizePoints(bool use_absolute_scale); 84 | 85 | std::vector> corres_; 86 | std::vector pointcloud_; 87 | std::vector features_; 88 | std::vector> 89 | means_; // for normalization 90 | float global_scale_; 91 | }; 92 | 93 | } // namespace teaser 94 | -------------------------------------------------------------------------------- /cpp/examples/include/quatro/quatro_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef CPP_EXAMPLES_INCLUDE_QUATRO_QUATRO_UTILS_H_ 2 | #define CPP_EXAMPLES_INCLUDE_QUATRO_QUATRO_UTILS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | // Due to the version of PCL (Original: 1.9 / Mine: 1.8 in Ubuntu 18.04) 16 | // I copied raw files of TEASER++ 17 | // The original files can be found in: 18 | // github.com/MIT-SPARK/TEASER-plusplus/tree/master/teaser/include/teaser 19 | #include 20 | #include 21 | #include 22 | 23 | #include "quatro/fpfh.h" 24 | #include "quatro/matcher.h" 25 | 26 | // Macro constants for generating noise and outliers 27 | #define NOISE_BOUND 0.05 28 | #define N_OUTLIERS 700 29 | #define OUTLIER_TRANSLATION_LB 0.6 30 | #define OUTLIER_TRANSLATION_UB 2.0 31 | 32 | inline Eigen::Matrix3d get3DRot(const double yaw_deg, 33 | const double pitch_deg, 34 | const double roll_deg) { 35 | double yaw = yaw_deg * M_PI / 180.0; 36 | double pitch = pitch_deg * M_PI / 180.0; 37 | double roll = roll_deg * M_PI / 180.0; 38 | 39 | Eigen::Matrix3d yaw_mat = Eigen::Matrix3d::Identity(); 40 | Eigen::Matrix3d pitch_mat = Eigen::Matrix3d::Identity(); 41 | Eigen::Matrix3d roll_mat = Eigen::Matrix3d::Identity(); 42 | double cy = cos(yaw); 43 | double sy = sin(yaw); 44 | yaw_mat(0, 0) = cy; 45 | yaw_mat(0, 1) = -sy; 46 | yaw_mat(1, 0) = sy; 47 | yaw_mat(1, 1) = cy; 48 | double cp = cos(pitch); 49 | double sp = sin(pitch); 50 | pitch_mat(0, 0) = cp; 51 | pitch_mat(0, 2) = sp; 52 | pitch_mat(2, 0) = -sp; 53 | pitch_mat(2, 2) = cp; 54 | double cr = cos(roll); 55 | double sr = sin(roll); 56 | roll_mat(1, 1) = cr; 57 | roll_mat(1, 2) = -sr; 58 | roll_mat(2, 1) = sr; 59 | roll_mat(2, 2) = cr; 60 | 61 | return yaw_mat * pitch_mat * roll_mat; 62 | } 63 | 64 | inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) { 65 | return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0))); 66 | } 67 | 68 | void loadCloud(const std::string filename, pcl::PointCloud &cloud) { 69 | FILE *file = fopen(filename.c_str(), "rb"); 70 | if (!file) { 71 | std::cerr << "error: failed to load " << filename << std::endl; 72 | } 73 | 74 | std::vector buffer(1000000); 75 | size_t num_points = 76 | fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 77 | fclose(file); 78 | 79 | cloud.reserve(num_points); 80 | 81 | pcl::PointXYZ pt; 82 | for (int i = 0; i < num_points; i++) { 83 | pt.x = buffer[i * 4]; 84 | pt.y = buffer[i * 4 + 1]; 85 | pt.z = buffer[i * 4 + 2]; 86 | cloud.push_back(pt); 87 | // Intensity is not in use 88 | // pt.intensity = buffer[i * 4 + 3]; 89 | } 90 | } 91 | 92 | void voxelize(pcl::PointCloud::Ptr pc_src, 93 | pcl::PointCloud &pc_dst, 94 | float var_voxel_size) { 95 | static pcl::VoxelGrid voxel_filter; 96 | voxel_filter.setInputCloud(pc_src); 97 | voxel_filter.setLeafSize(var_voxel_size, var_voxel_size, var_voxel_size); 98 | voxel_filter.filter(pc_dst); 99 | } 100 | 101 | void calcErrors(const Eigen::Matrix4d &T, 102 | const Eigen::Matrix3d est_rot, 103 | const Eigen::Vector3d est_ts, 104 | double &rot_error, 105 | double &ts_error) { 106 | rot_error = getAngularError(T.topLeftCorner(3, 3), est_rot) * 180.0 / M_PI; 107 | ts_error = (T.topRightCorner(3, 1) - est_ts).norm(); 108 | } 109 | 110 | void colorize(const pcl::PointCloud &pc, 111 | pcl::PointCloud &pc_colored, 112 | const std::vector &color) { 113 | int N = pc.points.size(); 114 | 115 | pc_colored.clear(); 116 | pcl::PointXYZRGB pt_tmp; 117 | for (int i = 0; i < N; ++i) { 118 | const auto &pt = pc.points[i]; 119 | pt_tmp.x = pt.x; 120 | pt_tmp.y = pt.y; 121 | pt_tmp.z = pt.z; 122 | pt_tmp.r = color[0]; 123 | pt_tmp.g = color[1]; 124 | pt_tmp.b = color[2]; 125 | pc_colored.points.emplace_back(pt_tmp); 126 | } 127 | } 128 | 129 | void getParams(const double noise_bound, 130 | const std::string reg_type, 131 | const std::string robin_mode, 132 | teaser::RobustRegistrationSolver::Params ¶ms) { 133 | params.noise_bound = noise_bound; 134 | params.cbar2 = 1; 135 | params.estimate_scaling = false; 136 | params.rotation_max_iterations = 100; 137 | params.rotation_gnc_factor = 1.4; 138 | if (reg_type == "Quatro") { 139 | params.rotation_estimation_algorithm = 140 | teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO; 141 | params.inlier_selection_mode == 142 | teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU; 143 | } else if (reg_type == "TEASER") { 144 | params.rotation_estimation_algorithm = 145 | teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; 146 | params.inlier_selection_mode == 147 | teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT; 148 | } else { 149 | throw std::invalid_argument("Not implemented!"); 150 | } 151 | if (robin_mode == "max_clique" || robin_mode == "max_core") { 152 | params.use_max_clique = false; 153 | } 154 | params.rotation_cost_threshold = 0.0002; 155 | } 156 | 157 | #endif // CPP_EXAMPLES_INCLUDE_QUATRO_QUATRO_UTILS_H_ 158 | -------------------------------------------------------------------------------- /cpp/examples/src/conversion_speed_comparison.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | int main(int argc, char** argv) { 14 | // E.g., 15 | // ./downsampling_speed_comparison 16 | // /media/shapelim/UX960NVMe1/kiss-matcher-viz/vbr-colosseo/target.pcd 17 | const std::string input_pcd_file = argv[1]; 18 | 19 | // Load points 20 | pcl::PointCloud::Ptr points_pcl(new pcl::PointCloud); 21 | int src_load_result = pcl::io::loadPCDFile(input_pcd_file, *points_pcl); 22 | 23 | // -------------------------------------------------------------------------------- 24 | // convert PCL points to kiss_matcher PointCloud 25 | auto start_conversion = std::chrono::high_resolution_clock::now(); 26 | std::vector points_each; 27 | points_each.resize(points_pcl->size()); 28 | for (size_t i = 0; i < points_pcl->size(); ++i) { 29 | points_each[i] = 30 | Eigen::Vector3f(points_pcl->points[i].x, points_pcl->points[i].y, points_pcl->points[i].z); 31 | } 32 | auto end_each_conversion = std::chrono::high_resolution_clock::now(); 33 | auto conversion_time = 34 | std::chrono::duration(end_each_conversion - start_conversion).count(); 35 | std::cout << "Time taken (raw): " << conversion_time << " ms" << std::endl; 36 | 37 | std::vector points_eigen; 38 | points_eigen.resize(points_pcl->size()); 39 | for (size_t i = 0; i < points_pcl->size(); ++i) { 40 | points_eigen[i] = points_pcl->points[i].getVector3fMap(); 41 | } 42 | auto end_eigen_conversion = std::chrono::high_resolution_clock::now(); 43 | conversion_time = 44 | std::chrono::duration(end_eigen_conversion - end_each_conversion).count(); 45 | std::cout << "Time taken (getVector3fMap): " << conversion_time << " ms" << std::endl; 46 | 47 | std::vector points_memcpy(points_pcl->size()); 48 | std::memcpy(points_memcpy.data(), 49 | points_pcl->points.data(), 50 | points_pcl->size() * sizeof(Eigen::Vector3f)); 51 | auto end_memcpy_conversion = std::chrono::high_resolution_clock::now(); 52 | conversion_time = 53 | std::chrono::duration(end_memcpy_conversion - end_eigen_conversion) 54 | .count(); 55 | std::cout << "Conversion time (Memcpy): " << conversion_time << " ms" << std::endl; 56 | 57 | // ToDo(hlim): Hmmm...this is much faster. Should we change the pipeline? 58 | Eigen::Map> points_eigen_map( 59 | reinterpret_cast(points_pcl->points.data()), 3, points_pcl->size()); 60 | auto end_map_conversion = std::chrono::high_resolution_clock::now(); 61 | conversion_time = 62 | std::chrono::duration(end_map_conversion - end_memcpy_conversion).count(); 63 | std::cout << "Time taken (Eigen-map): " << conversion_time << " ms" << std::endl; 64 | std::cout << points_eigen_map.rows() << ", " << points_eigen_map.cols() << std::endl; 65 | 66 | std::vector points_map_then_assign(points_pcl->size()); 67 | for (size_t i = 0; i < points_pcl->size(); ++i) { 68 | points_map_then_assign[i] = points_eigen_map.col(i); 69 | } 70 | auto end_map_to_vector_conversion = std::chrono::high_resolution_clock::now(); 71 | conversion_time = 72 | std::chrono::duration(end_map_to_vector_conversion - end_map_conversion) 73 | .count(); 74 | std::cout << "Conversion time (map then assign): " << conversion_time << " ms" << std::endl; 75 | 76 | std::vector points_fast(points_pcl->size()); 77 | Eigen::Map>( 78 | reinterpret_cast(points_fast.data()), 3, points_pcl->size()) = 79 | Eigen::Map>( 80 | reinterpret_cast(points_pcl->points.data()), 3, points_pcl->size()); 81 | 82 | auto end_direct_map_conversion = std::chrono::high_resolution_clock::now(); 83 | conversion_time = std::chrono::duration(end_direct_map_conversion - 84 | end_map_to_vector_conversion) 85 | .count(); 86 | std::cout << "Conversion time (Direct Eigen-map to std::vector): " << conversion_time << " ms" 87 | << std::endl; 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /cpp/examples/src/fpfh.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2020, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Jingnan Shi, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | */ 8 | 9 | #include "quatro/fpfh.h" 10 | 11 | #include 12 | 13 | teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( 14 | const teaser::PointCloud& input_cloud, 15 | double normal_search_radius, 16 | double fpfh_search_radius) { 17 | // Intermediate variables 18 | pcl::PointCloud::Ptr normals(new pcl::PointCloud); 19 | teaser::FPFHCloudPtr descriptors(new pcl::PointCloud()); 20 | pcl::PointCloud::Ptr pcl_input_cloud(new pcl::PointCloud); 21 | for (auto& i : input_cloud) { 22 | pcl::PointXYZ p(i.x, i.y, i.z); 23 | pcl_input_cloud->push_back(p); 24 | } 25 | 26 | // Estimate normals 27 | pcl::NormalEstimation normalEstimation; 28 | normalEstimation.setInputCloud(pcl_input_cloud); 29 | normalEstimation.setRadiusSearch(normal_search_radius); 30 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); 31 | normalEstimation.setSearchMethod(kdtree); 32 | normalEstimation.compute(*normals); 33 | 34 | // Estimate FPFH 35 | setInputCloud(pcl_input_cloud); 36 | setInputNormals(normals); 37 | setSearchMethod(kdtree); 38 | setRadiusSearch(fpfh_search_radius); 39 | compute(*descriptors); 40 | 41 | return descriptors; 42 | } 43 | 44 | void teaser::FPFHEstimation::setInputCloud(pcl::PointCloud::Ptr input_cloud) { 45 | fpfh_estimation_->setInputCloud(input_cloud); 46 | } 47 | 48 | void teaser::FPFHEstimation::setInputNormals(pcl::PointCloud::Ptr input_normals) { 49 | fpfh_estimation_->setInputNormals(input_normals); 50 | } 51 | 52 | void teaser::FPFHEstimation::setSearchMethod( 53 | pcl::search::KdTree::Ptr search_method) { 54 | fpfh_estimation_->setSearchMethod(search_method); 55 | } 56 | 57 | void teaser::FPFHEstimation::compute(pcl::PointCloud& output_cloud) { 58 | fpfh_estimation_->compute(output_cloud); 59 | } 60 | void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); } 61 | -------------------------------------------------------------------------------- /cpp/examples/src/run_matching_using_bunny.cc: -------------------------------------------------------------------------------- 1 | // An example showing TEASER++ registration with FPFH features with the Stanford bunny model 2 | #include 3 | #include 4 | 5 | #include "quatro/quatro_utils.h" 6 | 7 | int main() { 8 | // Load the .ply file 9 | teaser::PLYReader reader; 10 | teaser::PointCloud src_cloud; 11 | auto status = reader.read("./data/bun_zipper.ply", src_cloud); 12 | int N = src_cloud.size(); 13 | 14 | // Convert the point cloud to Eigen 15 | Eigen::Matrix src(3, N); 16 | for (size_t i = 0; i < N; ++i) { 17 | src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z; 18 | } 19 | 20 | // Homogeneous coordinates 21 | Eigen::Matrix src_h; 22 | src_h.resize(4, src.cols()); 23 | src_h.topRows(3) = src; 24 | src_h.bottomRows(1) = Eigen::Matrix::Ones(N); 25 | 26 | // Apply an arbitrary SE(3) transformation 27 | Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); 28 | Eigen::Matrix3d random_rot = get3DRot(60, 0, 0); 29 | T.block<3, 3>(0, 0) = random_rot; 30 | T(0, 3) = -0.02576939; 31 | T(1, 3) = -0.037705398; 32 | 33 | // Apply transformation 34 | Eigen::Matrix tgt_h = T * src_h; 35 | Eigen::Matrix tgt = tgt_h.topRows(3); 36 | 37 | // Convert to teaser point cloud 38 | teaser::PointCloud tgt_cloud; 39 | for (size_t i = 0; i < tgt.cols(); ++i) { 40 | tgt_cloud.push_back({static_cast(tgt(0, i)), 41 | static_cast(tgt(1, i)), 42 | static_cast(tgt(2, i))}); 43 | } 44 | 45 | std::string robin_mode = "max_core"; 46 | float voxel_size = 0.01; 47 | kiss_matcher::KISSMatcherConfig config = kiss_matcher::KISSMatcherConfig(voxel_size); 48 | kiss_matcher::KISSMatcher matcher(config); 49 | 50 | const auto &[src_matched, tgt_matched] = matcher.match(src, tgt); 51 | 52 | int M = src_matched.size(); 53 | Eigen::Matrix src_matched_eigen(3, M); 54 | Eigen::Matrix tgt_matched_eigen(3, M); 55 | 56 | for (int m = 0; m < M; ++m) { 57 | src_matched_eigen.col(m) << src_matched[m].cast(); 58 | tgt_matched_eigen.col(m) << tgt_matched[m].cast(); 59 | } 60 | 61 | // Prepare solver parameters 62 | teaser::RobustRegistrationSolver::Params quatro_param, teaser_param; 63 | getParams(NOISE_BOUND / 2, "Quatro", robin_mode, quatro_param); 64 | std::chrono::steady_clock::time_point begin_q = std::chrono::steady_clock::now(); 65 | teaser::RobustRegistrationSolver Quatro(quatro_param); 66 | Quatro.solve(src_matched_eigen, tgt_matched_eigen); 67 | std::chrono::steady_clock::time_point end_q = std::chrono::steady_clock::now(); 68 | auto solution_by_quatro = Quatro.getSolution(); 69 | 70 | getParams(NOISE_BOUND / 2, "TEASER", robin_mode, teaser_param); 71 | std::chrono::steady_clock::time_point begin_t = std::chrono::steady_clock::now(); 72 | teaser::RobustRegistrationSolver TEASER(teaser_param); 73 | TEASER.solve(src_matched_eigen, tgt_matched_eigen); 74 | std::chrono::steady_clock::time_point end_t = std::chrono::steady_clock::now(); 75 | auto solution_by_teaser = TEASER.getSolution(); 76 | 77 | std::cout << "=====================================" << std::endl; 78 | std::cout << " Quatro Results " << std::endl; 79 | std::cout << "=====================================" << std::endl; 80 | double rot_error_quatro, ts_error_quatro; 81 | calcErrors(T, 82 | solution_by_quatro.rotation, 83 | solution_by_quatro.translation, 84 | rot_error_quatro, 85 | ts_error_quatro); 86 | // Compare results 87 | std::cout << "Error (deg): " << rot_error_quatro << std::endl; 88 | std::cout << "Estimated translation (m): " << ts_error_quatro << std::endl; 89 | std::cout << "Time taken (s): " 90 | << std::chrono::duration_cast(end_q - begin_q).count() / 91 | 1000000.0 92 | << std::endl; 93 | 94 | std::cout << "=====================================" << std::endl; 95 | std::cout << " TEASER++ Results " << std::endl; 96 | std::cout << "=====================================" << std::endl; 97 | double rot_error_teaser, ts_error_teaser; 98 | calcErrors(T, 99 | solution_by_teaser.rotation, 100 | solution_by_teaser.translation, 101 | rot_error_teaser, 102 | ts_error_teaser); 103 | // Compare results 104 | std::cout << "Error (deg): " << rot_error_teaser << std::endl; 105 | std::cout << "Estimated translation (m): " << ts_error_teaser << std::endl; 106 | std::cout << "Time taken (s): " 107 | << std::chrono::duration_cast(end_t - begin_t).count() / 108 | 1000000.0 109 | << std::endl; 110 | 111 | // Visualization 112 | pcl::PointCloud src_raw; 113 | pcl::PointCloud tgt_raw; 114 | pcl::PointCloud est_q, est_t; 115 | 116 | for (int k = 0; k < src_cloud.size(); ++k) { 117 | src_raw.push_back(pcl::PointXYZ(src_cloud[k].x, src_cloud[k].y, src_cloud[k].z)); 118 | tgt_raw.push_back(pcl::PointXYZ(tgt_cloud[k].x, tgt_cloud[k].y, tgt_cloud[k].z)); 119 | } 120 | Eigen::Matrix4f solution_eigen = Eigen::Matrix4f::Identity(); 121 | solution_eigen.block<3, 3>(0, 0) = solution_by_quatro.rotation.cast(); 122 | solution_eigen.topRightCorner(3, 1) = solution_by_quatro.translation.cast(); 123 | std::cout << solution_eigen << std::endl; 124 | pcl::transformPointCloud(src_raw, est_q, solution_eigen); 125 | 126 | solution_eigen.block<3, 3>(0, 0) = solution_by_teaser.rotation.cast(); 127 | solution_eigen.topRightCorner(3, 1) = solution_by_teaser.translation.cast(); 128 | std::cout << solution_eigen << std::endl; 129 | pcl::transformPointCloud(src_raw, est_t, solution_eigen); 130 | 131 | pcl::PointCloud::Ptr src_colored(new pcl::PointCloud); 132 | pcl::PointCloud::Ptr tgt_colored(new pcl::PointCloud); 133 | pcl::PointCloud::Ptr est_q_colored(new pcl::PointCloud); 134 | pcl::PointCloud::Ptr est_t_colored(new pcl::PointCloud); 135 | 136 | // If you want to save it, please use below line 137 | // pcl::io::savePCDFileASCII("your_path/src_warped.pcd", est_t); 138 | 139 | colorize(src_raw, *src_colored, {255, 0, 0}); 140 | colorize(tgt_raw, *tgt_colored, {0, 255, 0}); 141 | colorize(est_q, *est_q_colored, {0, 0, 255}); 142 | colorize(est_t, *est_t_colored, {255, 0, 255}); 143 | 144 | pcl::visualization::PCLVisualizer viewer1("Simple Cloud Viewer"); 145 | viewer1.addPointCloud(src_colored, "src_red"); 146 | viewer1.addPointCloud(tgt_colored, "tgt_green"); 147 | viewer1.addPointCloud(est_q_colored, "est_q_blue"); 148 | viewer1.addPointCloud(est_t_colored, "est_t_magenta"); 149 | 150 | while (!viewer1.wasStopped()) { 151 | viewer1.spinOnce(); 152 | } 153 | } 154 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/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/kiss_matcher/3rdparty/eigen/eigen.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | 24 | set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") 25 | set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") 26 | set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") 27 | set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") 28 | set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") 29 | 30 | include(FetchContent) 31 | FetchContent_Declare(eigen URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.tar.gz 32 | PATCH_COMMAND patch -p1 < ${CMAKE_CURRENT_LIST_DIR}/eigen.patch UPDATE_DISCONNECTED 1 33 | OVERRIDE_FIND_PACKAGE) 34 | FetchContent_GetProperties(eigen) 35 | if(NOT eigen_POPULATED) 36 | FetchContent_Populate(eigen) 37 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 38 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 39 | else() 40 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 41 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 42 | add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) 43 | get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) 44 | set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") 45 | endif() 46 | endif() 47 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/3rdparty/eigen/eigen.patch: -------------------------------------------------------------------------------- 1 | commit cf82186416d04ea5df2a397d8fe09dc78d40ca65 2 | Author: Antonio Sánchez 3 | Date: Sat Mar 5 05:49:45 2022 +0000 4 | 5 | Adds new CMake Options for controlling build components. 6 | 7 | diff --git a/CMakeLists.txt b/CMakeLists.txt 8 | index de1c23e91..0af36a53a 100644 9 | --- a/CMakeLists.txt 10 | +++ b/CMakeLists.txt 11 | @@ -477,6 +477,9 @@ if(EIGEN_BUILD_TESTING) 12 | add_subdirectory(failtest) 13 | endif() 14 | 15 | +include(CMakeDetermineFortranCompiler) 16 | +option(EIGEN_BUILD_BLAS "Toggles the building of the Eigen Blas library" ${CMAKE_Fortran_COMPILER}) 17 | +option(EIGEN_BUILD_LAPACK "Toggles the building of the included Eigen LAPACK library" ${CMAKE_Fortran_COMPILER}) 18 | if(EIGEN_LEAVE_TEST_IN_ALL_TARGET) 19 | add_subdirectory(blas) 20 | add_subdirectory(lapack) 21 | @@ -611,6 +614,8 @@ set_target_properties (eigen PROPERTIES EXPORT_NAME Eigen) 22 | 23 | install (TARGETS eigen EXPORT Eigen3Targets) 24 | 25 | +option(EIGEN_BUILD_CMAKE_PACKAGE "Enables the creation of EigenConfig.cmake and related files" ON) 26 | +if(EIGEN_BUILD_CMAKE_PACKAGE) 27 | configure_package_config_file ( 28 | ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Eigen3Config.cmake.in 29 | ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 30 | @@ -655,6 +660,7 @@ install (FILES ${CMAKE_CURRENT_BINARY_DIR}/Eigen3Config.cmake 31 | # Add uninstall target 32 | add_custom_target ( uninstall 33 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/EigenUninstall.cmake) 34 | +endif() 35 | 36 | if (EIGEN_SPLIT_TESTSUITE) 37 | ei_split_testsuite("${EIGEN_SPLIT_TESTSUITE}") 38 | diff --git a/blas/CMakeLists.txt b/blas/CMakeLists.txt 39 | index 8d3cb86dc..c530957fb 100644 40 | --- a/blas/CMakeLists.txt 41 | +++ b/blas/CMakeLists.txt 42 | @@ -1,6 +1,7 @@ 43 | 44 | project(EigenBlas CXX) 45 | 46 | +if(EIGEN_BUILD_BLAS) 47 | include(CheckLanguage) 48 | check_language(Fortran) 49 | if(CMAKE_Fortran_COMPILER) 50 | @@ -59,4 +60,4 @@ if(EIGEN_BUILD_TESTING) 51 | endif() 52 | 53 | endif() 54 | - 55 | +endif() 56 | diff --git a/lapack/CMakeLists.txt b/lapack/CMakeLists.txt 57 | index c8ca64001..8d6d75401 100644 58 | --- a/lapack/CMakeLists.txt 59 | +++ b/lapack/CMakeLists.txt 60 | @@ -1,5 +1,7 @@ 61 | project(EigenLapack CXX) 62 | 63 | +if(EIGEN_BUILD_LAPACK AND EIGEN_BUILD_BLAS) 64 | + 65 | include(CheckLanguage) 66 | check_language(Fortran) 67 | if(CMAKE_Fortran_COMPILER) 68 | @@ -457,3 +459,6 @@ if(EXISTS ${eigen_full_path_to_testing_lapack}) 69 | 70 | endif() 71 | 72 | +elseif(EIGEN_BUILD_LAPACK AND NOT EIGEN_BUILD_BLAS) 73 | + message(FATAL_ERROR "EIGEN_BUILD_LAPACK requires EIGEN_BUILD_BLAS") 74 | +endif() #EIGEN_BUILD_LAPACK 75 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/3rdparty/find_dependencies.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # Silence timestamp warning 24 | if(CMAKE_VERSION VERSION_GREATER 3.24) 25 | cmake_policy(SET CMP0135 OLD) 26 | endif() 27 | 28 | function(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) 29 | string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) 30 | set(USE_FROM_SYSTEM_OPTION "USE_SYSTEM_${PACKAGE_NAME_UP}") 31 | if(${${USE_FROM_SYSTEM_OPTION}}) 32 | find_package(${PACKAGE_NAME} QUIET NO_MODULE) 33 | endif() 34 | if(NOT ${${USE_FROM_SYSTEM_OPTION}} OR NOT TARGET ${TARGET_NAME}) 35 | set(${USE_FROM_SYSTEM_OPTION} OFF PARENT_SCOPE) 36 | include(${INCLUDED_CMAKE_PATH}) 37 | endif() 38 | endfunction() 39 | 40 | find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") 41 | find_external_dependency("TBB" "TBB::tbb" "${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake") 42 | find_external_dependency("robin" "robin::robin" "${CMAKE_CURRENT_LIST_DIR}/robin/robin.cmake") 43 | # ToDos 44 | # find_external_dependency("tsl-robin-map" "tsl::robin_map" "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") 45 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/3rdparty/robin/robin.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Hyungtae Lim and coauthors. 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(hlim) `OFF` means that we gonna generate static library to make it more independent 24 | # Thus, `libpmc.a` will be created 25 | option(PMC_BUILD_SHARED "Build pmc as a shared library (.so)" OFF) 26 | 27 | include(FetchContent) 28 | FetchContent_Declare(robin URL https://github.com/MIT-SPARK/ROBIN/archive/refs/tags/v.1.2.4.tar.gz) 29 | FetchContent_GetProperties(robin) 30 | if(NOT robin) 31 | FetchContent_Populate(robin) 32 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 33 | add_subdirectory(${robin_SOURCE_DIR} ${robin_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 34 | else() 35 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 36 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 37 | add_subdirectory(${robin_SOURCE_DIR} ${robin_BINARY_DIR} EXCLUDE_FROM_ALL) 38 | get_target_property(robin_include_dirs robin INTERFACE_INCLUDE_DIRECTORIES) 39 | set_target_properties(robin PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${robin_include_dirs}") 40 | endif() 41 | endif() 42 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/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 "TBB BUILD_SHARED_LIBS" ON) 24 | option(TBBMALLOC_BUILD "TBB TBBMALLOC_BUILD" ON) 25 | option(TBB_EXAMPLES "TBB TBB_EXAMPLES" OFF) 26 | option(TBB_STRICT "TBB TBB_STRICT" OFF) 27 | option(TBB_TEST "TBB TBB_TEST" OFF) 28 | #option(TBB_SANITIZE "TBB TBB_SANITIZE" "thread") 29 | 30 | include(FetchContent) 31 | FetchContent_Declare(TBB 32 | URL https://github.com/uxlfoundation/oneTBB/archive/refs/tags/v2022.0.0.tar.gz 33 | OVERRIDE_FIND_PACKAGE) 34 | FetchContent_MakeAvailable(TBB) 35 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/3rdparty/teaserpp/teaserpp.cmake: -------------------------------------------------------------------------------- 1 | option(BUILD_PYTHON_BINDINGS "Build Python bindings" OFF) 2 | 3 | include(FetchContent) 4 | 5 | FetchContent_Declare( 6 | googletest 7 | GIT_REPOSITORY https://github.com/google/googletest.git 8 | GIT_TAG release-1.8.1 9 | BINARY_DIR ${CMAKE_BINARY_DIR}/googletest-src 10 | BINARY_DIR ${CMAKE_BINARY_DIR}/googletest-build 11 | ) 12 | FetchContent_MakeAvailable(googletest) 13 | 14 | FetchContent_Declare( 15 | pmc 16 | GIT_REPOSITORY https://github.com/jingnanshi/pmc.git 17 | GIT_TAG master 18 | ) 19 | FetchContent_MakeAvailable(pmc) 20 | 21 | FetchContent_Declare( 22 | tinyply 23 | GIT_REPOSITORY https://github.com/jingnanshi/tinyply.git 24 | GIT_TAG 0b9fff8e8bd4d37256554fe40cf76b2f3134377b 25 | ) 26 | FetchContent_MakeAvailable(tinyply) 27 | 28 | add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR} EXCLUDE_FROM_ALL) 29 | add_subdirectory(${pmc_SOURCE_DIR} ${pmc_BINARY_DIR}) 30 | add_subdirectory(${tinyply_SOURCE_DIR} ${tinyply_BINARY_DIR}) 31 | 32 | FetchContent_Declare(teaserpp URL https://github.com/MIT-SPARK/TEASER-plusplus/archive/refs/tags/v2.0.tar.gz) 33 | FetchContent_GetProperties(teaserpp) 34 | if(NOT teaserpp_POPULATED) 35 | FetchContent_Populate(teaserpp) 36 | if(${CMAKE_VERSION} GREATER_EQUAL 3.25) 37 | add_subdirectory(${teaserpp_SOURCE_DIR} ${teaserpp_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) 38 | else() 39 | # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will 40 | # consider this 3rdparty headers as source code and fail due the -Werror flag. 41 | add_subdirectory(${teaserpp_SOURCE_DIR} ${teaserpp_BINARY_DIR} EXCLUDE_FROM_ALL) 42 | get_target_property(teaserpp_include_dirs teaserpp INTERFACE_INCLUDE_DIRECTORIES) 43 | set_target_properties(teaserpp PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${teaserpp_include_dirs}") 44 | endif() 45 | endif() 46 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/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/kiss_matcher/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.2.1.tar.gz) 25 | FetchContent_MakeAvailable(tessil) 26 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.24) 2 | project(kiss_matcher VERSION 0.3.0 LANGUAGES CXX) 3 | 4 | message("") 5 | message("██╗ ██╗██╗███████╗███████╗ ") 6 | message("██║ ██╔╝██║██╔════╝██╔════╝ ") 7 | message("█████╔╝ ██║███████╗███████╗█████╗") 8 | message("██╔═██╗ ██║╚════██║╚════██║╚════╝") 9 | message("██║ ██╗██║███████║███████║ ") 10 | message("╚═╝ ╚═╝╚═╝╚══════╝╚══════╝ ") 11 | message("") 12 | message("███╗ ███╗ █████╗ ████████╗ ██████╗██╗ ██╗███████╗██████╗ ") 13 | message("████╗ ████║██╔══██╗╚══██╔══╝██╔════╝██║ ██║██╔════╝██╔══██╗") 14 | message("██╔████╔██║███████║ ██║ ██║ ███████║█████╗ ██████╔╝") 15 | message("██║╚██╔╝██║██╔══██║ ██║ ██║ ██╔══██║██╔══╝ ██╔══██╗") 16 | message("██║ ╚═╝ ██║██║ ██║ ██║ ╚██████╗██║ ██║███████╗██║ ██║") 17 | message("╚═╝ ╚═╝╚═╝ ╚═╝ ╚═╝ ╚═════╝╚═╝ ╚═╝╚══════╝╚═╝ ╚═╝") 18 | message("") 19 | 20 | option(USE_CCACHE "Build using Ccache if found on the path" ON) 21 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 22 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" OFF) 23 | option(USE_SYSTEM_ROBIN "Use system pre-installed ROBIN from SPARK @ MIT" ON) 24 | option(USE_ADDRESS_SANITIZER "Use address sanitizer" OFF) 25 | 26 | # Set a default build type if none is specified 27 | if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 28 | message(STATUS "Setting build type to 'Release' as none was specified.") 29 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build." FORCE) 30 | # Provide options for cmake-gui or ccmake 31 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 32 | endif() 33 | set(CMAKE_CXX_STANDARD 17) 34 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 35 | set(CMAKE_CXX_EXTENSIONS OFF) 36 | 37 | if (USE_ADDRESS_SANITIZER) 38 | add_compile_options(-fsanitize=address -fsanitize-recover=address) 39 | add_link_options(-fsanitize=address -fsanitize-recover=address) 40 | endif() 41 | 42 | include(GNUInstallDirs) 43 | include(3rdparty/find_dependencies.cmake) 44 | include(cmake/CompilerOptions.cmake) 45 | 46 | # NOTE(hlim): Without this line, pybinded KISS-Matcher does not work 47 | find_library(LZ4_LIBRARY lz4 REQUIRED) 48 | 49 | include(FindOpenMP) #The best way to set proper compiler settings for using OpenMP in all platforms 50 | if (OPENMP_FOUND) #The best way to set proper compiler settings for using OpenMP in all platforms 51 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 52 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 53 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 54 | else (OPENMP_FOUND) 55 | message("ERROR: OpenMP could not be found.") 56 | endif (OPENMP_FOUND) 57 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall ${CMAKE_CXX_FLAGS}") 58 | 59 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/core) 60 | 61 | set(TARGET_NAME kiss_matcher_core) 62 | # NOTE(hlim): Without `STATIC`, it leads to ImportError in Pybinding as follows: 63 | # "libkiss_matcher_core.so: cannot open shared object file: NO such file or directory ..." 64 | # See Issue #37 (https://github.com/MIT-SPARK/KISS-Matcher/issues/37) 65 | add_library(${TARGET_NAME} STATIC 66 | core/kiss_matcher/ROBINMatching.cpp 67 | core/kiss_matcher/FasterPFH.cpp 68 | core/kiss_matcher/KISSMatcher.cpp 69 | core/kiss_matcher/GncSolver.cpp 70 | ) 71 | add_library(kiss_matcher::kiss_matcher_core ALIAS kiss_matcher_core) 72 | 73 | target_link_libraries(${TARGET_NAME} 74 | PUBLIC Eigen3::Eigen robin::robin ${OpenMP_LIBS} ${EIGEN3_LIBS} TBB::tbb ${LZ4_LIBRARY} 75 | ) 76 | 77 | # To make kiss_matcher::core global for Pybinding 78 | set_global_target_properties(${TARGET_NAME}) 79 | 80 | target_include_directories(${TARGET_NAME} 81 | PRIVATE 82 | ${CMAKE_CURRENT_SOURCE_DIR}/core/ 83 | PUBLIC 84 | $ 85 | $ 86 | $ 87 | $) 88 | 89 | install(TARGETS ${TARGET_NAME} 90 | EXPORT ${PROJECT_NAME}-export 91 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 92 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 93 | ) 94 | 95 | install(EXPORT ${PROJECT_NAME}-export 96 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} 97 | NAMESPACE ${PROJECT_NAME}:: 98 | FILE ${PROJECT_NAME}Targets.cmake 99 | ) 100 | 101 | install(DIRECTORY core/kiss_matcher DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 102 | 103 | export(TARGETS ${MYPROJECT_EXPORTED_TARGETS} FILE ${PROJECT_NAME}-exports.cmake) 104 | 105 | install(FILES cmake/${PROJECT_NAME}Config.cmake 106 | DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} 107 | ) 108 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/KISS-Matcher/908fa6c2c8c63ec6f348d2bccf68268277d5f40f/cpp/kiss_matcher/COLCON_IGNORE -------------------------------------------------------------------------------- /cpp/kiss_matcher/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/KISS-Matcher/908fa6c2c8c63ec6f348d2bccf68268277d5f40f/cpp/kiss_matcher/README.md -------------------------------------------------------------------------------- /cpp/kiss_matcher/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 | $<$:-Wno-error> 34 | $<$:-Wall> 35 | $<$:-Wextra> 36 | $<$:-Wconversion> 37 | $<$:-Wno-sign-conversion> 38 | $<$:-Wno-float-conversion> 39 | # GNU 40 | $<$:-fdiagnostics-color=always> 41 | $<$:-Wno-error> 42 | $<$:-Wall> 43 | $<$:-Wextra> 44 | $<$:-pedantic> 45 | $<$:-Wcast-align> 46 | $<$:-Wcast-qual> 47 | $<$:-Wconversion> 48 | $<$:-Wdisabled-optimization> 49 | $<$:-Woverloaded-virtual> 50 | $<$:-Wno-float-conversion>) 51 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 52 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 53 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 54 | PUBLIC $ $) 55 | endfunction() 56 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/cmake/kiss_matcherConfig.cmake: -------------------------------------------------------------------------------- 1 | get_filename_component(MYPROJECT_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 2 | include(CMakeFindDependencyMacro) 3 | 4 | find_dependency(Eigen3 3.3 REQUIRED) 5 | find_dependency(OpenMP REQUIRED) 6 | 7 | # The parameter must adhere to the format: ${PROJECT_NAME}Targets.cmake 8 | include("${MYPROJECT_CMAKE_DIR}/kiss_matcherTargets.cmake") 9 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/FasterPFH.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2024, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Hyungtae Lim, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | */ 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "kiss_matcher/kdtree/kdtree_tbb.hpp" 27 | #include "kiss_matcher/points/point_cloud.hpp" 28 | 29 | using MyKdTree = kiss_matcher::UnsafeKdTree; 30 | 31 | // was #define NOT_ASSIGNED -1 but results in a sign comparison warning 32 | // as it is compared against the indices which are uint32_t or size_t 33 | // This quiets the warning but is probably not what is intended. 34 | // NEEDS thought! 35 | #define NOT_ASSIGNED std::numeric_limits::max() 36 | 37 | #define UNASSIGNED_NORMAL \ 38 | Eigen::Vector3f(std::numeric_limits::quiet_NaN(), \ 39 | std::numeric_limits::quiet_NaN(), \ 40 | std::numeric_limits::quiet_NaN()) 41 | 42 | struct pair_hash { 43 | std::size_t operator()(const std::pair& pair) const { 44 | return ((1 << 20) - 1) & (pair.first * 73856093 ^ pair.second * 19349669); 45 | } 46 | }; 47 | 48 | namespace kiss_matcher { 49 | 50 | struct FasterPFH { 51 | using Vector3fVector = std::vector; 52 | using Vector3fVectorTuple = std::tuple; 53 | using Voxel = Eigen::Vector3i; 54 | struct Correspondences { 55 | std::vector neighboring_indices; 56 | std::vector neighboring_dists; 57 | bool is_planar = false; 58 | inline void clear() { 59 | if (!neighboring_indices.empty()) neighboring_indices.clear(); 60 | if (!neighboring_dists.empty()) neighboring_dists.clear(); 61 | } 62 | }; 63 | 64 | FasterPFH() {} 65 | 66 | explicit FasterPFH(const float normal_radius, 67 | const float fpfh_radius, 68 | const float thr_linearity, 69 | const std::string criteria = "L2", 70 | const bool use_non_maxima_suppression = false) 71 | : normal_radius_(normal_radius), 72 | fpfh_radius_(fpfh_radius), 73 | thr_linearity_(thr_linearity), 74 | criteria_(criteria), 75 | use_non_maxima_suppression_(use_non_maxima_suppression) { 76 | sqr_fpfh_radius_ = fpfh_radius * fpfh_radius; 77 | } 78 | 79 | inline void Clear() { 80 | corrs_fpfh_.clear(); 81 | voxel_indices_.clear(); 82 | spfh_indices_.clear(); 83 | fpfh_indices_.clear(); 84 | spfh_hist_lookup_.clear(); // why int? To employee `NOT_ASSIGNED` 85 | 86 | spfh_pairs_.clear(); 87 | spfh_hash_table_.clear(); 88 | 89 | num_points_per_voxel_.clear(); 90 | num_valid_voxels_.clear(); 91 | 92 | hist_f1_.clear(); 93 | hist_f2_.clear(); 94 | hist_f3_.clear(); 95 | } 96 | 97 | void setInputCloud(const std::vector& points); 98 | 99 | // void SetNormalsForValidPoints(); 100 | 101 | // void SetFPFHIndices(); 102 | 103 | std::tuple EstimateNormalVectorWithLinearityFiltering( 104 | const Correspondences& corr_fpfh, 105 | const float normal_radius, 106 | const float thr_linearity); 107 | bool IsNormalValid(const Eigen::Vector3f& normal); 108 | 109 | // For FPFH 110 | // void Compute(const std::vector &points, 111 | // std::vector &valid_points, 112 | // std::vector &descriptors); 113 | 114 | void ComputeFeature(std::vector& points, 115 | std::vector& descriptors); 116 | 117 | void ComputeSPFHSignatures(const tsl::robin_map& spfh_hist_lookup, 118 | std::vector& hist_f1, 119 | std::vector& hist_f2, 120 | std::vector& hist_f3); 121 | 122 | void ComputePointSPFHSignature(const uint32_t p_idx, 123 | Eigen::VectorXf& hist_f1, 124 | Eigen::VectorXf& hist_f2, 125 | Eigen::VectorXf& hist_f3); 126 | 127 | bool ComputePairFeatures(const Eigen::Vector3f& p1, 128 | const Eigen::Vector3f& n1, 129 | const Eigen::Vector3f& p2, 130 | const Eigen::Vector3f& n2, 131 | float& f1, 132 | float& f2, 133 | float& f3, 134 | float& f4); 135 | 136 | void FilterIndicesCausingNaN(std::vector& spfh_indices); 137 | 138 | void WeightPointSPFHSignature(const std::vector& hist_f1, 139 | const std::vector& hist_f2, 140 | const std::vector& hist_f3, 141 | const std::vector& indices, 142 | const std::vector& dists, 143 | Eigen::VectorXf& fpfh_histogram); 144 | 145 | // For initialization 146 | float normal_radius_; 147 | float fpfh_radius_; 148 | float thr_linearity_; 149 | std::string criteria_; // "L1" or "L2" 150 | bool use_non_maxima_suppression_ = false; 151 | 152 | float sqr_fpfh_radius_; 153 | int num_points_; 154 | int minimum_num_valid_ = 3; // heuristics 155 | 156 | std::vector num_points_per_voxel_; 157 | std::vector num_valid_voxels_; 158 | 159 | std::vector indices_incremental_for_normal_; 160 | std::vector indices_incremental_for_fpfh_; 161 | 162 | // The sizes of below member variables are same with `num_points_` 163 | std::vector points_; 164 | std::vector normals_; 165 | std::vector corrs_fpfh_; 166 | std::vector voxel_indices_; 167 | std::vector is_valid_; 168 | std::vector is_visited_; 169 | 170 | std::vector spfh_indices_; // voxels whose normals are valid 171 | // tsl::robin_set redundant_indices_; 172 | std::vector fpfh_indices_; // Originally, spfh_indices_ \ redundant_indices_ 173 | 174 | tsl::robin_set spfh_pairs_; 175 | tsl::robin_map spfh_hist_lookup_; // why int? To employee `NOT_ASSIGNED` 176 | // From 177 | // https://github.com/PointCloudLibrary/pcl/blob/master/features/include/pcl/features/fpfh.h#L188 178 | /** \brief The number of subdivisions for each angular feature interval. */ 179 | 180 | int nr_bins_f1_{11}, nr_bins_f2_{11}, nr_bins_f3_{11}; 181 | 182 | tsl::robin_map, Eigen::Vector4f, pair_hash> spfh_hash_table_; 183 | 184 | /** \brief Placeholder for the f1 histogram. */ 185 | std::vector hist_f1_; 186 | 187 | /** \brief Placeholder for the f2 histogram. */ 188 | std::vector hist_f2_; 189 | 190 | /** \brief Placeholder for the f3 histogram. */ 191 | std::vector hist_f3_; 192 | 193 | /** \brief Placeholder for a point's FPFH signature. */ 194 | Eigen::VectorXf fpfh_histogram_; 195 | 196 | /** \brief Float constant = 1.0 / (2.0 * M_PI) */ 197 | float d_pi_ = 1.0f / (2.0f * static_cast(M_PI)); 198 | }; 199 | } // namespace kiss_matcher 200 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/ROBINMatching.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2024, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Hyungtae Lim, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | */ 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #define USE_UNORDERED_MAP 1 26 | 27 | namespace kiss_matcher { 28 | 29 | class ROBINMatching { 30 | public: 31 | typedef std::vector Feature; 32 | typedef flann::Index> KDTree; 33 | 34 | ROBINMatching() {} 35 | 36 | ROBINMatching(const float noise_bound, 37 | const int num_max_corr = 5000, 38 | const float tuple_scale = 0.95); 39 | 40 | // Warning: Do not use `use_ratio_test` in the scan-level registration, 41 | // because setting `use_ratio_test` to `true` sometimes reduces the number of correspondences 42 | std::vector> establishCorrespondences( 43 | 44 | std::vector& source_points, 45 | std::vector& target_points, 46 | Feature& source_features, 47 | Feature& target_features, 48 | std::string robin_mode, 49 | float tuple_scale = 0.95, 50 | bool use_ratio_test = false); 51 | 52 | // For a deeper understanding, please refer to Section III.D 53 | // ttps://arxiv.org/pdf/2409.15615 54 | std::vector applyOutlierPruning(const std::vector& src_matched, 55 | const std::vector& tgt_matched, 56 | const std::string& robin_mode = "max_core"); 57 | 58 | inline std::vector> getCrossCheckedCorrespondences() { 59 | std::vector> corres_out; 60 | corres_out.reserve(corres_cross_checked_.size()); 61 | for (const auto& corres_tuple : corres_cross_checked_) { 62 | if (swapped_) { 63 | corres_out.emplace_back(std::pair(corres_tuple.second, corres_tuple.first)); 64 | } else { 65 | corres_out.emplace_back(std::pair(corres_tuple.first, corres_tuple.second)); 66 | } 67 | } 68 | return corres_out; 69 | } 70 | 71 | inline std::vector> getCrosscheckedCorrespondences() { 72 | return corres_cross_checked_; 73 | } 74 | 75 | inline std::vector> getFinalCorrespondences() { return corres_; } 76 | 77 | double getRejectionTime() { return rejection_time_; } 78 | 79 | size_t getNumInitialCorrespondences() { return num_init_corr_; } 80 | 81 | size_t getNumPrunedCorrespondences() { return num_pruned_corr_; } 82 | 83 | private: 84 | size_t fi_ = 0; // source idx 85 | size_t fj_ = 1; // destination idx 86 | 87 | ssize_t nPti_; 88 | ssize_t nPtj_; 89 | 90 | bool swapped_ = false; 91 | 92 | template 93 | void buildKDTree(const std::vector& data, KDTree* tree); 94 | 95 | template 96 | void searchKDTree(const KDTree& tree, 97 | const T& input, 98 | std::array& indices, 99 | std::array& dists, 100 | int nn); 101 | 102 | template 103 | void searchKDTreeAll(KDTree* tree, 104 | const std::vector& inputs, 105 | std::vector& indices, 106 | std::vector& dists, 107 | int nn); 108 | 109 | // NOTE(hlim): Without `isValidIndex, sometimes segmentation fault occurs. 110 | // For this reason, we over-add isValidIndex for the safety purpose 111 | bool isValidIndex(const size_t index, const size_t vector_size) { return index < vector_size; } 112 | 113 | void match(const std::string& robin_mode, float tuple_scale, bool use_ratio_test = false); 114 | 115 | void setStatuses(); 116 | 117 | void runTupleTest(const std::vector>& corres, 118 | std::vector>& corres_out, 119 | const float tuple_scale); 120 | 121 | // For a deeper understanding, please refer to Section III.D 122 | // ttps://arxiv.org/pdf/2409.15615 123 | void applyOutlierPruning(const std::vector>& corres, 124 | std::vector>& corres_out, 125 | const std::string& robin_mode = "max_core"); 126 | 127 | std::vector> corres_cross_checked_; 128 | std::vector> corres_; 129 | std::vector> pointcloud_; 130 | std::vector features_; 131 | std::vector> 132 | means_; // for normalization 133 | 134 | size_t num_max_corr_; 135 | size_t num_init_corr_ = 0; 136 | size_t num_pruned_corr_ = 0; 137 | 138 | float noise_bound_; 139 | 140 | float tuple_test_ratio_ = 0.95; 141 | 142 | float thr_dist_ = 30; // Empirically, potentially imprecise matching is rejected 143 | float thr_ratio_test_ = 0.9; // The lower, the more strict 144 | float sqr_thr_dist_ = thr_dist_ * thr_dist_; 145 | 146 | double rejection_time_; 147 | }; 148 | 149 | } // namespace kiss_matcher 150 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/kdtree/kdtree.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kiss_matcher { 13 | 14 | /// @brief Unsafe KdTree with arbitrary nanoflann Adaptor. 15 | /// @note This class does not hold the ownership of the input points. 16 | /// You must keep the input points along with this class. 17 | template class Adaptor> 18 | class UnsafeKdTreeGeneric { 19 | public: 20 | using Ptr = std::shared_ptr; 21 | using ConstPtr = std::shared_ptr; 22 | using ThisClass = UnsafeKdTreeGeneric; 23 | using Index = 24 | Adaptor, ThisClass, 3, uint32_t>; 25 | 26 | /// @brief Constructor 27 | /// @param points Input points 28 | explicit UnsafeKdTreeGeneric(const PointCloud &points) 29 | : points(points), index(3, *this, kiss_matcher::KDTreeSingleIndexAdaptorParams(10)) { 30 | index.buildIndex(); 31 | } 32 | 33 | /// @brief Constructor 34 | /// @param points Input points 35 | /// @params num_threads Number of threads used for building the index (This argument is only 36 | /// valid for OMP implementation) 37 | explicit UnsafeKdTreeGeneric(const PointCloud &points, int num_threads) 38 | : points(points), index(3, *this, kiss_matcher::KDTreeSingleIndexAdaptorParams(10)) { 39 | index.buildIndex(num_threads); 40 | } 41 | 42 | ~UnsafeKdTreeGeneric() {} 43 | 44 | // Interfaces for nanoflann 45 | size_t kdtree_get_point_count() const { return traits::size(points); } 46 | double kdtree_get_pt(const size_t idx, const size_t dim) const { 47 | return traits::point(points, idx)[dim]; 48 | } 49 | 50 | template 51 | bool kdtree_get_bbox(BBox &) const { 52 | return false; 53 | } 54 | 55 | /// @brief Find k-nearest neighbors 56 | size_t knn_search(const Eigen::Vector4d &pt, 57 | size_t k, 58 | size_t *k_indices, 59 | double *k_sq_dists) const { 60 | return index.knnSearch(pt.data(), k, k_indices, k_sq_dists); 61 | } 62 | 63 | /// @brief Find neighbors within the radius 64 | size_t radius_search(const Eigen::Vector4d &pt, 65 | double r, 66 | std::vector> &indices_sq_dists) const { 67 | kiss_matcher::SearchParams params; 68 | params.sorted = false; // to boost the speed 69 | return index.radiusSearch(pt.data(), r, indices_sq_dists, params); 70 | } 71 | 72 | private: 73 | const PointCloud &points; ///< Input points 74 | Index index; ///< KdTree index 75 | }; 76 | 77 | /// @brief KdTree with arbitrary nanoflann Adaptor 78 | template class Adaptor> 79 | class KdTreeGeneric { 80 | public: 81 | using Ptr = std::shared_ptr; 82 | using ConstPtr = std::shared_ptr; 83 | 84 | /// @brief Constructor 85 | /// @param points Input points 86 | explicit KdTreeGeneric(const std::shared_ptr &points) 87 | : points(points), tree(*points) {} 88 | 89 | /// @brief Constructor 90 | /// @param points Input points 91 | explicit KdTreeGeneric(const std::shared_ptr &points, int num_threads) 92 | : points(points), tree(*points, num_threads) {} 93 | 94 | ~KdTreeGeneric() {} 95 | 96 | /// @brief Find k-nearest neighbors 97 | size_t knn_search(const Eigen::Vector4d &pt, 98 | size_t k, 99 | size_t *k_indices, 100 | double *k_sq_dists) const { 101 | return tree.knn_search(pt, k, k_indices, k_sq_dists); 102 | } 103 | 104 | /// @brief Find neighbors within the radius 105 | size_t radius_search(const Eigen::Vector4d &pt, 106 | double r, 107 | std::vector> &indices_sq_dists) const { 108 | kiss_matcher::SearchParams params; 109 | params.sorted = false; // to boost the speed 110 | return tree.radius_search(pt.data(), r, indices_sq_dists, params); 111 | } 112 | 113 | private: 114 | const std::shared_ptr points; ///< Input points 115 | const UnsafeKdTreeGeneric tree; ///< KdTree 116 | }; 117 | 118 | /// @brief Standard KdTree (unsafe) 119 | template 120 | using UnsafeKdTree = UnsafeKdTreeGeneric; 121 | 122 | /// @brief Standard KdTree 123 | template 124 | using KdTree = KdTreeGeneric; 125 | 126 | namespace traits { 127 | 128 | template class Adaptor> 129 | struct Traits> { 130 | static size_t knn_search(const UnsafeKdTreeGeneric &tree, 131 | const Eigen::Vector4d &point, 132 | size_t k, 133 | size_t *k_indices, 134 | double *k_sq_dists) { 135 | return tree.knn_search(point, k, k_indices, k_sq_dists); 136 | } 137 | 138 | static size_t radius_search(const UnsafeKdTreeGeneric &tree, 139 | const Eigen::Vector4d &point, 140 | double r, 141 | std::vector> &indices_sq_dists) { 142 | kiss_matcher::SearchParams params; 143 | params.sorted = false; // to boost the speed 144 | return tree.radius_search(point, r, indices_sq_dists); 145 | } 146 | }; 147 | 148 | template class Adaptor> 149 | struct Traits> { 150 | static size_t knn_search(const KdTreeGeneric &tree, 151 | const Eigen::Vector4d &point, 152 | size_t k, 153 | size_t *k_indices, 154 | double *k_sq_dists) { 155 | return tree.knn_search(point, k, k_indices, k_sq_dists); 156 | } 157 | 158 | static size_t radius_search(const KdTreeGeneric &tree, 159 | const Eigen::Vector4d &point, 160 | double r, 161 | std::vector> &indices_sq_dists) { 162 | kiss_matcher::SearchParams params; 163 | params.sorted = false; // to boost the speed 164 | return tree.radius_search(point, r, indices_sq_dists); 165 | } 166 | }; 167 | 168 | } // namespace traits 169 | 170 | } // namespace kiss_matcher 171 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/kdtree/kdtree_tbb.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace kiss_matcher { 11 | 12 | /// @brief Unsafe KdTree with multi-thread tree construction with TBB backend. 13 | /// @note This class only parallelizes the tree construction. The search is still single-threaded 14 | /// as in the normal KdTree. 15 | template 16 | using UnsafeKdTreeTBB = UnsafeKdTreeGeneric; 17 | 18 | /// @brief KdTree with multi-thread tree construction with TBB backend. 19 | /// @note This class only parallelizes the tree construction. The search is still single-threaded 20 | /// as in the normal KdTree. 21 | template 22 | using KdTreeTBB = KdTreeGeneric; 23 | 24 | } // namespace kiss_matcher 25 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/kdtree/traits.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace kiss_matcher { 10 | 11 | namespace traits { 12 | 13 | template 14 | struct Traits; 15 | 16 | /// @brief Find k-nearest neighbors. 17 | /// @param tree Nearest neighbor search (e.g., KdTree) 18 | /// @param point Query point 19 | /// @param k Number of neighbors 20 | /// @param k_index [out] Index of the nearest neighbor 21 | /// @param k_sq_dist [out] Squared distance to the nearest neighbor 22 | /// @return Number of found neighbors 23 | template 24 | size_t knn_search(const T& tree, 25 | const Eigen::Vector4d& point, 26 | size_t k, 27 | size_t* k_indices, 28 | double* k_sq_dists) { 29 | return Traits::knn_search(tree, point, k, k_indices, k_sq_dists); 30 | } 31 | 32 | template 33 | struct has_nearest_neighbor_search { 34 | template ::nearest_neighbor_search, 0)> 35 | static std::true_type test(U*); 36 | static std::false_type test(...); 37 | 38 | static constexpr bool value = decltype(test((T*)nullptr))::value; 39 | }; 40 | 41 | /// @brief Find the nearest neighbor. 42 | /// @param tree Nearest neighbor search (e.g., KdTree) 43 | /// @param point Query point 44 | /// @param k_index [out] Index of the nearest neighbor 45 | /// @param k_sq_dist [out] Squared distance to the nearest neighbor 46 | /// @return 1 if a neighbor is found else 0 47 | template ::value, bool> = true> 48 | size_t nearest_neighbor_search(const T& tree, 49 | const Eigen::Vector4d& point, 50 | size_t* k_index, 51 | double* k_sq_dist) { 52 | return Traits::nearest_neighbor_search(tree, point, k_index, k_sq_dist); 53 | } 54 | 55 | /// @brief Find the nearest neighbor. If Traits::nearest_neighbor_search is not defined, fallback 56 | /// to knn_search with k=1. 57 | /// @param tree Nearest neighbor search (e.g., KdTree) 58 | /// @param point Query point 59 | /// @param k_index [out] Index of the nearest neighbor 60 | /// @param k_sq_dist [out] Squared distance to the nearest neighbor 61 | /// @return 1 if a neighbor is found else 0 62 | template ::value, bool> = true> 63 | size_t nearest_neighbor_search(const T& tree, 64 | const Eigen::Vector4d& point, 65 | size_t* k_index, 66 | double* k_sq_dist) { 67 | return Traits::knn_search(tree, point, 1, k_index, k_sq_dist); 68 | } 69 | 70 | } // namespace traits 71 | 72 | } // namespace kiss_matcher 73 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/points/eigen.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace kiss_matcher { 9 | namespace traits { 10 | 11 | template <> 12 | struct Traits { 13 | static size_t size(const Eigen::MatrixXd& points) { return points.rows(); } 14 | static bool has_points(const Eigen::MatrixXd& points) { return points.rows(); } 15 | static Eigen::Vector4d point(const Eigen::MatrixXd& points, size_t i) { 16 | return Eigen::Vector4d(points(i, 0), points(i, 1), points(i, 2), 1.0); 17 | } 18 | }; 19 | 20 | } // namespace traits 21 | } // namespace kiss_matcher 22 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/points/fast_floor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace kiss_matcher { 6 | 7 | /// @brief Fast floor (https://stackoverflow.com/questions/824118/why-is-floor-so-slow). 8 | /// @param pt Double vector 9 | /// @return Floored int vector 10 | inline Eigen::Array4i fast_floor(const Eigen::Array4d& pt) { 11 | const Eigen::Array4i ncoord = pt.cast(); 12 | return ncoord - (pt < ncoord.cast()).cast(); 13 | } 14 | 15 | inline Eigen::Array3i fast_floor_array3f(const Eigen::Array3f& pt) { 16 | Eigen::Array3i ncoord = pt.cast(); 17 | return ncoord - (pt < ncoord.cast()).cast(); 18 | } 19 | 20 | inline Eigen::Array3i fast_floor_vector3f(const Eigen::Vector3f& pt) { 21 | return fast_floor_array3f(pt.array()); 22 | } 23 | 24 | } // namespace kiss_matcher 25 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/points/point_cloud.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "kiss_matcher/points/traits.hpp" 11 | 12 | namespace kiss_matcher { 13 | 14 | /** 15 | * @brief Point cloud 16 | */ 17 | struct PointCloud { 18 | public: 19 | using Ptr = std::shared_ptr; 20 | using ConstPtr = std::shared_ptr; 21 | 22 | /// @brief Constructor 23 | PointCloud() {} 24 | 25 | /// @brief Constructor 26 | /// @param points Points to initialize the point cloud 27 | template 28 | explicit PointCloud(const std::vector, Allocator>& points) { 29 | this->resize(points.size()); 30 | for (size_t i = 0; i < points.size(); i++) { 31 | this->point(i) << points[i].template cast().template head<3>(), 1.0; 32 | } 33 | } 34 | 35 | /// @brief Destructor 36 | ~PointCloud() {} 37 | 38 | /// @brief Number of points. 39 | size_t size() const { return points.size(); } 40 | 41 | /// @brief Check if the point cloud is empty. 42 | bool empty() const { return points.empty(); } 43 | 44 | /// @brief Resize point/normal/cov buffers. 45 | /// @param n Number of points 46 | void resize(size_t n) { 47 | points.resize(n); 48 | normals.resize(n); 49 | covs.resize(n); 50 | } 51 | 52 | /// @brief Get i-th point. 53 | Eigen::Vector4d& point(size_t i) { return points[i]; } 54 | 55 | /// @brief Get i-th normal. 56 | Eigen::Vector4d& normal(size_t i) { return normals[i]; } 57 | 58 | /// @brief Get i-th covariance. 59 | Eigen::Matrix4d& cov(size_t i) { return covs[i]; } 60 | 61 | /// @brief Get i-th point (const). 62 | const Eigen::Vector4d& point(size_t i) const { return points[i]; } 63 | 64 | /// @brief Get i-th normal (const). 65 | const Eigen::Vector4d& normal(size_t i) const { return normals[i]; } 66 | 67 | /// @brief Get i-th covariance (const). 68 | const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; } 69 | 70 | public: 71 | std::vector points; ///< Point coordinates (x, y, z, 1) 72 | std::vector normals; ///< Point normals (nx, ny, nz, 0) 73 | std::vector covs; ///< Point covariances (3x3 matrix) + zero padding 74 | }; 75 | 76 | namespace traits { 77 | 78 | template <> 79 | struct Traits { 80 | using Points = PointCloud; 81 | 82 | static size_t size(const Points& points) { return points.size(); } 83 | 84 | static bool has_points(const Points& points) { return !points.points.empty(); } 85 | static bool has_normals(const Points& points) { return !points.normals.empty(); } 86 | static bool has_covs(const Points& points) { return !points.covs.empty(); } 87 | 88 | static const Eigen::Vector4d& point(const Points& points, size_t i) { return points.point(i); } 89 | static const Eigen::Vector4d& normal(const Points& points, size_t i) { return points.normal(i); } 90 | static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.cov(i); } 91 | 92 | static void resize(Points& points, size_t n) { points.resize(n); } 93 | static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { 94 | points.point(i) = pt; 95 | } 96 | static void set_normal(Points& points, size_t i, const Eigen::Vector4d& n) { 97 | points.normal(i) = n; 98 | } 99 | static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.cov(i) = cov; } 100 | }; 101 | 102 | } // namespace traits 103 | 104 | } // namespace kiss_matcher 105 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/points/traits.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: Copyright 2024 Kenji Koide 2 | // SPDX-License-Identifier: MIT 3 | #pragma once 4 | 5 | #include 6 | 7 | namespace kiss_matcher { 8 | 9 | namespace traits { 10 | 11 | template 12 | struct Traits {}; 13 | 14 | /// @brief Get the number of points. 15 | template 16 | size_t size(const T& points) { 17 | return Traits::size(points); 18 | } 19 | 20 | /// @brief Check if the point cloud has points. 21 | template 22 | bool has_points(const T& points) { 23 | return Traits::has_points(points); 24 | } 25 | 26 | /// @brief Check if the point cloud has normals. 27 | template 28 | bool has_normals(const T& points) { 29 | return Traits::has_normals(points); 30 | } 31 | 32 | /// @brief Check if the point cloud has covariances. 33 | template 34 | bool has_covs(const T& points) { 35 | return Traits::has_covs(points); 36 | } 37 | 38 | /// @brief Get i-th point. 4D vector is used to take advantage of SIMD intrinsics. The last element 39 | /// must be filled by one (x, y, z, 1). 40 | template 41 | auto point(const T& points, size_t i) { 42 | return Traits::point(points, i); 43 | } 44 | 45 | /// @brief Get i-th normal. 4D vector is used to take advantage of SIMD intrinsics. The last element 46 | /// must be filled by zero (nx, ny, nz, 0). 47 | template 48 | auto normal(const T& points, size_t i) { 49 | return Traits::normal(points, i); 50 | } 51 | 52 | /// @brief Get i-th covariance. Only the top-left 3x3 matrix is filled, and the bottom row and the 53 | /// right col must be filled by zero. 54 | template 55 | auto cov(const T& points, size_t i) { 56 | return Traits::cov(points, i); 57 | } 58 | 59 | /// @brief Resize the point cloud (this function should resize all attributes) 60 | template 61 | void resize(T& points, size_t n) { 62 | Traits::resize(points, n); 63 | } 64 | 65 | /// @brief Set i-th point. (x, y, z, 1) 66 | template 67 | void set_point(T& points, size_t i, const Eigen::Vector4d& pt) { 68 | Traits::set_point(points, i, pt); 69 | } 70 | 71 | /// @brief Set i-th normal. (nx, nz, nz, 0) 72 | template 73 | void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) { 74 | Traits::set_normal(points, i, pt); 75 | } 76 | 77 | /// @brief Set i-th covariance. Only the top-left 3x3 matrix should be filled. 78 | template 79 | void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) { 80 | Traits::set_cov(points, i, cov); 81 | } 82 | 83 | } // namespace traits 84 | } // namespace kiss_matcher 85 | -------------------------------------------------------------------------------- /cpp/kiss_matcher/core/kiss_matcher/points/vector3i_hash.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace kiss_matcher { 6 | 7 | /** 8 | * @brief Spatial hashing function. 9 | * Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable 10 | * Objects", VMV2003. 11 | */ 12 | struct XORVector3iHash { 13 | public: 14 | size_t operator()(const Eigen::Vector3i& x) const { 15 | const size_t p1 = 73856093; 16 | const size_t p2 = 19349669; // 19349663 was not a prime number 17 | const size_t p3 = 83492791; 18 | return static_cast((x[0] * p1) ^ (x[1] * p2) ^ (x[2] * p3)); 19 | } 20 | 21 | static size_t hash(const Eigen::Vector3i& x) { return XORVector3iHash()(x); } 22 | static bool equal(const Eigen::Vector3i& x1, const Eigen::Vector3i& x2) { return x1 == x2; } 23 | }; 24 | 25 | } // namespace kiss_matcher 26 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16...3.26) 2 | 3 | project(kiss_matcher_pybind) 4 | 5 | # Set build type 6 | set(CMAKE_BUILD_TYPE Release) 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 9 | 10 | message(STATUS "Python Interpreter Version: ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") 11 | 12 | include(FetchContent) 13 | FetchContent_Declare( 14 | pybind11 15 | GIT_REPOSITORY https://github.com/pybind/pybind11 16 | GIT_TAG master 17 | ) 18 | FetchContent_MakeAvailable(pybind11) 19 | 20 | set(PYBIND11_NEWPYTHON ON) 21 | find_package(Python COMPONENTS Interpreter Development.Module REQUIRED) 22 | # find_package(pybind11 CONFIG REQUIRED) 23 | 24 | if (DEFINED SKBUILD) 25 | message(STATUS "Building with Scikit-Build") 26 | endif () 27 | 28 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_matcher/) 29 | message(STATUS "Install KISS-Matcher using local C++ files...") 30 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_matcher ${CMAKE_CURRENT_BINARY_DIR}/kiss_matcher) 31 | else() 32 | cmake_minimum_required(VERSION 3.18) 33 | message(STATUS "Performing out-of-tree build, fetching KISS-Matcher v${CMAKE_PROJECT_VERSION} Release from Github") 34 | include(FetchContent) 35 | FetchContent_Declare( 36 | ext_kiss_matcher_core PREFIX kiss_matcher 37 | GIT_REPOSITORY https://github.com/MIT-SPARK/KISS-Matcher 38 | GIT_TAG main 39 | SOURCE_SUBDIR cpp/kiss_matcher) 40 | FetchContent_MakeAvailable(ext_kiss_matcher_core) 41 | endif() 42 | 43 | set(OUTPUT_MODULE_NAME kiss_matcher) 44 | 45 | pybind11_add_module(${OUTPUT_MODULE_NAME} kiss_matcher/pybind/kiss_matcher_pybind.cpp) 46 | target_link_libraries(${OUTPUT_MODULE_NAME} PUBLIC kiss_matcher_core) 47 | 48 | # To ignore warning messages 49 | target_compile_options(${OUTPUT_MODULE_NAME} PRIVATE 50 | $<$:-w> 51 | $<$:-w> 52 | $<$:/w> 53 | ) 54 | 55 | # fix for clang 56 | # see: https://github.com/pybind/pybind11/issues/1818 57 | if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 58 | target_compile_options(${OUTPUT_MODULE_NAME} PUBLIC -fsized-deallocation) 59 | endif () 60 | 61 | install(TARGETS ${OUTPUT_MODULE_NAME} DESTINATION .) 62 | 63 | 64 | 65 | # add_subdirectory(kiss_matcher/pybind) 66 | -------------------------------------------------------------------------------- /python/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/KISS-Matcher/908fa6c2c8c63ec6f348d2bccf68268277d5f40f/python/COLCON_IGNORE -------------------------------------------------------------------------------- /python/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Hyungtae Lim, Daebeom Kim, Gunhee Shin, Jingnan Shi, 4 | Ignacio Vizzo, Hyun Myung, Jaesik Park, and Luca Carlone. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /python/examples/run_kiss_matcher.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | 3 | import kiss_matcher 4 | import numpy as np 5 | import open3d as o3d # Only for the load of pcd and visualization 6 | 7 | 8 | # KITTI type bin file 9 | def read_bin(bin_path): 10 | scan = np.fromfile(bin_path, dtype=np.float32) 11 | scan = scan.reshape((-1, 4)) 12 | return scan[:, :3] 13 | 14 | 15 | def read_pcd(pcd_path): 16 | pcd = o3d.io.read_point_cloud(pcd_path) 17 | return np.asarray(pcd.points) 18 | 19 | 20 | def remove_nan_from_point_cloud(point_cloud): 21 | return point_cloud[np.isfinite(point_cloud).any(axis=1)] 22 | 23 | 24 | def rotate_point_cloud_yaw(point_cloud, yaw_angle_deg): 25 | yaw_angle_rad = np.radians(yaw_angle_deg) 26 | rotation_matrix = np.array([ 27 | [np.cos(yaw_angle_rad), -np.sin(yaw_angle_rad), 0], 28 | [np.sin(yaw_angle_rad), 29 | np.cos(yaw_angle_rad), 0], 30 | [0, 0, 1], 31 | ]) 32 | return (rotation_matrix @ point_cloud.T).T 33 | 34 | 35 | if __name__ == "__main__": 36 | # Just tips for checking whether pybinding goes sucessfully or not. 37 | # attributes = dir(kiss_matcher) 38 | # for attr in attributes: 39 | # print(attr) 40 | 41 | parser = argparse.ArgumentParser( 42 | description="Point cloud registration using KISS-Matcher.") 43 | parser.add_argument( 44 | "--src_path", 45 | type=str, 46 | required=True, 47 | help="Source point cloud file (.bin or .pcd)", 48 | ) 49 | parser.add_argument( 50 | "--tgt_path", 51 | type=str, 52 | required=True, 53 | help="Target point cloud file (.bin or .pcd)", 54 | ) 55 | parser.add_argument( 56 | "--resolution", 57 | type=float, 58 | required=True, 59 | help="Resolution for processing (e.g., voxel size)", 60 | ) 61 | parser.add_argument( 62 | "--yaw_aug_angle", 63 | type=float, 64 | required=False, 65 | help="Yaw augmentation angle in degrees", 66 | ) 67 | 68 | args = parser.parse_args() 69 | 70 | # Load source point cloud 71 | if args.src_path.endswith(".bin"): 72 | src = read_bin(args.src_path) 73 | elif args.src_path.endswith(".pcd"): 74 | src = read_pcd(args.src_path) 75 | else: 76 | raise ValueError("Unsupported file format for src. Use .bin or .pcd") 77 | 78 | # Apply yaw augmentation if provided 79 | if args.yaw_aug_angle is not None: 80 | src = rotate_point_cloud_yaw(src, args.yaw_aug_angle) 81 | 82 | # Load target point cloud 83 | if args.tgt_path.endswith(".bin"): 84 | tgt = read_bin(args.tgt_path) 85 | elif args.tgt_path.endswith(".pcd"): 86 | tgt = read_pcd(args.tgt_path) 87 | else: 88 | raise ValueError("Unsupported file format for tgt. Use .bin or .pcd") 89 | 90 | # Remove NaN values from the point cloud. 91 | # See https://github.com/MIT-SPARK/KISS-Matcher/pull/16 92 | src = remove_nan_from_point_cloud(src) 93 | tgt = remove_nan_from_point_cloud(tgt) 94 | 95 | print(f"Loaded source point cloud: {src.shape}") 96 | print(f"Loaded target point cloud: {tgt.shape}") 97 | print(f"Resolution: {args.resolution}") 98 | print(f"Yaw Augmentation Angle: {args.yaw_aug_angle}") 99 | 100 | params = kiss_matcher.KISSMatcherConfig(args.resolution) 101 | matcher = kiss_matcher.KISSMatcher(params) 102 | result = matcher.estimate(src, tgt) 103 | matcher.print() 104 | 105 | num_rot_inliers = matcher.get_num_rotation_inliers() 106 | num_final_inliers = matcher.get_num_final_inliers() 107 | # NOTE(hlim): By checking the final inliers, we can determine whether 108 | # the registration was successful or not. The larger the threshold, 109 | # the more conservatively the decision is made. 110 | # See https://github.com/MIT-SPARK/KISS-Matcher/issues/24 111 | thres_num_inliers = 5 112 | if (num_final_inliers < thres_num_inliers): 113 | print("\033[1;33m=> Registration might have failed :(\033[0m\n") 114 | else: 115 | print("\033[1;32m=> Registration likely succeeded XD\033[0m\n") 116 | 117 | # ------------------------------------------------------------ 118 | # Visualization 119 | # ------------------------------------------------------------ 120 | # Apply transformation to src 121 | rotation_matrix = np.array(result.rotation) 122 | translation_vector = np.array(result.translation) 123 | transformed_src = (rotation_matrix @ src.T).T + translation_vector 124 | 125 | # Create Open3D point clouds 126 | src_o3d = o3d.geometry.PointCloud() 127 | src_o3d.points = o3d.utility.Vector3dVector(src) 128 | src_o3d.colors = o3d.utility.Vector3dVector( 129 | np.array([[0.78, 0.78, 0.78] for _ in range(src.shape[0])])) # Gray 130 | 131 | tgt_o3d = o3d.geometry.PointCloud() 132 | tgt_o3d.points = o3d.utility.Vector3dVector(tgt) 133 | tgt_o3d.colors = o3d.utility.Vector3dVector( 134 | np.array([[0.35, 0.65, 0.90] for _ in range(tgt.shape[0])])) # Cyan 135 | 136 | transformed_src_o3d = o3d.geometry.PointCloud() 137 | transformed_src_o3d.points = o3d.utility.Vector3dVector(transformed_src) 138 | transformed_src_o3d.colors = o3d.utility.Vector3dVector( 139 | np.array([[1.0, 0.63, 0.00] 140 | for _ in range(transformed_src.shape[0])])) # Orange 141 | 142 | # visualization 143 | vis = o3d.visualization.Visualizer() 144 | vis.create_window(window_name="KISS-Matcher Viz", 145 | width=1600, 146 | height=1200, 147 | left=300, 148 | top=300) 149 | vis.get_render_option().point_size = 0.3 150 | vis.get_render_option().background_color = np.array([0, 0, 0 151 | ]) # Black background 152 | vis.get_render_option().light_on = True 153 | 154 | vis.add_geometry(src_o3d) 155 | vis.add_geometry(tgt_o3d) 156 | vis.add_geometry(transformed_src_o3d) 157 | 158 | vis.run() 159 | vis.destroy_window() 160 | 161 | print("Visualization complete.") 162 | -------------------------------------------------------------------------------- /python/kiss_matcher/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Hyungtae Lim et al. 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 13 | # in all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | # from .kiss_matcher import * 23 | __version__ = "1.0.0" 24 | -------------------------------------------------------------------------------- /python/kiss_matcher/pybind/.gitignore: -------------------------------------------------------------------------------- 1 | include/ 2 | share/ 3 | -------------------------------------------------------------------------------- /python/kiss_matcher/pybind/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # find_package(Eigen3 REQUIRED) 24 | 25 | # pybind11_add_module(kiss_matcher_pybind MODULE kiss_matcher_pybind.cpp) 26 | # target_link_libraries(kiss_matcher_pybind PRIVATE kiss_matcher_core Eigen3::Eigen) 27 | # install(TARGETS kiss_matcher_pybind DESTINATION .) 28 | -------------------------------------------------------------------------------- /python/kiss_matcher/pybind/kiss_matcher_pybind.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "kiss_matcher/GncSolver.hpp" 36 | #include "kiss_matcher/KISSMatcher.hpp" 37 | 38 | #include "./stl_vector_eigen.h" 39 | 40 | namespace py = pybind11; 41 | using namespace py::literals; 42 | using namespace kiss_matcher; 43 | 44 | PYBIND11_MAKE_OPAQUE(std::vector); 45 | 46 | PYBIND11_MODULE(kiss_matcher, m) { 47 | m.doc() = "Pybind11 bindings for KISSMatcher library"; 48 | m.attr("__version__") = "0.3.1"; 49 | 50 | py::class_(m, "KISSMatcherConfig") 51 | .def(py::init(), 52 | "voxel_size"_a = 0.3, 53 | "use_voxel_sampling"_a = true, 54 | "use_quatro"_a = false, 55 | "thr_linearity"_a = 1.0, 56 | "num_max_corr"_a = 5000, 57 | "normal_r_gain"_a = 3.0, 58 | "fpfh_r_gain"_a = 5.0, 59 | "robin_noise_bound_gain"_a = 1.0, 60 | "solver_noise_bound_gain"_a = 0.75, 61 | "enable_noise_bound_clamping"_a = true) 62 | .def_readwrite("voxel_size", &KISSMatcherConfig::voxel_size_) 63 | .def_readwrite("use_voxel_sampling", &KISSMatcherConfig::use_voxel_sampling_) 64 | .def_readwrite("use_quatro", &KISSMatcherConfig::use_quatro_) 65 | .def_readwrite("thr_linearity", &KISSMatcherConfig::thr_linearity_) 66 | .def_readwrite("num_max_corr", &KISSMatcherConfig::num_max_corr_) 67 | .def_readwrite("normal_radius", &KISSMatcherConfig::normal_radius_) 68 | .def_readwrite("fpfh_radius", &KISSMatcherConfig::fpfh_radius_) 69 | .def_readwrite("robin_noise_bound_gain", &KISSMatcherConfig::robin_noise_bound_gain_) 70 | .def_readwrite("solver_noise_bound_gain", &KISSMatcherConfig::solver_noise_bound_gain_) 71 | .def_readwrite("robin_noise_bound", &KISSMatcherConfig::robin_noise_bound_) 72 | .def_readwrite("solver_noise_bound", &KISSMatcherConfig::solver_noise_bound_); 73 | 74 | // Bind RegistrationSolution 75 | py::class_(m, "RegistrationSolution") 76 | .def_readwrite("valid", &RegistrationSolution::valid) 77 | .def_readwrite("translation", &RegistrationSolution::translation) 78 | .def_readwrite("rotation", &RegistrationSolution::rotation); 79 | 80 | // Bind KISSMatcher 81 | py::class_(m, "KISSMatcher") 82 | .def(py::init(), "voxel_size"_a) 83 | .def(py::init(), "config"_a) 84 | .def("reset", &KISSMatcher::reset, "Reset the matcher state") 85 | .def("reset_solver", &KISSMatcher::resetSolver, "Reset the solver") 86 | .def("match", 87 | py::overload_cast &, 88 | const std::vector &>(&KISSMatcher::match), 89 | "src"_a, 90 | "tgt"_a, 91 | "Match keypoints from source and target") 92 | .def("match", 93 | py::overload_cast &, 94 | const Eigen::Matrix &>(&KISSMatcher::match), 95 | "src"_a, 96 | "tgt"_a, 97 | "Match keypoints from Eigen matrices") 98 | .def("estimate", &KISSMatcher::estimate, "src"_a, "tgt"_a, "Estimate transformation") 99 | .def("solve", 100 | &KISSMatcher::solve, 101 | "src_matched"_a, 102 | "tgt_matched"_a, 103 | "Estimate relative pose given already matched point clouds") 104 | .def("prune_and_solve", 105 | &KISSMatcher::pruneAndSolve, 106 | "src_matched"_a, 107 | "tgt_matched"_a, 108 | "Prune correspondences and estimate relative pose given already matched point clouds") 109 | .def("get_processed_input_clouds", 110 | &KISSMatcher::getProcessedInputClouds, 111 | "Get processed (i.e., voxelization) point clouds") 112 | .def("get_keypoints_from_faster_pfh", 113 | &KISSMatcher::getKeypointsFromFasterPFH, 114 | "Get keypoints from FasterPFH") 115 | .def("get_keypoints_from_initial_matching", 116 | &KISSMatcher::getKeypointsFromInitialMatching, 117 | "Get keypoints from initial matching") 118 | .def("get_initial_correspondences", 119 | &KISSMatcher::getInitialCorrespondences, 120 | "Get initial correspondences") 121 | .def("get_final_correspondences", 122 | &KISSMatcher::getFinalCorrespondences, 123 | "Get final correspondences") 124 | .def("get_num_rotation_inliers", 125 | &KISSMatcher::getNumRotationInliers, 126 | "Get # of rotation inliers") 127 | .def( 128 | "get_num_final_inliers", &KISSMatcher::getNumFinalInliers, "Get # of translation inliers") 129 | .def("clear", &KISSMatcher::clear, "Clear internal states") 130 | .def("get_processing_time", 131 | &KISSMatcher::getProcessingTime, 132 | "Get processing (i.e., voxelization) time") 133 | .def("get_extraction_time", &KISSMatcher::getExtractionTime, "Get feature extraction time") 134 | .def("get_rejection_time", &KISSMatcher::getRejectionTime, "Get outlier rejection time") 135 | .def("get_matching_time", &KISSMatcher::getMatchingTime, "Get matching time") 136 | .def("get_solver_time", &KISSMatcher::getSolverTime, "Get solver time") 137 | .def("print", &KISSMatcher::print, "Print matcher state"); 138 | } 139 | -------------------------------------------------------------------------------- /python/kiss_matcher/pybind/stl_vector_eigen.h: -------------------------------------------------------------------------------- 1 | // ---------------------------------------------------------------------------- 2 | // NOTE: This fily has been adapted from the Open3D project, but copyright 3 | // still belongs to Open3D. All rights reserved 4 | // ---------------------------------------------------------------------------- 5 | // - Open3D: www.open3d.org - 6 | // ---------------------------------------------------------------------------- 7 | // The MIT License (MIT) 8 | // 9 | // Copyright (c) 2018-2021 www.open3d.org 10 | // 11 | // Permission is hereby granted, free of charge, to any person obtaining a copy 12 | // of this software and associated documentation files (the "Software"), to deal 13 | // in the Software without restriction, including without limitation the rights 14 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 15 | // copies of the Software, and to permit persons to whom the Software is 16 | // furnished to do so, subject to the following conditions: 17 | // 18 | // The above copyright notice and this permission notice shall be included in 19 | // all copies or substantial portions of the Software. 20 | // 21 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 22 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 23 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 24 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 25 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 26 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 27 | // IN THE SOFTWARE. 28 | // ---------------------------------------------------------------------------- 29 | #pragma once 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | // pollute namespace with py 38 | #include 39 | namespace py = pybind11; 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace pybind11 { 46 | 47 | template , typename... Args> 48 | py::class_ bind_vector_without_repr(py::module &m, 49 | std::string const &name, 50 | Args &&...args) { 51 | // hack function to disable __repr__ for the convenient function 52 | // bind_vector() 53 | using Class_ = py::class_; 54 | Class_ cl(m, name.c_str(), std::forward(args)...); 55 | cl.def(py::init<>()); 56 | cl.def( 57 | "__bool__", 58 | [](const Vector &v) -> bool { return !v.empty(); }, 59 | "Check whether the list is nonempty"); 60 | cl.def("__len__", &Vector::size); 61 | return cl; 62 | } 63 | 64 | // - This function is used by Pybind for std::vector constructor. 65 | // This optional constructor is added to avoid too many Python <-> C++ API 66 | // calls when the vector size is large using the default biding method. 67 | // Pybind matches np.float64 array to py::array_t buffer. 68 | // - Directly using templates for the py::array_t and py::array_t 69 | // and etc. doesn't work. The current solution is to explicitly implement 70 | // bindings for each py array types. 71 | template 72 | std::vector py_array_to_vectors_double( 73 | py::array_t array) { 74 | int64_t eigen_vector_size = EigenVector::SizeAtCompileTime; 75 | if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { 76 | throw py::cast_error(); 77 | } 78 | std::vector eigen_vectors(array.shape(0)); 79 | auto array_unchecked = array.mutable_unchecked<2>(); 80 | for (auto i = 0; i < array_unchecked.shape(0); ++i) { 81 | eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); 82 | } 83 | return eigen_vectors; 84 | } 85 | 86 | } // namespace pybind11 87 | 88 | template , 90 | typename holder_type = std::unique_ptr, 91 | typename InitFunc> 92 | py::class_ pybind_eigen_vector_of_vector(py::module &m, 93 | const std::string &bind_name, 94 | const std::string &repr_name, 95 | InitFunc init_func) { 96 | using Scalar = typename EigenVector::Scalar; 97 | auto vec = py::bind_vector_without_repr>( 98 | m, bind_name, py::buffer_protocol(), py::module_local()); 99 | vec.def(py::init(init_func)); 100 | vec.def_buffer([](std::vector &v) -> py::buffer_info { 101 | size_t rows = EigenVector::RowsAtCompileTime; 102 | return py::buffer_info(v.data(), 103 | sizeof(Scalar), 104 | py::format_descriptor::format(), 105 | 2, 106 | {v.size(), rows}, 107 | {sizeof(EigenVector), sizeof(Scalar)}); 108 | }); 109 | vec.def("__repr__", [repr_name](const std::vector &v) { 110 | return repr_name + std::string(" with ") + std::to_string(v.size()) + 111 | std::string(" elements.\n") + std::string("Use numpy.asarray() to access data."); 112 | }); 113 | vec.def("__copy__", [](std::vector &v) { return std::vector(v); }); 114 | vec.def("__deepcopy__", [](std::vector &v) { return std::vector(v); }); 115 | 116 | // py::detail must be after custom constructor 117 | using Class_ = py::class_>; 118 | py::detail::vector_if_copy_constructible(vec); 119 | py::detail::vector_if_equal_operator(vec); 120 | py::detail::vector_modifiers(vec); 121 | py::detail::vector_accessor(vec); 122 | 123 | return vec; 124 | } 125 | -------------------------------------------------------------------------------- /python/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit_build_core", "pybind11"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "kiss_matcher" 7 | version = "1.0.0" 8 | requires-python = ">=3.8" 9 | description ='Python binding for KISS-Matcher' 10 | authors = [ 11 | { name = "Hyungtae Lim", email = "shapelim@mit.edu" }, 12 | ] 13 | classifiers = [ 14 | "Intended Audience :: Developers", 15 | "Intended Audience :: Education", 16 | "Intended Audience :: Other Audience", 17 | "Intended Audience :: Science/Research", 18 | "License :: OSI Approved :: MIT License", 19 | "Operating System :: MacOS", 20 | "Operating System :: Microsoft :: Windows", 21 | "Operating System :: Unix", 22 | "Programming Language :: C++", 23 | "Programming Language :: Python :: 3", 24 | "Programming Language :: Python :: 3.6", 25 | "Programming Language :: Python :: 3.7", 26 | "Programming Language :: Python :: 3.8", 27 | "Programming Language :: Python :: 3.9", 28 | "Programming Language :: Python :: 3.10", 29 | "Programming Language :: Python :: 3.11", 30 | "Programming Language :: Python :: 3.12", 31 | "Programming Language :: Python :: 3.13", 32 | ] 33 | 34 | dependencies = [ 35 | "numpy", 36 | "open3d", 37 | ] 38 | 39 | [tool.scikit-build] 40 | -------------------------------------------------------------------------------- /python/utils/bin2pcd.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | 4 | import numpy as np 5 | import open3d as o3d 6 | 7 | 8 | def bin_to_pcd(bin_file_name): 9 | size_float = 4 10 | list_pcd = [] 11 | with open(bin_file_name, "rb") as file: 12 | byte = file.read(size_float * 4) 13 | while byte: 14 | x, y, z, _ = struct.unpack("ffff", byte) 15 | list_pcd.append([x, y, z]) 16 | byte = file.read(size_float * 4) 17 | np_pcd = np.asarray(list_pcd) 18 | pcd = o3d.geometry.PointCloud() 19 | pcd.points = o3d.utility.Vector3dVector(np_pcd) 20 | return pcd 21 | 22 | 23 | def main(binFileName, pcd_file_name): 24 | pcd = bin_to_pcd(binFileName) 25 | o3d.io.write_point_cloud(pcd_file_name, pcd) 26 | 27 | 28 | if __name__ == "__main__": 29 | a = sys.argv[1] 30 | b = sys.argv[2] 31 | main(a, b) 32 | -------------------------------------------------------------------------------- /python/utils/download_datasets.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | import urllib.request 4 | import zipfile 5 | 6 | DATA_DIR = os.path.join(os.getcwd(), "data") 7 | ZIP_DIR = os.path.join(os.getcwd(), "zip_files") 8 | 9 | DROPBOX_FILES = [ 10 | "https://www.dropbox.com/scl/fi/1lsz6bxwan0sytj87ea9h/Vel16.zip?rlkey=g4jpze2j6iu6hk9ahq0m6t7o3&st=vrfpk4nv&dl=1", 11 | "https://www.dropbox.com/scl/fi/weqfi572gbi5z654d56ai/Vel64.zip?rlkey=l9upmgfjx7nhkbgl9du7igrfu&st=4q9gyous&dl=1", 12 | "https://www.dropbox.com/scl/fi/lnsbaqmbgz0qi1r8ocesd/HeLiPR-KAIST05.zip?rlkey=50jyhl180qpmf1j5jn9ru37ru&st=q0kay7o3&dl=1", 13 | "https://www.dropbox.com/scl/fi/c6c1jxrld17ywj7x2ok1q/VBR-Collosseo.zip?rlkey=b1sk0xvnntqy8kyw1apob37ob&st=5imrvvwo&dl=1", 14 | "https://www.dropbox.com/scl/fi/73l59cj5ypfvjrkrc23qx/KITTI00-to-KITTI360.zip?rlkey=pqukxxgpxaq1pugo6dhzq8xa4&st=yns7uolj&dl=1", 15 | "https://www.dropbox.com/scl/fi/lb49afp7x5ja3bfptbo68/KITTI00-to-07.zip?rlkey=qkwq99vwwlnxnxj3nhdptrusr&st=nzx8ts9j&dl=1", 16 | "https://www.dropbox.com/scl/fi/n6sypvt4rdssn172mn2jv/bun_zipper.ply?rlkey=hk2danjxt29a7ahq374s8m7ak&st=o5udnqjv&dl=1", 17 | ] 18 | 19 | DOWNLOADED_FILES = [ 20 | os.path.join(ZIP_DIR, "Vel16.zip"), 21 | os.path.join(ZIP_DIR, "Vel64.zip"), 22 | os.path.join(ZIP_DIR, "HeLiPR-KAIST05.zip"), 23 | os.path.join(ZIP_DIR, "VBR-Collosseo.zip"), 24 | os.path.join(ZIP_DIR, "KITTI00-to-KITTI360.zip"), 25 | os.path.join(ZIP_DIR, "KITTI00-to-07.zip"), 26 | os.path.join(ZIP_DIR, "bun_zipper.ply"), 27 | ] 28 | 29 | os.makedirs(DATA_DIR, exist_ok=True) 30 | os.makedirs(ZIP_DIR, exist_ok=True) 31 | 32 | 33 | def download_file(url, dest_file): 34 | print(f"Downloading {url} -> {dest_file}") 35 | urllib.request.urlretrieve(url, dest_file) 36 | print(f"Downloaded: {dest_file}") 37 | 38 | 39 | def extract_zip(zip_path, extract_to): 40 | print(f"Extracting {zip_path} to {extract_to}") 41 | with zipfile.ZipFile(zip_path, "r") as zip_ref: 42 | zip_ref.extractall(extract_to) 43 | print(f"Extracted: {zip_path}") 44 | 45 | 46 | def main(): 47 | for url, dest_file in zip(DROPBOX_FILES, DOWNLOADED_FILES): 48 | if not os.path.exists(dest_file): 49 | download_file(url, dest_file) 50 | else: 51 | print(f"File already exists: {dest_file}, skipping download.") 52 | 53 | if dest_file.endswith(".zip"): 54 | extract_zip(dest_file, DATA_DIR) 55 | else: 56 | os.replace(dest_file, 57 | os.path.join(DATA_DIR, os.path.basename(dest_file))) 58 | 59 | # Remove `ZIP_DIR` directory 60 | shutil.rmtree(ZIP_DIR, ignore_errors=True) 61 | print("✅ All datasets downloaded and extracted successfully!") 62 | 63 | 64 | if __name__ == "__main__": 65 | main() 66 | -------------------------------------------------------------------------------- /ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.24) 2 | project(kiss_matcher_ros) 3 | 4 | # Set a default build type if none is specified 5 | if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 6 | message(STATUS "Setting build type to 'Release' as none was specified.") 7 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build." FORCE) 8 | # Provide options for cmake-gui or ccmake 9 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 10 | endif() 11 | 12 | set(CMAKE_CXX_STANDARD 17) 13 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 14 | set(CMAKE_CXX_EXTENSIONS OFF) 15 | cmake_policy(SET CMP0074 NEW) 16 | 17 | option(USE_ADDRESS_SANITIZER "Use address sanitizer" OFF) 18 | if (USE_ADDRESS_SANITIZER) 19 | add_compile_options(-fsanitize=address -fsanitize-recover=address) 20 | add_link_options(-fsanitize=address -fsanitize-recover=address) 21 | endif() 22 | 23 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_matcher/ AND NOT IS_SYMLINK ${CMAKE_CURRENT_SOURCE_DIR}) 24 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kiss_matcher ${CMAKE_CURRENT_BINARY_DIR}/kiss_matcher) 25 | else() 26 | #This is what kiss_icp does. 27 | #cmake_minimum_required(VERSION 3.18) 28 | #message(STATUS "Performing out-of-tree build, fetching KISS-Matcher v${CMAKE_PROJECT_VERSION} Release from Github") 29 | #include(FetchContent) 30 | #FetchContent_Declare( 31 | # ext_kiss_matcher_core PREFIX kiss_matcher_core 32 | # URL https://github.com/PRBonn/kiss-matcher/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR 33 | # cpp/kiss_matcher) 34 | #FetchContent_MakeAvailable(ext_kiss_matcher_core) 35 | endif() 36 | 37 | # Load small_gicp 38 | include(FetchContent) 39 | FetchContent_Declare( 40 | small_gicp 41 | GIT_REPOSITORY https://github.com/koide3/small_gicp 42 | GIT_TAG master 43 | ) 44 | FetchContent_MakeAvailable(small_gicp) 45 | 46 | find_package(ament_cmake REQUIRED) 47 | find_package(nav_msgs REQUIRED) 48 | find_package(rcutils REQUIRED) 49 | find_package(rclcpp REQUIRED) 50 | find_package(rclcpp_components REQUIRED) 51 | find_package(geometry_msgs REQUIRED) 52 | find_package(std_msgs REQUIRED) 53 | find_package(sensor_msgs REQUIRED) 54 | find_package(visualization_msgs REQUIRED) 55 | find_package(pcl_conversions REQUIRED) 56 | find_package(PCL REQUIRED) 57 | find_package(tf2_ros REQUIRED) 58 | find_package(tf2_eigen REQUIRED) 59 | find_package(tf2_geometry_msgs REQUIRED) 60 | find_package(tf2_sensor_msgs REQUIRED) 61 | 62 | find_package(PCL REQUIRED) 63 | include_directories( 64 | include 65 | src 66 | ${PCL_INCLUDE_DIRS}) 67 | link_directories(${PCL_LIBRARY_DIRS}) 68 | add_definitions(${PCL_DEFINITIONS}) 69 | 70 | find_package(GTSAM REQUIRED) 71 | 72 | ################################################################################ 73 | add_executable(run_kiss_matcher src/run_kiss_matcher.cpp) 74 | target_include_directories(run_kiss_matcher 75 | PUBLIC 76 | ${PCL_INCLUDE_DIRS} 77 | ) 78 | 79 | target_link_libraries(run_kiss_matcher 80 | kiss_matcher::kiss_matcher_core 81 | ${PCL_LIBRARIES} 82 | ) 83 | 84 | add_executable(registration_visualizer src/registration_visualizer.cpp) 85 | ament_target_dependencies(registration_visualizer 86 | rclcpp 87 | tf2_ros 88 | sensor_msgs 89 | pcl_conversions 90 | PCL 91 | ) 92 | target_link_libraries(registration_visualizer 93 | kiss_matcher::kiss_matcher_core 94 | ${PCL_LIBRARIES} 95 | ) 96 | 97 | add_executable(kiss_matcher_sam src/slam/pose_graph_manager.cpp 98 | src/slam/loop_closure.cpp 99 | src/slam/loop_detector.cpp) 100 | ament_target_dependencies(kiss_matcher_sam 101 | rclcpp 102 | tf2_ros 103 | sensor_msgs 104 | nav_msgs 105 | visualization_msgs 106 | pcl_conversions 107 | PCL 108 | ) 109 | 110 | target_link_libraries(kiss_matcher_sam 111 | kiss_matcher::kiss_matcher_core 112 | ${PCL_LIBRARIES} 113 | small_gicp 114 | gtsam 115 | ) 116 | 117 | add_executable(inter_frame_alignment src/inter_frame_alignment.cpp 118 | src/slam/loop_closure.cpp) 119 | ament_target_dependencies(inter_frame_alignment 120 | rclcpp 121 | tf2_ros 122 | sensor_msgs 123 | nav_msgs 124 | visualization_msgs 125 | pcl_conversions 126 | PCL 127 | ) 128 | 129 | target_link_libraries(inter_frame_alignment 130 | kiss_matcher::kiss_matcher_core 131 | ${PCL_LIBRARIES} 132 | small_gicp 133 | gtsam 134 | ) 135 | 136 | install(TARGETS 137 | registration_visualizer 138 | run_kiss_matcher 139 | kiss_matcher_sam 140 | inter_frame_alignment 141 | LIBRARY DESTINATION lib 142 | RUNTIME DESTINATION lib/${PROJECT_NAME} 143 | ) 144 | 145 | install(DIRECTORY launch config rviz 146 | DESTINATION share/${PROJECT_NAME} 147 | ) 148 | 149 | ament_package() 150 | -------------------------------------------------------------------------------- /ros/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 MIT-SPARK 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 |
2 |

KISS-Matcher-SAM

3 | 4 | 5 | 6 | 7 | 8 |
9 |
10 |
11 |
12 |

KISS Matcher

13 |

LiDAR SLAM = Any LO/LIO + KISS-Matcher-SAM

14 |
15 | 16 | ______________________________________________________________________ 17 | 18 | ## ROS2 KISS-Matcher-SAM 19 | 20 | This repository provides LiDAR SLAM using KISS-Matcher. 21 | If you just want to **perform registration using ROS2**, see [README_ROS2_REGISTRATION.md](https://github.com/MIT-SPARK/KISS-Matcher/blob/main/ros/README_ROS2_REGISTRATION.md). 22 | 23 | ## :gear: How To Build & RUN 24 | 25 | 1. Put this repository in your colcon workspace, and then build this repository as follows: 26 | 27 | ```bash 28 | cd ${YOUR_ROS2_WORKSPACE}/src 29 | git clone https://github.com/MIT-SPARK/KISS-Matcher.git 30 | cd .. 31 | colcon build --packages-select kiss_matcher_ros 32 | ``` 33 | 34 | And then source your workspace using `source install/setup.bash` or `source install/setup.zsh` depending on your shell. 35 | 36 | 2. Then, run the command below: 37 | 38 | ``` 39 | ros2 launch kiss_matcher_ros run_kiss_matcher_sam.launch.yaml 40 | ``` 41 | 42 | If you want to run it on your own dataset, make sure to set the `/cloud` and `/odom` topics appropriately using: 43 | 44 | ``` 45 | ros2 launch kiss_matcher_ros run_kiss_matcher_sam.launch.yaml \ 46 | odom_topic:= scan_topic:= 47 | ``` 48 | 49 | ______________________________________________________________________ 50 | 51 | ## 🚀 Example demos 52 | 53 | To run `KISS-Matcher-SAM`, you need to need an external LiDAR(-inertial) odometry. We use [SPARK-FAST-LIO](https://github.com/MIT-SPARK/spark-fast-lio) as an example. 54 | We provide **two out-of-the-box ROS2** examples using pre-processed ROS2 bag data (because the original data are only available in ROS1). 55 | All pre-processed ROS2 bag files can be found [**here**](https://www.dropbox.com/scl/fo/i56kucdzxpzq1mr5jula7/ALJpdqvOZT1hTaQXEePCvyI?rlkey=y5bvslyazf09erko7gl0aylll&st=dh91zyho&dl=0). 56 | 57 | ### 🇺🇸 SLAM on the MIT campus 58 | 59 | 1. Download `10_14_acl_jackal` and `10_14_hathor` from our [Dropbox](https://www.dropbox.com/scl/fo/i56kucdzxpzq1mr5jula7/ALJpdqvOZT1hTaQXEePCvyI?rlkey=y5bvslyazf09erko7gl0aylll&st=dh91zyho&dl=0). 60 | 61 | 1. Build and run `spark_fast_lio` with the following commands: 62 | 63 | **To build:** 64 | 65 | ```shell 66 | cd ${YOUR_COLCON_WORKSPACE}/src 67 | git clone https://github.com/MIT-SPARK/spark-fast-lio.git 68 | colcon build --packages-up-to spark_fast_lio 69 | ``` 70 | 71 | **To run:** 72 | 73 | ``` 74 | ros2 launch spark_fast_lio mapping_mit_campus.launch.yaml scene_id:=acl_jackal 75 | ``` 76 | 77 | 3. Adjust the parameters of `kiss_matcher_ros` as below (see [Issue #47](https://github.com/MIT-SPARK/KISS-Matcher/issues/47). 78 | This is necessary because the scan was acquired using a VLP-16, which is relatively sparse: 79 | 80 | ![Image](https://github.com/user-attachments/assets/824d4686-a897-4261-9eef-9662f16622a4) 81 | 82 | and run 83 | 84 | ``` 85 | ros2 launch kiss_matcher_ros run_kiss_matcher_sam.launch.yaml 86 | ``` 87 | 88 | 4. In another terminal, play the ROS 2 bag file: 89 | 90 | ``` 91 | ros2 bag play 10_14_acl_jackal 92 | ``` 93 | 94 | #### Qualitative mapping result 95 | 96 | ![](https://github.com/user-attachments/assets/bd24f054-9818-426c-814c-4422e438727a) 97 | 98 | ### 🏟️ SLAM on the Colosseum 99 | 100 | 1. Download `colosse_train0` from our [Dropbox](https://www.dropbox.com/scl/fo/i56kucdzxpzq1mr5jula7/ALJpdqvOZT1hTaQXEePCvyI?rlkey=y5bvslyazf09erko7gl0aylll&st=dh91zyho&dl=0). 101 | 102 | 1. Follow the same steps as above to run spark_fast_lio and kiss_matcher_ros. 103 | *No parameter tuning is needed for this dataset*. 104 | 105 | ``` 106 | ros2 launch spark_fast_lio mapping_vbr_colosseo.launch.yaml 107 | ``` 108 | 109 | (In another terminal) 110 | 111 | ``` 112 | ros2 launch kiss_matcher_ros run_kiss_matcher_sam.launch.yaml 113 | ``` 114 | 115 | 3. Finally, play the bag file: 116 | 117 | ``` 118 | ros2 bag play colosseo_train0 119 | ``` 120 | 121 | ## How to Tune Parameters? 122 | 123 | More details can be found in [config/slam_config.yaml](https://github.com/MIT-SPARK/KISS-Matcher/blob/main/ros/config/slam_config.yaml). 124 | -------------------------------------------------------------------------------- /ros/README_ROS2_REGISTRATION.md: -------------------------------------------------------------------------------- 1 |
2 |

KISS-Matcher

3 | 4 | 5 | 6 | 7 | 8 |
9 |
10 |
11 |
12 |

KISS Matcher

13 |

Keep it simple, make it scalable.

14 |
15 | 16 | ______________________________________________________________________ 17 | 18 | ## KISS-Matcher Registraion Example 19 | 20 | 1. If you only need to download example datasets, please use [download_datasets.py](https://github.com/kimdaebeom/KISS-Matcher/blob/main/python/utils/download_datasets.py). 21 | 22 | 1. Then, launch the visualization using the following command: 23 | 24 | ```bash 25 | ros2 launch kiss_matcher_ros visualizer_launch.py 26 | ``` 27 | 28 | ## 🛠 Configuration 29 | 30 | You can customize the visualization parameters in **`config/params.yaml`** before launching the node. 31 | 32 | ### **Example Configuration** 33 | 34 | ```yaml 35 | registration_visualizer: 36 | ros__parameters: 37 | base_dir: "src/KISS-Matcher/cpp/examples/build/data/" 38 | 39 | # Specify the source and target PCD files 40 | src_pcd_path: "Vel64/kitti_000540.pcd" 41 | tgt_pcd_path: "Vel64/kitti_001319.pcd" 42 | 43 | # Registration settings 44 | resolution: 0.2 # Voxel grid resolution 45 | moving_rate: 200.0 # Animation steps 46 | frame_rate: 30.0 # FPS for animation 47 | scale_factor: 1.0 # Scale factor for the point cloud 48 | ``` 49 | 50 | ### **Parameter Descriptions** 51 | 52 | | Parameter | Description | 53 | |---------------|-------------| 54 | | `src_pcd_path` | Path to the source PCD file | 55 | | `tgt_pcd_path` | Path to the target PCD file | 56 | | `resolution` | Voxel grid downsampling resolution | 57 | | `moving_rate` | Number of animation steps of transition | 58 | | `frame_rate` | Frames per second for animation in RViz | 59 | | `scale_factor` | Scaling factor applied to the point clouds | 60 | 61 | ______________________________________________________________________ 62 | 63 | ## 📌 Notes 64 | 65 | - Ensure your **PCD files** exist in the correct directory specified in `params.yaml`. 66 | - If visualization does not start, verify that **PCL and ROS2 dependencies** are correctly installed. 67 | - Modify the **frame rate (`frame_rate`) and animation steps (`moving_rate`)** for smoother visualization. 68 | 69 | Now, you can visualize **KISS-Matcher registration results** in ROS2! 🎉 70 | -------------------------------------------------------------------------------- /ros/config/alignment_config.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | frame_update_hz: 0.033 # Frequency to perform alignment 4 | 5 | voxel_resolution: 1.0 6 | 7 | verbose: true 8 | loop: 9 | verbose: true 10 | 11 | local_reg: 12 | num_threads: 8 # Number of threads used for local registration 13 | correspondences_number: 20 # Minimum number of correspondences required 14 | max_num_iter: 32 # Maximum iterations for local registration 15 | scale_factor_for_corr_dist: 3.0 # ⭐ Scale for correspondence search distance 16 | overlap_threshold: 0.0 # Not used in the alignment. `num_inliers_threshold` is enough. 17 | 18 | global_reg: 19 | enable: true 20 | # NOTE(hlim): The best value of `num_inliers_threshold` highly depends on `num_submap_keyframes` 21 | # Thus, heuristic tuning is required for the best performance. 22 | # Larger values make it more conservative (i.e., higher `num_inliers_threshold` enhances LC precision) 23 | num_inliers_threshold: 20 # ⭐ Minimum inlier count to accept KISS-Matcher initial alignment 24 | -------------------------------------------------------------------------------- /ros/config/kimera_multi/ouster64.yaml: -------------------------------------------------------------------------------- 1 | # ===================================================================================== 2 | # Parameter Guide: 3 | # - The ⭐ symbol highlights parameters that significantly impact SLAM performance. 4 | # - Tune these carefully based on your robot and environment 5 | # ===================================================================================== 6 | /**: 7 | ros__parameters: 8 | loop_pub_hz: 0.1 # Frequency to publish loop closures (Hz) 9 | loop_detector_hz: 2.0 # Frequency to update loop by external loop detector (Hz) 10 | loop_nnsearch_hz: 2.0 # Frequency to update loop by nearest neighbor search (Hz) 11 | loop_pub_delayed_time: 30.0 # Delay before publishing loop closure results (sec) 12 | map_update_hz: 0.2 # Frequency to update the map (Hz) 13 | vis_hz: 1.0 # Visualization update frequency (Hz) 14 | 15 | store_voxelized_scan: true # Save the cloud of node in voxelized one or not. 16 | voxel_resolution: 0.3 # ⭐ [Unit: m] Voxel grid resolution for downsampling 17 | map_voxel_resolution: 1.0 # Resolution used when building map. Only for visualization 18 | save_voxel_resolution: 0.3 # Resolution used when saving voxelized map 19 | 20 | keyframe: 21 | keyframe_threshold: 1.5 # ⭐ [Unit: m] Distance threshold to add a new keyframe 22 | # NOTE(hlim): Once `num_submap_keyframes` is larger than 1, 23 | # Submap-to-submap registration is performed. 24 | # The main advantage of KISS-Matcher is its super-fast speed in submap matching! 25 | # see: https://arxiv.org/pdf/2409.15615 26 | num_submap_keyframes: 1 # ⭐ > 0. Number of keyframes per submap. It should be an odd number 27 | 28 | loop: 29 | verbose: true # Enable verbose output 30 | # If true, Z-axis differences are considered in loop detection. 31 | # If false, loop detection assumes the robot moves mainly on a single horizontal plane. 32 | is_multilayer_env: false # ⭐ Whether the robot operates in a multilayer environment 33 | # NOTE(hlim): You may need to tune `loop_detection_radius` and `loop_detection_timediff_threshold` for your environment 34 | # after checking the Loop closure radius marker in Rviz. 35 | loop_detection_radius: 20.0 # ⭐ [Unit: m] Radius for detecting loop candidates. 36 | loop_detection_timediff_threshold: 90.0 # ⭐ [Unit: t] Minimum time difference for loop candidates 37 | 38 | local_reg: 39 | num_threads: 8 # Number of threads used for local registration 40 | correspondences_number: 20 # Minimum number of correspondences required 41 | max_num_iter: 32 # Maximum iterations for local registration 42 | # NOTE(hlim): `scale_factor_for_corr_dist` * `voxel_resolution` is set to 43 | # distance threshold for ICP-variants (i.e., it is a factor relative to voxel resolution) 44 | scale_factor_for_corr_dist: 5.0 # ⭐ Scale for correspondence search distance 45 | # NOTE(hlim): The best value of `overlap_threshold` highly depends on `num_submap_keyframes` 46 | # Thus, heuristic tuning is required for the best performance. 47 | # Larger values make it more conservative (i.e., higher `overlap_threshold` enhances LC precision) 48 | overlap_threshold: 55.0 # ⭐ [Unit: %] Threshold for the ratio of ICP inliers 49 | # (i.e., the percentage of correspondences within `corr_dist`). 50 | # A value close to 100% means the source and target are nearly perfectly aligned. 51 | # In cases of partial overlap, this ratio can never reach 100%. 52 | # If you set `num_submap_keyframes` as 1 (i.e., scan-to-scan matching), 53 | # it's recommended to use a higher value. 54 | global_reg: 55 | enable: true 56 | # NOTE(hlim): The best value of `num_inliers_threshold` highly depends on `num_submap_keyframes` 57 | # Thus, heuristic tuning is required for the best performance. 58 | # Larger values make it more conservative (i.e., higher `num_inliers_threshold` enhances LC precision) 59 | num_inliers_threshold: 50 # ⭐ Minimum inlier count to accept KISS-Matcher initial alignment 60 | 61 | result: 62 | save_map_pcd: false 63 | save_map_bag: false 64 | save_in_kitti_format: false 65 | seq_name: "sequence" 66 | -------------------------------------------------------------------------------- /ros/config/kimera_multi/velodyne16.yaml: -------------------------------------------------------------------------------- 1 | # ===================================================================================== 2 | # Parameter Guide: 3 | # - The ⭐ symbol highlights parameters that significantly impact SLAM performance. 4 | # - Tune these carefully based on your robot and environment 5 | # ===================================================================================== 6 | /**: 7 | ros__parameters: 8 | loop_pub_hz: 0.1 # Frequency to publish loop closures (Hz) 9 | loop_detector_hz: 2.0 # Frequency to update loop by external loop detector (Hz) 10 | loop_nnsearch_hz: 2.0 # Frequency to update loop by nearest neighbor search (Hz) 11 | loop_pub_delayed_time: 30.0 # Delay before publishing loop closure results (sec) 12 | map_update_hz: 0.2 # Frequency to update the map (Hz) 13 | vis_hz: 1.0 # Visualization update frequency (Hz) 14 | 15 | store_voxelized_scan: true # Save the cloud of node in voxelized one or not. 16 | voxel_resolution: 0.3 # ⭐ [Unit: m] Voxel grid resolution for downsampling 17 | map_voxel_resolution: 1.0 # Resolution used when building map. Only for visualization 18 | save_voxel_resolution: 0.3 # Resolution used when saving voxelized map 19 | 20 | keyframe: 21 | keyframe_threshold: 1.5 # ⭐ [Unit: m] Distance threshold to add a new keyframe 22 | # NOTE(hlim): Once `num_submap_keyframes` is larger than 1, 23 | # Submap-to-submap registration is performed. 24 | # The main advantage of KISS-Matcher is its super-fast speed in submap matching! 25 | # see: https://arxiv.org/pdf/2409.15615 26 | num_submap_keyframes: 5 # ⭐ > 0. Number of keyframes per submap. It should be an odd number 27 | 28 | loop: 29 | verbose: true # Enable verbose output 30 | # If true, Z-axis differences are considered in loop detection. 31 | # If false, loop detection assumes the robot moves mainly on a single horizontal plane. 32 | is_multilayer_env: false # ⭐ Whether the robot operates in a multilayer environment 33 | # NOTE(hlim): You may need to tune `loop_detection_radius` and `loop_detection_timediff_threshold` for your environment 34 | # after checking the Loop closure radius marker in Rviz. 35 | loop_detection_radius: 20.0 # ⭐ [Unit: m] Radius for detecting loop candidates. 36 | loop_detection_timediff_threshold: 90.0 # ⭐ [Unit: t] Minimum time difference for loop candidates 37 | 38 | local_reg: 39 | num_threads: 8 # Number of threads used for local registration 40 | correspondences_number: 20 # Minimum number of correspondences required 41 | max_num_iter: 32 # Maximum iterations for local registration 42 | # NOTE(hlim): `scale_factor_for_corr_dist` * `voxel_resolution` is set to 43 | # distance threshold for ICP-variants (i.e., it is a factor relative to voxel resolution) 44 | scale_factor_for_corr_dist: 5.0 # ⭐ Scale for correspondence search distance 45 | # NOTE(hlim): The best value of `overlap_threshold` highly depends on `num_submap_keyframes` 46 | # Thus, heuristic tuning is required for the best performance. 47 | # Larger values make it more conservative (i.e., higher `overlap_threshold` enhances LC precision) 48 | overlap_threshold: 30.0 # ⭐ [Unit: %] Threshold for the ratio of ICP inliers 49 | # (i.e., the percentage of correspondences within `corr_dist`). 50 | # A value close to 100% means the source and target are nearly perfectly aligned. 51 | # In cases of partial overlap, this ratio can never reach 100%. 52 | # If you set `num_submap_keyframes` as 1 (i.e., scan-to-scan matching), 53 | # it's recommended to use a higher value. 54 | global_reg: 55 | enable: true 56 | # NOTE(hlim): The best value of `num_inliers_threshold` highly depends on `num_submap_keyframes` 57 | # Thus, heuristic tuning is required for the best performance. 58 | # Larger values make it more conservative (i.e., higher `num_inliers_threshold` enhances LC precision) 59 | num_inliers_threshold: 10 # ⭐ Minimum inlier count to accept KISS-Matcher initial alignment 60 | 61 | result: 62 | save_map_pcd: false 63 | save_map_bag: false 64 | save_in_kitti_format: false 65 | seq_name: "sequence" 66 | -------------------------------------------------------------------------------- /ros/config/params.yaml: -------------------------------------------------------------------------------- 1 | registration_visualizer: 2 | ros__parameters: 3 | base_dir: "src/KISS-Matcher/cpp/examples/build/data/" 4 | # NOTE(dkim): These are provided examples of PCD pairs 5 | # src_pcd_path: "HeLiPR-KAIST05/Aeva.pcd" 6 | # tgt_pcd_path: "HeLiPR-KAIST05/Ouster.pcd" 7 | # src_pcd_path: "KITTI00-to-07/kitti00.pcd" 8 | # tgt_pcd_path: "KITTI00-to-07/kitti07.pcd" 9 | # src_pcd_path: "KITTI00-to-KITTI360/kitti00.pcd" 10 | # tgt_pcd_path: "KITTI00-to-KITTI360/kitti360_09.pcd" 11 | # src_pcd_path: "VBR-Collosseo/test0.pcd" 12 | # tgt_pcd_path: "VBR-Collosseo/train0.pcd" 13 | src_pcd_path: "Vel16/src.pcd" 14 | tgt_pcd_path: "Vel16/tgt.pcd" 15 | # src_pcd_path: "Vel64/kitti_000540.pcd" 16 | # tgt_pcd_path: "Vel64/kitti_001319.pcd" 17 | # src_pcd_path: "VBR-Collosseo/test0.pcd" 18 | # tgt_pcd_path: "VBR-Collosseo/train0.pcd" 19 | 20 | resolution: 0.2 21 | moving_rate: 200.0 22 | frame_rate: 30.0 23 | scale_factor: 1.0 24 | -------------------------------------------------------------------------------- /ros/config/slam_config.yaml: -------------------------------------------------------------------------------- 1 | # ===================================================================================== 2 | # Parameter Guide: 3 | # - The ⭐ symbol highlights parameters that significantly impact SLAM performance. 4 | # - Tune these carefully based on your robot and environment 5 | # ===================================================================================== 6 | /**: 7 | ros__parameters: 8 | loop_pub_hz: 0.1 # Frequency to publish loop closures (Hz) 9 | loop_detector_hz: 2.0 # Frequency to update loop by external loop detector (Hz) 10 | loop_nnsearch_hz: 2.0 # Frequency to update loop by nearest neighbor search (Hz) 11 | loop_pub_delayed_time: 30.0 # Delay before publishing loop closure results (sec) 12 | map_update_hz: 0.2 # Frequency to update the map (Hz) 13 | vis_hz: 1.0 # Visualization update frequency (Hz) 14 | 15 | store_voxelized_scan: false # Save the cloud of node in voxelized one or not. 16 | voxel_resolution: 0.3 # ⭐ [Unit: m] Voxel grid resolution for downsampling 17 | map_voxel_resolution: 1.0 # Resolution used when building map. Only for visualization 18 | save_voxel_resolution: 0.3 # Resolution used when saving voxelized map 19 | 20 | keyframe: 21 | keyframe_threshold: 1.5 # ⭐ [Unit: m] Distance threshold to add a new keyframe 22 | # NOTE(hlim): Once `num_submap_keyframes` is larger than 1, 23 | # Submap-to-submap registration is performed. 24 | # The main advantage of KISS-Matcher is its super-fast speed in submap matching! 25 | # see: https://arxiv.org/pdf/2409.15615 26 | num_submap_keyframes: 1 # ⭐ > 0. Number of keyframes per submap. It should be an odd number 27 | 28 | loop: 29 | verbose: true # Enable verbose output 30 | # If true, Z-axis differences are considered in loop detection. 31 | # If false, loop detection assumes the robot moves mainly on a single horizontal plane. 32 | is_multilayer_env: false # ⭐ Whether the robot operates in a multilayer environment 33 | # NOTE(hlim): You may need to tune `loop_detection_radius` and `loop_detection_timediff_threshold` for your environment 34 | # after checking the Loop closure radius marker in Rviz. 35 | loop_detection_radius: 20.0 # ⭐ [Unit: m] Radius for detecting loop candidates. 36 | loop_detection_timediff_threshold: 90.0 # ⭐ [Unit: t] Minimum time difference for loop candidates 37 | 38 | local_reg: 39 | num_threads: 8 # Number of threads used for local registration 40 | correspondences_number: 20 # Minimum number of correspondences required 41 | max_num_iter: 32 # Maximum iterations for local registration 42 | # NOTE(hlim): `scale_factor_for_corr_dist` * `voxel_resolution` is set to 43 | # distance threshold for ICP-variants (i.e., it is a factor relative to voxel resolution) 44 | scale_factor_for_corr_dist: 5.0 # ⭐ Scale for correspondence search distance 45 | # NOTE(hlim): The best value of `overlap_threshold` highly depends on `num_submap_keyframes` 46 | # Thus, heuristic tuning is required for the best performance. 47 | # Larger values make it more conservative (i.e., higher `overlap_threshold` enhances LC precision) 48 | overlap_threshold: 55.0 # ⭐ [Unit: %] Threshold for the ratio of ICP inliers 49 | # (i.e., the percentage of correspondences within `corr_dist`). 50 | # A value close to 100% means the source and target are nearly perfectly aligned. 51 | # In cases of partial overlap, this ratio can never reach 100%. 52 | # If you set `num_submap_keyframes` as 1 (i.e., scan-to-scan matching), 53 | # it's recommended to use a higher value. 54 | global_reg: 55 | enable: true 56 | # NOTE(hlim): The best value of `num_inliers_threshold` highly depends on `num_submap_keyframes` 57 | # Thus, heuristic tuning is required for the best performance. 58 | # Larger values make it more conservative (i.e., higher `num_inliers_threshold` enhances LC precision) 59 | num_inliers_threshold: 50 # ⭐ Minimum inlier count to accept KISS-Matcher initial alignment 60 | 61 | result: 62 | save_map_pcd: false 63 | save_map_bag: false 64 | save_in_kitti_format: false 65 | seq_name: "sequence" 66 | -------------------------------------------------------------------------------- /ros/include/slam/loop_closure.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_LOOP_CLOSURE_H 4 | #define KISS_MATCHER_LOOP_CLOSURE_H 5 | 6 | ///// C++ common headers 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "slam/loop_types.hpp" 23 | #include "slam/pose_graph_node.hpp" 24 | #include "slam/utils.hpp" 25 | 26 | using NodePair = std::tuple, pcl::PointCloud>; 27 | 28 | namespace kiss_matcher { 29 | struct GICPConfig { 30 | int num_threads_ = 4; 31 | int correspondence_randomness_ = 20; 32 | int max_num_iter_ = 20; 33 | 34 | double max_corr_dist_ = 1.0; 35 | double scale_factor_for_corr_dist_ = 5.0; 36 | double overlap_threshold_ = 90.0; 37 | }; 38 | 39 | struct LoopClosureConfig { 40 | bool verbose_ = false; 41 | bool enable_global_registration_ = true; 42 | bool is_multilayer_env_ = false; 43 | size_t num_submap_keyframes_ = 11; 44 | size_t num_inliers_threshold_ = 100; 45 | double voxel_res_ = 0.1; 46 | double loop_detection_radius_; 47 | double loop_detection_timediff_threshold_; 48 | GICPConfig gicp_config_; 49 | KISSMatcherConfig matcher_config_; 50 | }; 51 | 52 | // Registration Output 53 | struct RegOutput { 54 | bool is_valid_ = false; 55 | bool is_converged_ = false; 56 | size_t num_final_inliers_ = 0; 57 | double overlapness_ = 0.0; 58 | Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(); 59 | }; 60 | 61 | class LoopClosure { 62 | private: 63 | // For coarse-to-fine alignment 64 | std::shared_ptr global_reg_handler_ = nullptr; 65 | std::shared_ptr> local_reg_handler_ = nullptr; 66 | 67 | pcl::PointCloud::Ptr src_cloud_; 68 | pcl::PointCloud::Ptr tgt_cloud_; 69 | pcl::PointCloud::Ptr coarse_aligned_; 70 | pcl::PointCloud::Ptr aligned_; 71 | pcl::PointCloud::Ptr debug_cloud_; 72 | LoopClosureConfig config_; 73 | 74 | rclcpp::Logger logger_; 75 | 76 | std::chrono::steady_clock::time_point last_success_icp_time_; 77 | bool has_success_icp_time_ = false; 78 | 79 | public: 80 | explicit LoopClosure(const LoopClosureConfig &config, const rclcpp::Logger &logger); 81 | ~LoopClosure(); 82 | double calculateDistance(const Eigen::Matrix4d &pose1, const Eigen::Matrix4d &pose2); 83 | 84 | LoopCandidates getLoopCandidatesFromQuery(const PoseGraphNode &query_frame, 85 | const std::vector &keyframes); 86 | 87 | LoopCandidate getClosestCandidate(const LoopCandidates &candidates); 88 | 89 | LoopIdxPairs fetchClosestLoopCandidate(const PoseGraphNode &query_frame, 90 | const std::vector &keyframes); 91 | 92 | LoopIdxPairs fetchLoopCandidates(const PoseGraphNode &query_frame, 93 | const std::vector &keyframes, 94 | const size_t num_max_candidates = 3, 95 | const double reliable_window_sec = 30); 96 | 97 | NodePair setSrcAndTgtCloud(const std::vector &keyframes, 98 | const size_t src_idx, 99 | const size_t tgt_idx, 100 | const size_t num_submap_keyframes, 101 | const double voxel_res, 102 | const bool enable_global_registration); 103 | 104 | void setSrcAndTgtCloud(const pcl::PointCloud &src_cloud, 105 | const pcl::PointCloud &tgt_cloud); 106 | 107 | RegOutput icpAlignment(const pcl::PointCloud &src, 108 | const pcl::PointCloud &tgt); 109 | 110 | RegOutput coarseToFineAlignment(const pcl::PointCloud &src, 111 | const pcl::PointCloud &tgt); 112 | 113 | RegOutput performLoopClosure(const PoseGraphNode &query_keyframe, 114 | const std::vector &keyframes); 115 | 116 | RegOutput performLoopClosure(const std::vector &keyframes, 117 | const size_t query_idx, 118 | const size_t match_idx); 119 | 120 | pcl::PointCloud getSourceCloud(); 121 | pcl::PointCloud getTargetCloud(); 122 | pcl::PointCloud getCoarseAlignedCloud(); 123 | pcl::PointCloud getFinalAlignedCloud(); 124 | pcl::PointCloud getDebugCloud(); 125 | }; 126 | } // namespace kiss_matcher 127 | #endif // KISS_MATCHER_LOOP_CLOSURE_H 128 | -------------------------------------------------------------------------------- /ros/include/slam/loop_detector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_LOOP_DETECTOR_H 4 | #define KISS_MATCHER_LOOP_DETECTOR_H 5 | 6 | ///// C++ common headers 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "slam/loop_types.hpp" 23 | #include "slam/pose_graph_node.hpp" 24 | #include "slam/utils.hpp" 25 | 26 | #define LOOP_CANDIDATE_NOT_FOUND -1 27 | 28 | namespace kiss_matcher { 29 | 30 | struct LoopDetectorConfig { 31 | bool verbose_ = false; 32 | }; 33 | 34 | class LoopDetector { 35 | private: 36 | LoopDetectorConfig config_; 37 | rclcpp::Logger logger_; 38 | 39 | public: 40 | explicit LoopDetector(const LoopDetectorConfig &config, const rclcpp::Logger &logger); 41 | ~LoopDetector(); 42 | LoopIdxPairs fetchLoopCandidates(const PoseGraphNode &query_frame, 43 | const std::vector &keyframes); 44 | }; 45 | } // namespace kiss_matcher 46 | #endif // KISS_MATCHER_LOOP_DETECTOR_H 47 | -------------------------------------------------------------------------------- /ros/include/slam/loop_types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_LOOP_TYPES_H 4 | #define KISS_MATCHER_LOOP_TYPES_H 5 | 6 | #include // for size_t 7 | #include // for std::numeric_limits 8 | #include 9 | #include 10 | 11 | struct LoopCandidate { 12 | bool found_ = false; 13 | size_t idx_ = std::numeric_limits::max(); 14 | double distance_ = std::numeric_limits::max(); 15 | }; 16 | 17 | using LoopCandidates = std::vector; 18 | 19 | using LoopIdxPair = std::pair; 20 | 21 | using LoopIdxPairs = std::vector; 22 | 23 | #endif // KISS_MATCHER_LOOP_TYPES_H 24 | -------------------------------------------------------------------------------- /ros/include/slam/pose_graph_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_POSE_GRAPH_MANAGER_H 4 | #define KISS_MATCHER_POSE_GRAPH_MANAGER_H 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | // message_filters in ROS2 34 | #include "message_filters/subscriber.h" 35 | #include "message_filters/sync_policies/approximate_time.h" 36 | #include "message_filters/synchronizer.h" 37 | 38 | // TF2 39 | #include 40 | #include 41 | #include 42 | // #include 43 | // #include 44 | // #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #include "../tictoc.hpp" 57 | #include "slam/loop_closure.h" 58 | #include "slam/loop_detector.h" 59 | #include "slam/pose_graph_node.hpp" 60 | #include "slam/utils.hpp" 61 | 62 | // #include 63 | 64 | namespace fs = std::filesystem; 65 | using namespace std::chrono; 66 | typedef message_filters::sync_policies::ApproximateTime 68 | NodeSyncPolicy; 69 | 70 | class PoseGraphManager : public rclcpp::Node { 71 | public: 72 | PoseGraphManager() = delete; 73 | explicit PoseGraphManager(const rclcpp::NodeOptions &options); 74 | ~PoseGraphManager(); 75 | 76 | private: 77 | void appendKeyframePose(const kiss_matcher::PoseGraphNode &node); 78 | 79 | void callbackNode(const nav_msgs::msg::Odometry::ConstSharedPtr &odom_msg, 80 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr &scan_msg); 81 | /**** Timer functions ****/ 82 | // void loopPubTimerFunc(); 83 | void buildMap(); 84 | void detectLoopClosureByLoopDetector(); 85 | void detectLoopClosureByNNSearch(); 86 | 87 | void visualizeCurrentData(const Eigen::Matrix4d ¤t_odom, 88 | const rclcpp::Time ×tamp, 89 | const std::string &frame_id); 90 | void visualizePoseGraph(); 91 | 92 | void performRegistration(); 93 | 94 | void visualizeLoopClosureClouds(); 95 | 96 | visualization_msgs::msg::Marker visualizeLoopMarkers(const gtsam::Values &corrected_poses) const; 97 | visualization_msgs::msg::Marker visualizeLoopDetectionRadius( 98 | const geometry_msgs::msg::Point &latest_position) const; 99 | 100 | bool checkIfKeyframe(const kiss_matcher::PoseGraphNode &query_node, 101 | const kiss_matcher::PoseGraphNode &latest_node); 102 | 103 | void saveFlagCallback(const std_msgs::msg::String::ConstSharedPtr &msg); 104 | 105 | std::string map_frame_; 106 | std::string base_frame_; 107 | std::string package_path_; 108 | std::string seq_name_; 109 | 110 | std::mutex realtime_pose_mutex_; 111 | std::mutex keyframes_mutex_; 112 | std::mutex graph_mutex_; 113 | std::mutex lc_mutex_; 114 | std::mutex vis_mutex_; 115 | 116 | Eigen::Matrix4d last_corrected_pose_ = Eigen::Matrix4d::Identity(); 117 | Eigen::Matrix4d odom_delta_ = Eigen::Matrix4d::Identity(); 118 | kiss_matcher::PoseGraphNode current_frame_; 119 | std::vector keyframes_; 120 | 121 | bool is_initialized_ = false; 122 | bool loop_closure_added_ = false; 123 | bool need_map_update_ = false; 124 | bool need_graph_vis_update_ = false; 125 | bool need_lc_cloud_vis_update_ = false; 126 | 127 | std::shared_ptr isam_handler_ = nullptr; 128 | gtsam::NonlinearFactorGraph gtsam_graph_; 129 | gtsam::Values init_esti_; 130 | gtsam::Values corrected_esti_; 131 | 132 | double keyframe_thr_; 133 | double scan_voxel_res_; 134 | double map_voxel_res_; 135 | double save_voxel_res_; 136 | double loop_pub_delayed_time_; 137 | double loop_detection_radius_; // Only for visualization 138 | int sub_key_num_; 139 | 140 | size_t succeeded_query_idx_; 141 | std::vector> vis_loop_edges_; 142 | // pose_graph_tools_msgs::msg::PoseGraph loop_msgs_; 143 | std::queue loop_idx_pair_queue_; 144 | 145 | kiss_matcher::TicToc timer_; 146 | 147 | std::unique_ptr tf_broadcaster_; 148 | 149 | pcl::PointCloud odoms_, corrected_odoms_; 150 | nav_msgs::msg::Path odom_path_, corrected_path_; 151 | 152 | bool store_voxelized_scan_ = false; 153 | 154 | bool save_map_bag_ = false; 155 | bool save_map_pcd_ = false; 156 | bool save_in_kitti_format_ = false; 157 | double last_lc_time_ = 0.0; 158 | 159 | std::shared_ptr loop_closure_; 160 | 161 | // NOTE(hlim): We do not provide a loop detector implementation directly, 162 | // but you can plug in your own detector via this interface. 163 | std::shared_ptr loop_detector_; 164 | 165 | pcl::PointCloud::Ptr map_cloud_; 166 | 167 | // ROS2 interface 168 | rclcpp::Publisher::SharedPtr corrected_path_pub_; 169 | rclcpp::Publisher::SharedPtr path_pub_; 170 | 171 | rclcpp::Publisher::SharedPtr scan_pub_; 172 | rclcpp::Publisher::SharedPtr map_pub_; 173 | rclcpp::Publisher::SharedPtr loop_detection_pub_; 174 | rclcpp::Publisher::SharedPtr loop_detection_radius_pub_; 175 | rclcpp::Publisher::SharedPtr realtime_pose_pub_; 176 | rclcpp::Publisher::SharedPtr debug_src_pub_; 177 | rclcpp::Publisher::SharedPtr debug_tgt_pub_; 178 | rclcpp::Publisher::SharedPtr debug_coarse_aligned_pub_; 179 | rclcpp::Publisher::SharedPtr debug_fine_aligned_pub_; 180 | rclcpp::Publisher::SharedPtr debug_cloud_pub_; 181 | 182 | rclcpp::Subscription::SharedPtr sub_save_flag_; 183 | 184 | // rclcpp::Publisher::SharedPtr loop_closures_pub_; 185 | 186 | // message_filters 187 | std::shared_ptr> sub_odom_; 188 | std::shared_ptr> sub_scan_; 189 | std::shared_ptr> sub_node_; 190 | 191 | // Timers 192 | rclcpp::TimerBase::SharedPtr hydra_loop_timer_; 193 | rclcpp::TimerBase::SharedPtr map_timer_; 194 | rclcpp::TimerBase::SharedPtr loop_detector_timer_; 195 | rclcpp::TimerBase::SharedPtr loop_nnsearch_timer_; 196 | rclcpp::TimerBase::SharedPtr graph_vis_timer_; 197 | rclcpp::TimerBase::SharedPtr lc_reg_timer_; 198 | rclcpp::TimerBase::SharedPtr lc_vis_timer_; 199 | }; 200 | 201 | #endif // KISS_MATCHER_POSE_GRAPH_MANAGER_H 202 | -------------------------------------------------------------------------------- /ros/include/slam/pose_graph_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_POSE_GRAPH_NODE_HPP 4 | #define KISS_MATCHER_POSE_GRAPH_NODE_HPP 5 | 6 | #include 7 | 8 | #include "slam/utils.hpp" 9 | 10 | namespace kiss_matcher { 11 | 12 | struct PoseGraphNode { 13 | pcl::PointCloud scan_; 14 | pcl::PointCloud voxelized_scan_; // Used for map visualization 15 | Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(); 16 | Eigen::Matrix4d pose_corrected_ = Eigen::Matrix4d::Identity(); 17 | double timestamp_; 18 | size_t idx_; 19 | bool nnsearch_processed_ = false; 20 | bool loop_detector_processed_ = false; 21 | 22 | PoseGraphNode() {} 23 | inline PoseGraphNode(const nav_msgs::msg::Odometry &odom, 24 | const sensor_msgs::msg::PointCloud2 &scan, 25 | const size_t idx, 26 | const double voxel_size, 27 | const bool store_voxelized_scan = false, 28 | const bool is_wrt_lidar_frame = false) { 29 | tf2::Quaternion q; 30 | q.setX(odom.pose.pose.orientation.x); 31 | q.setY(odom.pose.pose.orientation.y); 32 | q.setZ(odom.pose.pose.orientation.z); 33 | q.setW(odom.pose.pose.orientation.w); 34 | 35 | tf2::Matrix3x3 rot_tf(q); 36 | Eigen::Matrix3d rot; 37 | matrixTF2ToEigen(rot_tf, rot); 38 | 39 | pose_.block<3, 3>(0, 0) = rot; 40 | pose_(0, 3) = odom.pose.pose.position.x; 41 | pose_(1, 3) = odom.pose.pose.position.y; 42 | pose_(2, 3) = odom.pose.pose.position.z; 43 | 44 | // This will be updated after pose graph optimization 45 | pose_corrected_ = pose_; 46 | 47 | pcl::PointCloud scan_tmp; 48 | pcl::fromROSMsg(scan, scan_tmp); 49 | 50 | if (store_voxelized_scan) { 51 | auto voxelized_scan = *voxelize(scan_tmp, voxel_size); 52 | scan_tmp = voxelized_scan; 53 | } 54 | if (is_wrt_lidar_frame) { 55 | scan_ = scan_tmp; 56 | } else { 57 | pcl::PointCloud pcd_wrt_lidar_frame = transformPcd(scan_tmp, pose_.inverse()); 58 | scan_ = pcd_wrt_lidar_frame; 59 | } 60 | 61 | // ROS2 stamp.sec / stamp.nanosec 62 | timestamp_ = odom.header.stamp.sec + 1e-9 * odom.header.stamp.nanosec; 63 | idx_ = idx; 64 | } 65 | }; 66 | } // namespace kiss_matcher 67 | 68 | #endif // KISS_MATCHER_POSE_GRAPH_NODE_HPP 69 | -------------------------------------------------------------------------------- /ros/include/slam/utils.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifndef KISS_MATCHER_UTILS_HPP 4 | #define KISS_MATCHER_UTILS_HPP 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | using PointType = pcl::PointXYZI; 35 | 36 | namespace kiss_matcher { 37 | inline void matrixEigenToTF2(const Eigen::Matrix3d &in, tf2::Matrix3x3 &out) { 38 | out.setValue( 39 | in(0, 0), in(0, 1), in(0, 2), in(1, 0), in(1, 1), in(1, 2), in(2, 0), in(2, 1), in(2, 2)); 40 | } 41 | 42 | inline void matrixTF2ToEigen(const tf2::Matrix3x3 &in, Eigen::Matrix3d &out) { 43 | out << in[0][0], in[0][1], in[0][2], in[1][0], in[1][1], in[1][2], in[2][0], in[2][1], in[2][2]; 44 | } 45 | 46 | inline pcl::PointCloud::Ptr voxelize(const pcl::PointCloud &cloud, 47 | float voxel_res) { 48 | pcl::VoxelGrid voxelgrid; 49 | voxelgrid.setLeafSize(voxel_res, voxel_res, voxel_res); 50 | 51 | pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud); 52 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 53 | 54 | cloud_ptr->reserve(cloud.size()); 55 | cloud_out->reserve(cloud.size()); 56 | *cloud_ptr = cloud; 57 | 58 | voxelgrid.setInputCloud(cloud_ptr); 59 | voxelgrid.filter(*cloud_out); 60 | return cloud_out; 61 | } 62 | 63 | inline pcl::PointCloud::Ptr voxelize(const pcl::PointCloud::Ptr &cloud, 64 | float voxel_res) { 65 | pcl::VoxelGrid voxelgrid; 66 | voxelgrid.setLeafSize(voxel_res, voxel_res, voxel_res); 67 | 68 | pcl::PointCloud::Ptr cloud_out(new pcl::PointCloud); 69 | cloud_out->reserve(cloud->size()); 70 | 71 | voxelgrid.setInputCloud(cloud); 72 | voxelgrid.filter(*cloud_out); 73 | return cloud_out; 74 | } 75 | 76 | inline gtsam::Pose3 eigenToGtsam(const Eigen::Matrix4d &pose) { 77 | tf2::Matrix3x3 mat_tf; 78 | matrixEigenToTF2(pose.block<3, 3>(0, 0), mat_tf); 79 | 80 | double roll, pitch, yaw; 81 | mat_tf.getRPY(roll, pitch, yaw); 82 | 83 | return gtsam::Pose3(gtsam::Rot3::RzRyRx(roll, pitch, yaw), 84 | gtsam::Point3(pose(0, 3), pose(1, 3), pose(2, 3))); 85 | } 86 | 87 | inline Eigen::Matrix4d gtsamToEigen(const gtsam::Pose3 &pose) { 88 | Eigen::Matrix4d output = Eigen::Matrix4d::Identity(); 89 | 90 | tf2::Quaternion quat; 91 | quat.setRPY(pose.rotation().roll(), pose.rotation().pitch(), pose.rotation().yaw()); 92 | 93 | tf2::Matrix3x3 tf_rot(quat); 94 | Eigen::Matrix3d rot; 95 | matrixTF2ToEigen(tf_rot, rot); 96 | 97 | output.block<3, 3>(0, 0) = rot; 98 | output(0, 3) = pose.translation().x(); 99 | output(1, 3) = pose.translation().y(); 100 | output(2, 3) = pose.translation().z(); 101 | 102 | return output; 103 | } 104 | 105 | inline geometry_msgs::msg::PoseStamped eigenToPoseStamped(const Eigen::Matrix4d &pose, 106 | const std::string &frame_id = "map") { 107 | tf2::Matrix3x3 mat_tf; 108 | matrixEigenToTF2(pose.block<3, 3>(0, 0), mat_tf); 109 | 110 | double roll, pitch, yaw; 111 | mat_tf.getRPY(roll, pitch, yaw); 112 | 113 | tf2::Quaternion quat; 114 | quat.setRPY(roll, pitch, yaw); 115 | 116 | geometry_msgs::msg::PoseStamped msg; 117 | msg.header.frame_id = frame_id; 118 | msg.pose.position.x = pose(0, 3); 119 | msg.pose.position.y = pose(1, 3); 120 | msg.pose.position.z = pose(2, 3); 121 | msg.pose.orientation.w = quat.w(); 122 | msg.pose.orientation.x = quat.x(); 123 | msg.pose.orientation.y = quat.y(); 124 | msg.pose.orientation.z = quat.z(); 125 | 126 | return msg; 127 | } 128 | 129 | inline geometry_msgs::msg::Pose egienToGeoPose(const Eigen::Matrix4d &pose) { 130 | tf2::Matrix3x3 mat_tf; 131 | matrixEigenToTF2(pose.block<3, 3>(0, 0), mat_tf); 132 | 133 | double roll, pitch, yaw; 134 | mat_tf.getRPY(roll, pitch, yaw); 135 | 136 | tf2::Quaternion quat; 137 | quat.setRPY(roll, pitch, yaw); 138 | 139 | geometry_msgs::msg::Pose msg; 140 | msg.position.x = pose(0, 3); 141 | msg.position.y = pose(1, 3); 142 | msg.position.z = pose(2, 3); 143 | msg.orientation.w = quat.w(); 144 | msg.orientation.x = quat.x(); 145 | msg.orientation.y = quat.y(); 146 | msg.orientation.z = quat.z(); 147 | 148 | return msg; 149 | } 150 | 151 | inline geometry_msgs::msg::PoseStamped gtsamToPoseStamped(const gtsam::Pose3 &pose, 152 | const std::string &frame_id = "map") { 153 | double roll = pose.rotation().roll(); 154 | double pitch = pose.rotation().pitch(); 155 | double yaw = pose.rotation().yaw(); 156 | 157 | tf2::Quaternion quat; 158 | quat.setRPY(roll, pitch, yaw); 159 | 160 | geometry_msgs::msg::PoseStamped msg; 161 | msg.header.frame_id = frame_id; 162 | msg.pose.position.x = pose.translation().x(); 163 | msg.pose.position.y = pose.translation().y(); 164 | msg.pose.position.z = pose.translation().z(); 165 | msg.pose.orientation.w = quat.w(); 166 | msg.pose.orientation.x = quat.x(); 167 | msg.pose.orientation.y = quat.y(); 168 | msg.pose.orientation.z = quat.z(); 169 | 170 | return msg; 171 | } 172 | 173 | inline rclcpp::Time toRclcppTime(const double timestamp) { 174 | int32_t sec = static_cast(timestamp); 175 | uint32_t nanosec = static_cast((timestamp - sec) * 1e9); 176 | return rclcpp::Time(sec, nanosec, RCL_ROS_TIME); 177 | } 178 | 179 | template 180 | inline sensor_msgs::msg::PointCloud2 toROSMsg( 181 | const pcl::PointCloud &cloud, 182 | const std::string &frame_id = "map", 183 | const std::optional &stamp = std::nullopt) { 184 | sensor_msgs::msg::PointCloud2 cloud_ros; 185 | pcl::toROSMsg(cloud, cloud_ros); 186 | cloud_ros.header.frame_id = frame_id; 187 | 188 | if (stamp.has_value()) { 189 | cloud_ros.header.stamp = stamp.value(); 190 | } 191 | 192 | return cloud_ros; 193 | } 194 | 195 | template 196 | inline pcl::PointCloud transformPcd(const pcl::PointCloud &cloud_in, 197 | const Eigen::Matrix4d &pose) { 198 | if (cloud_in.empty()) { 199 | return cloud_in; 200 | } 201 | pcl::PointCloud cloud_out; 202 | pcl::transformPointCloud(cloud_in, cloud_out, pose); 203 | return cloud_out; 204 | } 205 | 206 | template 207 | inline std::vector convertCloudToVec(const pcl::PointCloud &cloud) { 208 | std::vector vec; 209 | vec.reserve(cloud.size()); 210 | for (const auto &pt : cloud.points) { 211 | if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z)) continue; 212 | vec.emplace_back(pt.x, pt.y, pt.z); 213 | } 214 | return vec; 215 | } 216 | } // namespace kiss_matcher 217 | #endif // KISS_MATCHER_UTILS_HPP 218 | -------------------------------------------------------------------------------- /ros/include/tictoc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace kiss_matcher { 8 | class TicToc { 9 | public: 10 | TicToc() { tic(); } 11 | 12 | // Reset the timer 13 | void tic() { start_ = std::chrono::steady_clock::now(); } 14 | 15 | // Return elapsed time in milliseconds or seconds 16 | double toc(const std::string& unit = "msec") const { 17 | const auto end = std::chrono::steady_clock::now(); 18 | std::chrono::duration elapsed_seconds = end - start_; 19 | 20 | if (unit == "msec") { 21 | return elapsed_seconds.count() * 1000.0; 22 | } else if (unit == "sec") { 23 | return elapsed_seconds.count(); 24 | } else { 25 | throw std::invalid_argument("Unsupported unit: " + unit + ". Use 'msec' or 'sec'."); 26 | } 27 | } 28 | 29 | private: 30 | std::chrono::time_point start_; 31 | }; 32 | } // namespace kiss_matcher 33 | -------------------------------------------------------------------------------- /ros/launch/inter_frame_alignment.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | - arg: { name: start_rviz, default: "true", description: "Start RViz automatically" } 4 | - arg: { name: namespace, default: "inter_frame", description: "Namespace for node" } 5 | 6 | - arg: { name: rviz_path, default: $(find-pkg-share kiss_matcher_ros)/rviz/kimera_multi_initial_alignment.rviz } 7 | - arg: { name: config_path, default: $(find-pkg-share kiss_matcher_ros)/config/alignment_config.yaml } 8 | 9 | - arg: { name: source_topic, default: "/source_cloud", description: "Input source cloud topic" } 10 | - arg: { name: target_topic, default: "/target_cloud", description: "Input target cloud topic" } 11 | 12 | - arg: { name: source_frame, default: "source_map", description: "Frame ID of the source cloud" } 13 | - arg: { name: target_frame, default: "target_map", description: "Frame ID of the target cloud" } 14 | 15 | - arg: { name: frame_update_hz, default: "0.1", description: "How often to run alignment (Hz)" } 16 | 17 | - node: 18 | namespace: $(var namespace) 19 | pkg: kiss_matcher_ros 20 | exec: inter_frame_alignment 21 | name: inter_frame_aligner 22 | output: screen 23 | on_exit: shutdown 24 | 25 | remap: 26 | - { from: 'source', to: $(var source_topic) } 27 | - { from: 'target', to: $(var target_topic) } 28 | 29 | param: 30 | - name: "source_frame" 31 | value: "$(var source_frame)" 32 | - name: "target_frame" 33 | value: "$(var target_frame)" 34 | - name: "frame_update_hz" 35 | value: "$(var frame_update_hz)" 36 | 37 | # NOTE(hlim): it overwrite variables 38 | - from: $(var config_path) 39 | 40 | - node: 41 | if: $(var start_rviz) 42 | pkg: rviz2 43 | exec: rviz2 44 | name: "inter_frame_rviz" 45 | launch-prefix: "nice" 46 | output: "screen" 47 | args: > 48 | -d $(var rviz_path) 49 | -------------------------------------------------------------------------------- /ros/launch/run_kiss_matcher_sam.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | launch: 3 | # In arg, all the default value should be string 4 | - arg: {name: start_rviz, default: "true"} 5 | - arg: {name: namespace, default: "km_sam", description: KISS-Matcher-SAM (KM-SAM)} 6 | 7 | - arg: {name: rviz_path, default: $(find-pkg-share kiss_matcher_ros)/rviz/kiss_matcher_sam.rviz} 8 | - arg: {name: config_path, default: $(find-pkg-share kiss_matcher_ros)/config/slam_config.yaml} 9 | 10 | - arg: {name: map_frame , default: 'map' , description: Global map frame ID} 11 | - arg: {name: base_frame, default: 'base', description: Robot's base frame ID} 12 | 13 | - arg: {name: odom_topic, default: "/odometry"} 14 | - arg: {name: scan_topic, default: "/cloud_registered"} 15 | 16 | - node: 17 | namespace: $(var namespace) 18 | pkg: kiss_matcher_ros 19 | exec: kiss_matcher_sam 20 | name: kiss_matcher_sam 21 | output: screen 22 | on_exit: shutdown 23 | 24 | remap: 25 | # Subscriber topics 26 | - { from: '/cloud', to: $(var scan_topic) } 27 | - { from: '/odom', to: $(var odom_topic) } 28 | 29 | param: 30 | - name: "map_frame" 31 | value: "$(var map_frame)" 32 | - name: "base_frame" 33 | value: "$$(var base_frame)" 34 | 35 | - from: $(var config_path) 36 | 37 | - node: 38 | if: $(var start_rviz) 39 | pkg: rviz2 40 | exec: rviz2 41 | name: "km_sam_rviz" 42 | launch-prefix: "nice" 43 | output: "screen" 44 | args: > 45 | -d $(var rviz_path) 46 | -------------------------------------------------------------------------------- /ros/launch/slam_in_kimera_multi.launch.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | # TODO(hlim): Need to further consider how to support both private and public Kimera-Multi data. 3 | # Currently, it requires static tfs, i.e., 4 | # `ros2 launch 12_08__static_tfs.launch.yaml ` 5 | launch: 6 | - arg: { name: scene_id, default: "acl_jackal2" } 7 | - arg: { name: robot_name, default: "acl_jackal2" } 8 | 9 | - arg: { name: enable_odom_rviz, default: "false" } 10 | - arg: { name: enable_kmsam_rviz, default: "true" } 11 | 12 | - arg: { name: sensor_type, default: "velodyne16" } 13 | 14 | - include: 15 | # Please make sure to build SPARK-Fast-LIO first (see https://github.com/MIT-SPARK/spark-fast-lio). 16 | file: $(find-pkg-share spark_fast_lio)/launch/mapping_kimera_multi.launch.yaml 17 | arg: 18 | - { name: scene_id, value: $(var scene_id) } 19 | - { name: robot_name, value: $(var robot_name) } 20 | - { name: start_rviz, value: $(var enable_odom_rviz) } 21 | 22 | - include: 23 | file: $(find-pkg-share kiss_matcher_ros)/launch/run_kiss_matcher_sam.launch.yaml 24 | arg: 25 | - { name: odom_topic, value: "/$(var scene_id)/odometry" } 26 | - { name: scan_topic, value: "/$(var scene_id)/cloud_registered" } 27 | - { name: map_frame, value: "$(var scene_id)/map" } 28 | - { name: base_frame, value: "$(var scene_id)/base_link" } 29 | - { name: start_rviz, value: $(var enable_kmsam_rviz) } 30 | - { name: namespace, value: $(var scene_id) } 31 | 32 | - { name: rviz_path, value: $(find-pkg-share kiss_matcher_ros)/rviz/slam_in_kimera_multi.rviz } 33 | - { name: config_path, value: $(find-pkg-share kiss_matcher_ros)/config/kimera_multi/$(var sensor_type).yaml } 34 | -------------------------------------------------------------------------------- /ros/launch/visualizer_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import launch 4 | import launch_ros.actions 5 | 6 | 7 | def generate_launch_description(): 8 | params_file = os.path.join(os.getcwd(), "src/KISS-Matcher/ros/config/params.yaml") 9 | 10 | return launch.LaunchDescription( 11 | [ 12 | launch_ros.actions.Node( 13 | package="kiss_matcher_ros", 14 | executable="registration_visualizer", 15 | name="registration_visualizer", 16 | parameters=[params_file], 17 | ), 18 | launch_ros.actions.Node( 19 | package="rviz2", 20 | executable="rviz2", 21 | name="rviz2", 22 | arguments=[ 23 | "-d", 24 | os.path.join( 25 | os.getcwd(), "src/KISS-Matcher/ros/rviz/kiss_matcher_reg.rviz" 26 | ), 27 | ], 28 | output="screen", 29 | ), 30 | ] 31 | ) 32 | -------------------------------------------------------------------------------- /ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kiss_matcher_ros 4 | 0.1.0 5 | PointCloud Visualizer for ROS2 6 | Hyungtae Lim 7 | Daebeom Kim 8 | 9 | MIT license 10 | 11 | rcutils 12 | rclcpp 13 | rclcpp_components 14 | geometry_msgs 15 | nav_msgs 16 | std_msgs 17 | sensor_msgs 18 | visualization_msgs 19 | tf2_ros 20 | tf2_eigen 21 | tf2_geometry_msgs 22 | tf2_sensor_msgs 23 | pcl_ros 24 | pcl_conversions 25 | gtsam 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /ros/rviz/kiss_matcher_reg.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 725 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: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 10 33 | Class: rviz_default_plugins/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.029999999329447746 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 1000 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Autocompute Intensity Bounds: true 51 | Autocompute Value Bounds: 52 | Max Value: 10 53 | Min Value: -10 54 | Value: true 55 | Axis: Z 56 | Channel Name: intensity 57 | Class: rviz_default_plugins/PointCloud2 58 | Color: 255; 183; 0 59 | Color Transformer: FlatColor 60 | Decay Time: 0 61 | Enabled: true 62 | Invert Rainbow: false 63 | Max Color: 255; 255; 255 64 | Max Intensity: 4096 65 | Min Color: 0; 0; 0 66 | Min Intensity: 0 67 | Name: src_pcd 68 | Position Transformer: XYZ 69 | Selectable: true 70 | Size (Pixels): 3 71 | Size (m): 0.009999999776482582 72 | Style: Points 73 | Topic: 74 | Depth: 5 75 | Durability Policy: Volatile 76 | Filter size: 10 77 | History Policy: Keep Last 78 | Reliability Policy: Reliable 79 | Value: /src_cloud 80 | Use Fixed Frame: true 81 | Use rainbow: true 82 | Value: true 83 | - Alpha: 1 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: 10 87 | Min Value: -10 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz_default_plugins/PointCloud2 92 | Color: 0; 229; 255 93 | Color Transformer: FlatColor 94 | Decay Time: 0 95 | Enabled: true 96 | Invert Rainbow: false 97 | Max Color: 255; 255; 255 98 | Max Intensity: 4096 99 | Min Color: 0; 0; 0 100 | Min Intensity: 0 101 | Name: tgt_pcd 102 | Position Transformer: XYZ 103 | Selectable: true 104 | Size (Pixels): 3 105 | Size (m): 0.009999999776482582 106 | Style: Points 107 | Topic: 108 | Depth: 5 109 | Durability Policy: Volatile 110 | Filter size: 10 111 | History Policy: Keep Last 112 | Reliability Policy: Reliable 113 | Value: /tgt_cloud 114 | Use Fixed Frame: true 115 | Use rainbow: true 116 | Value: true 117 | Enabled: true 118 | Global Options: 119 | Background Color: 255; 255; 255 120 | Fixed Frame: map 121 | Frame Rate: 30 122 | Name: root 123 | Tools: 124 | - Class: rviz_default_plugins/Interact 125 | Hide Inactive Objects: true 126 | - Class: rviz_default_plugins/MoveCamera 127 | - Class: rviz_default_plugins/Select 128 | - Class: rviz_default_plugins/FocusCamera 129 | - Class: rviz_default_plugins/Measure 130 | Line color: 128; 128; 0 131 | - Class: rviz_default_plugins/SetInitialPose 132 | Covariance x: 0.25 133 | Covariance y: 0.25 134 | Covariance yaw: 0.06853891909122467 135 | Topic: 136 | Depth: 5 137 | Durability Policy: Volatile 138 | History Policy: Keep Last 139 | Reliability Policy: Reliable 140 | Value: /initialpose 141 | - Class: rviz_default_plugins/SetGoal 142 | Topic: 143 | Depth: 5 144 | Durability Policy: Volatile 145 | History Policy: Keep Last 146 | Reliability Policy: Reliable 147 | Value: /goal_pose 148 | - Class: rviz_default_plugins/PublishPoint 149 | Single click: true 150 | Topic: 151 | Depth: 5 152 | Durability Policy: Volatile 153 | History Policy: Keep Last 154 | Reliability Policy: Reliable 155 | Value: /clicked_point 156 | Transformation: 157 | Current: 158 | Class: rviz_default_plugins/TF 159 | Value: true 160 | Views: 161 | Current: 162 | Class: rviz_default_plugins/Orbit 163 | Distance: 169.4262237548828 164 | Enable Stereo Rendering: 165 | Stereo Eye Separation: 0.05999999865889549 166 | Stereo Focal Distance: 1 167 | Swap Stereo Eyes: false 168 | Value: false 169 | Focal Point: 170 | X: 6.266591548919678 171 | Y: 15.620454788208008 172 | Z: 16.447492599487305 173 | Focal Shape Fixed Size: true 174 | Focal Shape Size: 0.05000000074505806 175 | Invert Z Axis: false 176 | Name: Current View 177 | Near Clip Distance: 0.009999999776482582 178 | Pitch: 0.7247968912124634 179 | Target Frame: 180 | Value: Orbit (rviz) 181 | Yaw: 3.6735754013061523 182 | Saved: ~ 183 | Window Geometry: 184 | Displays: 185 | collapsed: false 186 | Height: 1016 187 | Hide Left Dock: false 188 | Hide Right Dock: true 189 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000007380000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 190 | Selection: 191 | collapsed: false 192 | Time: 193 | collapsed: false 194 | Tool Properties: 195 | collapsed: false 196 | Views: 197 | collapsed: true 198 | Width: 1848 199 | X: 1152 200 | Y: 175 201 | -------------------------------------------------------------------------------- /ros/src/slam/loop_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "slam/loop_detector.h" 2 | 3 | using namespace kiss_matcher; 4 | 5 | LoopDetector::LoopDetector(const LoopDetectorConfig &config, const rclcpp::Logger &logger) 6 | : config_(config), logger_(logger) { 7 | // Fill your declaration here 8 | } 9 | 10 | LoopDetector::~LoopDetector() {} 11 | 12 | LoopIdxPairs LoopDetector::fetchLoopCandidates(const PoseGraphNode &query_frame, 13 | const std::vector &keyframes) { 14 | LoopIdxPairs loop_idx_pairs; 15 | //------------------------------------------------------------ 16 | // Implement your loop detector here 17 | //------------------------------------------------------------ 18 | return loop_idx_pairs; 19 | } 20 | -------------------------------------------------------------------------------- /shellscripts/install_robin.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -e 3 | 4 | command_exists() { 5 | command -v "$@" >/dev/null 2>&1 6 | } 7 | 8 | user_can_sudo() { 9 | command_exists sudo || return 1 10 | ! LANG= sudo -n -v 2>&1 | grep -q "may not run sudo" 11 | } 12 | 13 | if user_can_sudo; then 14 | SUDO="sudo" 15 | else 16 | SUDO="" # To support docker environment 17 | fi 18 | 19 | if [ "$1" == "assume-yes" ]; then 20 | APT_CONFIRM="--assume-yes" 21 | else 22 | APT_CONFIRM="" 23 | fi 24 | 25 | $SUDO apt-get update -y 26 | 27 | # Automatically install the prerequisites 28 | # `libflann-dev` is required for KISS-Matcher 29 | $SUDO apt-get install gcc g++ build-essential libeigen3-dev python3-pip python3-dev cmake git ninja-build libflann-dev -y 30 | 31 | # Install ROBIN 32 | echo " 33 | ------------------------------------------------------------------------------- 34 | ------====--------========-----------------------========-------===-------===== 35 | --*##########----+##########*--------+=---------=###########---=###=-----####+- 36 | +####*====+####--+##======+###+-----=+*+--------=##+=====++##-*=###=---+####--- 37 | ###+--------=###-+##=-------+##+---==+**=-------=##=-------=##*=###=--*###+---- 38 | ###+-------------+##=--------##*--====***=------=##=--------##-=###==####=----- 39 | +############*=--+##=-------+##=-===---+++=-----=##=-------=##-=#######+------- 40 | ---=+***########-+##======*###+-===-----+++=----=##======+##*--=#########=----- 41 | --=----------*##-+##=######*---===-------+++=---=##=*####*=----=####*-*###*---- 42 | ###*--------+###-+##=---------===---------=++=--=##=-==+###----=###=---=####=-- 43 | -##############=-+##=--------================+=-=##=-----###+--=###=-----*###*- 44 | --=+########*=---+##=-------=========+=====++++==##=------+###*=###=------=#### 45 | -------------------------------------------------------------------------------- 46 | "" 47 | 48 | mkdir -p install && cd install 49 | git clone https://github.com/MIT-SPARK/ROBIN.git 50 | mkdir -p ROBIN/build && cd ROBIN/build 51 | 52 | # To generate `libpmc.a` 53 | # See https://github.com/jingnanshi/pmc/pull/2 54 | cmake -DPMC_BUILD_SHARED=OFF .. 55 | 56 | if make -j$(nproc); then 57 | echo "Complete to Build ROBIN successfully!" 58 | else 59 | echo "Build failed, try 'make' several times ..." 60 | exit 1 61 | fi 62 | 63 | echo "Applying 'sudo make install'. Enter password" 64 | $SUDO make install 65 | -------------------------------------------------------------------------------- /shellscripts/install_teaserpp.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -e 3 | 4 | command_exists() { 5 | command -v "$@" >/dev/null 2>&1 6 | } 7 | 8 | user_can_sudo() { 9 | command_exists sudo || return 1 10 | ! LANG= sudo -n -v 2>&1 | grep -q "may not run sudo" 11 | } 12 | 13 | if user_can_sudo; then 14 | SUDO="sudo" 15 | else 16 | SUDO="" # To support docker environment 17 | fi 18 | 19 | if [ "$1" == "assume-yes" ]; then 20 | APT_CONFIRM="--assume-yes" 21 | else 22 | APT_CONFIRM="" 23 | fi 24 | 25 | $SUDO apt-get update -y 26 | 27 | # Automatically install the prerequisites 28 | # `libflann-dev` is required for KISS-Matcher 29 | $SUDO apt-get install gcc g++ build-essential libeigen3-dev python3-pip python3-dev cmake git ninja-build libflann-dev -y 30 | 31 | # Install TEASER-plusplus 32 | echo " 33 | ------------------------------------------------------------------------------- 34 | ------====--------========-----------------------========-------===-------===== 35 | --*##########----+##########*--------+=---------=###########---=###=-----####+- 36 | +####*====+####--+##======+###+-----=+*+--------=##+=====++##-*=###=---+####--- 37 | ###+--------=###-+##=-------+##+---==+**=-------=##=-------=##*=###=--*###+---- 38 | ###+-------------+##=--------##*--====***=------=##=--------##-=###==####=----- 39 | +############*=--+##=-------+##=-===---+++=-----=##=-------=##-=#######+------- 40 | ---=+***########-+##======*###+-===-----+++=----=##======+##*--=#########=----- 41 | --=----------*##-+##=######*---===-------+++=---=##=*####*=----=####*-*###*---- 42 | ###*--------+###-+##=---------===---------=++=--=##=-==+###----=###=---=####=-- 43 | -##############=-+##=--------================+=-=##=-----###+--=###=-----*###*- 44 | --=+########*=---+##=-------=========+=====++++==##=------+###*=###=------=#### 45 | -------------------------------------------------------------------------------- 46 | " 47 | 48 | mkdir -p install && cd install 49 | git clone https://github.com/MIT-SPARK/TEASER-plusplus.git 50 | mkdir -p TEASER-plusplus/build && cd TEASER-plusplus/build 51 | cmake .. 52 | 53 | if make -j$(nproc); then 54 | echo "Complete to Build TEASER++ successfully!" 55 | else 56 | echo "Build failed, try 'make' several times ..." 57 | exit 1 58 | fi 59 | 60 | echo "Applying 'sudo make install'. Enter password" 61 | $SUDO make install 62 | --------------------------------------------------------------------------------