├── .clang-format ├── .flake8 ├── .github └── workflows │ ├── build-deploy.yml │ └── build-only.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── Doxyfile.in ├── ListPrepend.cmake ├── doxygenTheme.cmake ├── gtest.cmake └── pybind11.cmake ├── docs ├── examples.md ├── footer.html ├── header.html ├── images │ ├── snapshot.gif │ └── snapshot.png ├── installation.md ├── mainpage.md └── remarks.md ├── examples ├── cpp │ ├── CMakeLists.txt │ ├── README.md │ ├── data │ └── main.cpp ├── data │ ├── 1531883530.449377000.pcd │ ├── 1531883530.949817000.pcd │ └── README.md └── python │ ├── .gitignore │ ├── README.md │ ├── data │ ├── main.py │ ├── render_option.json │ └── requirements.txt ├── kcp ├── CMakeLists.txt ├── include │ └── kcp │ │ ├── common.hpp │ │ ├── keypoint.hpp │ │ ├── solver.hpp │ │ └── utility.hpp └── src │ ├── keypoint.cpp │ ├── solver.cpp │ └── utility.cpp ├── pyproject.toml └── python ├── CMakeLists.txt └── kcp └── wrapper.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: true 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: All 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: true 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: true 47 | ColumnLimit: 0 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 1000000 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Delimiters: [pb] 97 | Language: TextProto 98 | BasedOnStyle: google 99 | ReflowComments: true 100 | SortIncludes: true 101 | SortUsingDeclarations: true 102 | SpaceAfterCStyleCast: false 103 | SpaceAfterTemplateKeyword: true 104 | SpaceBeforeAssignmentOperators: true 105 | SpaceBeforeParens: ControlStatements 106 | SpaceInEmptyParentheses: false 107 | SpacesBeforeTrailingComments: 2 108 | SpacesInAngles: false 109 | SpacesInContainerLiterals: true 110 | SpacesInCStyleCastParentheses: false 111 | SpacesInParentheses: false 112 | SpacesInSquareBrackets: false 113 | Standard: Auto 114 | TabWidth: 8 115 | UseTab: Never 116 | ... 117 | 118 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 100 3 | ignore = D203 4 | select = C,E,F,W,B,B950,Q0 5 | extend-ignore = E203, E501, W503 6 | exclude = 7 | # No need to traverse our git directory 8 | .git, 9 | # There's no value in checking cache directories 10 | __pycache__, 11 | # The conf file is mostly autogenerated, ignore it 12 | docs/source/conf.py, 13 | # The old directory contains Flake8 2.0 14 | old, 15 | # This contains our built documentation 16 | build, 17 | # This contains builds of flake8 that we don't want to check 18 | dist, 19 | # Third-party packages 20 | third-party, 21 | max-complexity = 10 22 | inline-quotes = " 23 | -------------------------------------------------------------------------------- /.github/workflows/build-deploy.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | on: 3 | push: 4 | branches: 5 | - main 6 | 7 | jobs: 8 | build-and-deploy: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - uses: actions/checkout@v2 12 | 13 | - name: Set up Python 3.10 14 | uses: actions/setup-python@v1 15 | with: 16 | python-version: "3.10" 17 | 18 | - name: Install dependencies (1/3) (GCC, CMake, Eigen3, Doxygen, Graphviz) 19 | run: | 20 | sudo apt update 21 | sudo apt install -y g++ build-essential cmake libeigen3-dev doxygen graphviz 22 | 23 | - name: Install dependencies (2/3) (nanoflann) 24 | run: | 25 | git clone https://github.com/jlblancoc/nanoflann 26 | cd nanoflann 27 | mkdir build && cd build 28 | cmake .. -DNANOFLANN_BUILD_EXAMPLES=OFF -DNANOFLANN_BUILD_TESTS=OFF 29 | make 30 | sudo make install 31 | 32 | - name: Install dependencies (3/3) (TEASER++) 33 | run: | 34 | git clone https://github.com/MIT-SPARK/TEASER-plusplus 35 | cd TEASER-plusplus 36 | git checkout d79d0c67 37 | mkdir build && cd build 38 | cmake .. -DBUILD_TESTS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_DOC=OFF 39 | make 40 | sudo make install 41 | 42 | - name: Build the KCP library 43 | run: | 44 | mkdir build && cd build 45 | cmake .. -DKCP_BUILD_PYTHON_BINDING=ON -DPYTHON_EXECUTABLE=$(which python3) -DKCP_BUILD_DOC=ON 46 | make 47 | 48 | - if: ${{ github.ref == 'refs/heads/main' }} 49 | name: GitHub Pages action 50 | uses: peaceiris/actions-gh-pages@v3.5.9 51 | with: 52 | github_token: ${{ secrets.GITHUB_TOKEN }} 53 | publish_dir: ./build/docs/html 54 | -------------------------------------------------------------------------------- /.github/workflows/build-only.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | on: 3 | pull_request: 4 | branches: 5 | - main 6 | 7 | jobs: 8 | build-kcp: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - uses: actions/checkout@v2 12 | 13 | - name: Set up Python 3.10 14 | uses: actions/setup-python@v1 15 | with: 16 | python-version: "3.10" 17 | 18 | - name: Install dependencies (1/3) (GCC, CMake, Eigen3, Doxygen, Graphviz) 19 | run: | 20 | sudo apt update 21 | sudo apt install -y g++ build-essential cmake libeigen3-dev doxygen graphviz 22 | 23 | - name: Install dependencies (2/3) (nanoflann) 24 | run: | 25 | git clone https://github.com/jlblancoc/nanoflann 26 | cd nanoflann 27 | mkdir build && cd build 28 | cmake .. -DNANOFLANN_BUILD_EXAMPLES=OFF -DNANOFLANN_BUILD_TESTS=OFF 29 | make 30 | sudo make install 31 | 32 | - name: Install dependencies (3/3) (TEASER++) 33 | run: | 34 | git clone https://github.com/MIT-SPARK/TEASER-plusplus 35 | cd TEASER-plusplus 36 | git checkout d79d0c67 37 | mkdir build && cd build 38 | cmake .. -DBUILD_TESTS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_DOC=OFF 39 | make 40 | sudo make install 41 | 42 | - name: Build the KCP library 43 | run: | 44 | mkdir build && cd build 45 | cmake .. -DKCP_BUILD_PYTHON_BINDING=ON -DPYTHON_EXECUTABLE=$(which python3) -DKCP_BUILD_DOC=ON 46 | make 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ---------------------------------------------- C++ --------------------------------------------- # 2 | 3 | # Prerequisites 4 | *.d 5 | 6 | # Compiled Object files 7 | *.slo 8 | *.lo 9 | *.o 10 | *.obj 11 | 12 | # Precompiled Headers 13 | *.gch 14 | *.pch 15 | 16 | # Compiled Dynamic libraries 17 | *.so 18 | *.dylib 19 | *.dll 20 | 21 | # Fortran module files 22 | *.mod 23 | *.smod 24 | 25 | # Compiled Static libraries 26 | *.lai 27 | *.la 28 | *.a 29 | *.lib 30 | 31 | # Executables 32 | *.exe 33 | *.out 34 | *.app 35 | 36 | .ccls-cache 37 | .clangd 38 | 39 | # --------------------------------------------- CMake -------------------------------------------- # 40 | 41 | CMakeLists.txt.user 42 | CMakeCache.txt 43 | CMakeFiles 44 | CMakeScripts 45 | Testing 46 | Makefile 47 | cmake_install.cmake 48 | install_manifest.txt 49 | compile_commands.json 50 | CTestTestfile.cmake 51 | _deps 52 | 53 | build/ 54 | 55 | # -------------------------------------------- Python -------------------------------------------- # 56 | 57 | # Byte-compiled / optimized / DLL files 58 | __pycache__/ 59 | *.py[cod] 60 | *$py.class 61 | 62 | # C extensions 63 | *.so 64 | 65 | # Distribution / packaging 66 | .Python 67 | build/ 68 | develop-eggs/ 69 | dist/ 70 | downloads/ 71 | eggs/ 72 | .eggs/ 73 | lib/ 74 | lib64/ 75 | parts/ 76 | sdist/ 77 | var/ 78 | wheels/ 79 | share/python-wheels/ 80 | *.egg-info/ 81 | .installed.cfg 82 | *.egg 83 | MANIFEST 84 | 85 | # PyInstaller 86 | # Usually these files are written by a python script from a template 87 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 88 | *.manifest 89 | *.spec 90 | 91 | # Installer logs 92 | pip-log.txt 93 | pip-delete-this-directory.txt 94 | 95 | # Unit test / coverage reports 96 | htmlcov/ 97 | .tox/ 98 | .nox/ 99 | .coverage 100 | .coverage.* 101 | .cache 102 | nosetests.xml 103 | coverage.xml 104 | *.cover 105 | *.py,cover 106 | .hypothesis/ 107 | .pytest_cache/ 108 | cover/ 109 | 110 | # Translations 111 | *.mo 112 | *.pot 113 | 114 | # Django stuff: 115 | *.log 116 | local_settings.py 117 | db.sqlite3 118 | db.sqlite3-journal 119 | 120 | # Flask stuff: 121 | instance/ 122 | .webassets-cache 123 | 124 | # Scrapy stuff: 125 | .scrapy 126 | 127 | # Sphinx documentation 128 | docs/_build/ 129 | 130 | # PyBuilder 131 | .pybuilder/ 132 | target/ 133 | 134 | # Jupyter Notebook 135 | .ipynb_checkpoints 136 | 137 | # IPython 138 | profile_default/ 139 | ipython_config.py 140 | 141 | # pyenv 142 | # For a library or package, you might want to ignore these files since the code is 143 | # intended to run in multiple environments; otherwise, check them in: 144 | # .python-version 145 | 146 | # pipenv 147 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 148 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 149 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 150 | # install all needed dependencies. 151 | #Pipfile.lock 152 | 153 | # poetry 154 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 155 | # This is especially recommended for binary packages to ensure reproducibility, and is more 156 | # commonly ignored for libraries. 157 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 158 | #poetry.lock 159 | 160 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 161 | __pypackages__/ 162 | 163 | # Celery stuff 164 | celerybeat-schedule 165 | celerybeat.pid 166 | 167 | # SageMath parsed files 168 | *.sage.py 169 | 170 | # Environments 171 | .env 172 | .venv 173 | env/ 174 | venv/ 175 | ENV/ 176 | env.bak/ 177 | venv.bak/ 178 | 179 | # Spyder project settings 180 | .spyderproject 181 | .spyproject 182 | 183 | # Rope project settings 184 | .ropeproject 185 | 186 | # mkdocs documentation 187 | /site 188 | 189 | # mypy 190 | .mypy_cache/ 191 | .dmypy.json 192 | dmypy.json 193 | 194 | # Pyre type checker 195 | .pyre/ 196 | 197 | # pytype static type analyzer 198 | .pytype/ 199 | 200 | # Cython debug symbols 201 | cython_debug/ 202 | 203 | # PyCharm 204 | # JetBrains specific template is maintainted in a separate JetBrains.gitignore that can 205 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 206 | # and can be added to the global gitignore or merged into this file. For a more nuclear 207 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 208 | #.idea/ 209 | 210 | # -------------------------------------------- VSCode -------------------------------------------- # 211 | 212 | .vscode/ 213 | 214 | # ---------------------------------------------- Vim --------------------------------------------- # 215 | 216 | # Swap 217 | [._]*.s[a-v][a-z] 218 | !*.svg # comment out if you don't need vector files 219 | [._]*.sw[a-p] 220 | [._]s[a-rt-v][a-z] 221 | [._]ss[a-gi-z] 222 | [._]sw[a-p] 223 | 224 | # Session 225 | Session.vim 226 | Sessionx.vim 227 | 228 | # Temporary 229 | .netrwhist 230 | *~ 231 | # Auto-generated tag files 232 | tags 233 | # Persistent undo 234 | [._]*.un~ 235 | 236 | # ----------------------------------------- Sublime Text ----------------------------------------- # 237 | 238 | *.tmlanguage.cache 239 | *.tmPreferences.cache 240 | *.stTheme.cache 241 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | project(KCP VERSION 1.0.0) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | if(CMAKE_VERSION VERSION_LESS "3.15") 7 | include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/ListPrepend.cmake") 8 | list_prepend(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 9 | else() 10 | list(PREPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 11 | endif() 12 | 13 | # option(KCP_BUILD_TESTS "Build integration tests" OFF) 14 | option(KCP_BUILD_PYTHON_BINDING "Build Python binding for KCP" OFF) 15 | option(KCP_BUILD_DOC "Build documentation of KCP" OFF) 16 | 17 | # Third-party libraries 18 | # --------------------- 19 | 20 | include(ExternalProject) 21 | include(FetchContent) 22 | 23 | # Eigen 24 | find_package(Eigen3 REQUIRED QUIET) 25 | 26 | # nanoflann 27 | find_package(nanoflann REQUIRED QUIET) 28 | 29 | # TEASER++ 30 | find_package(teaserpp REQUIRED QUIET) 31 | set(TEASER_LIBRARIES teaserpp::teaser_registration) 32 | 33 | # # GoogleTest 34 | # if (KCP_BUILD_TESTS) 35 | # include(gtest) 36 | # endif() 37 | 38 | # pybind11 39 | if (KCP_BUILD_PYTHON_BINDING) 40 | include(pybind11) 41 | endif() 42 | 43 | # doxygen 44 | if (KCP_BUILD_DOC) 45 | include(doxygenTheme) 46 | find_package(Doxygen OPTIONAL_COMPONENTS dot) 47 | endif() 48 | 49 | # Building KCP 50 | # ------------ 51 | 52 | add_subdirectory(kcp) 53 | 54 | if (KCP_BUILD_PYTHON_BINDING) 55 | add_subdirectory(python) 56 | endif() 57 | 58 | # if (KCP_BUILD_TESTS) 59 | # enable_testing() 60 | # add_subdirectory(test) 61 | # endif() 62 | 63 | if (KCP_BUILD_DOC) 64 | if (DOXYGEN_FOUND) 65 | set(DOXYGEN_IN "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Doxyfile.in") 66 | set(DOXYGEN_OUT "${CMAKE_CURRENT_BINARY_DIR}/Doxyfile") 67 | configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) 68 | message("Doxygen build started") 69 | 70 | set(DOXYGEN_USE_MDFILE_AS_MAINPAGE "${CMAKE_SOURCE_DIR}/README.md") 71 | 72 | add_custom_target(docs ALL 73 | COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT} 74 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 75 | COMMENT "Generating API documentation with Doxygen" 76 | VERBATIM) 77 | else() 78 | message("Doxygen need to be installed to generate the doxygen documentation") 79 | endif() 80 | endif() 81 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Yu-Kai Lin 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | - Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | - Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | - Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # KCP 2 | 3 | [![License](https://img.shields.io/badge/License-BSD_3--Clause-blue.svg?style=flat)](https://opensource.org/licenses/BSD-3-Clause) 4 | [![Build](https://github.com/StephLin/KCP/actions/workflows/build-deploy.yml/badge.svg)](https://StephLin.github.io/KCP) 5 | 6 | The official implementation of KCP: K-Closest Points and Maximum Clique Pruning 7 | for Efficient and Effective 3D Laser Scan Matching, accepted for publication in 8 | the IEEE Robotics and Automation Letters (RA-L). 9 | 10 | ![](docs/images/snapshot.gif) 11 | 12 | KCP is an efficient and effective local point cloud registration approach 13 | targeting for real-world 3D LiDAR scan matching problem. A simple (and naive) 14 | understanding is: **I**CP iteratively considers the closest point of each source 15 | point, but **K**CP considers the **k** closest points of each source point in 16 | the beginning, and outlier correspondences are mainly rejected by the maximum 17 | clique pruning method. KCP is written in **C++** and we also support **Python** 18 | binding of KCP (pykcp). 19 | 20 | For more, please refer to our paper: 21 | 22 | - Yu-Kai Lin, Wen-Chieh Lin, Chieh-Chih Wang, **K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching**. _IEEE Robotics and Automation Letters (RA-L)_, vol.7, no. 2, pp. 1471 -- 1477, Apr. 2022. ([paper](https://doi.org/10.1109/LRA.2021.3140130)) ([preprint](https://gpl.cs.nycu.edu.tw/Steve-Lin/KCP/preprint.pdf)) ([code](https://github.com/StephLin/KCP)) ([video](https://youtu.be/ZaDLEOz_yYc)) 23 | 24 | If you use this project in your research, please cite: 25 | 26 | ```bibtex 27 | @article{lin2022kcp, 28 | title={K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching}, 29 | author={Lin, Yu-Kai and Lin, Wen-Chieh and Wang, Chieh-Chih}, 30 | journal={IEEE Robotics and Automation Letters}, 31 | volume={7}, 32 | number={2}, 33 | pages={1471--1477}, 34 | year={2022}, 35 | doi={10.1109/LRA.2021.3140130}, 36 | } 37 | ``` 38 | 39 | and if you find this project helpful or interesting, please **:star:Star** the 40 | repository. Thank you! 41 | 42 | **Table of Contents** 43 | 44 | - [KCP](#kcp) 45 | - [:package: Resources](#package-resources) 46 | - [:gear: Installation](#gear-installation) 47 | - [Step 1. Preparing the Dependencies](#step-1-preparing-the-dependencies) 48 | - [GCC, CMake, Git, and Eigen3](#gcc-cmake-git-and-eigen3) 49 | - [nanoflann](#nanoflann) 50 | - [TEASER++](#teaser) 51 | - [Step 2. Preparing Dependencies of Python Binding (Optional)](#step-2-preparing-dependencies-of-python-binding-optional) 52 | - [Step 3. Building KCP](#step-3-building-kcp) 53 | - [Without Python Binding](#without-python-binding) 54 | - [With Python Binding](#with-python-binding) 55 | - [Step 4. Installing KCP to the System (Optional)](#step-4-installing-kcp-to-the-system-optional) 56 | - [:seedling: Examples](#seedling-examples) 57 | - [:memo: Some Remarks](#memo-some-remarks) 58 | - [Tuning Parameters](#tuning-parameters) 59 | - [Controlling Computational Cost](#controlling-computational-cost) 60 | - [Torwarding Global Registration Approaches](#torwarding-global-registration-approaches) 61 | - [:gift: Acknowledgement](#gift-acknowledgement) 62 | 63 | ## :package: Resources 64 | 65 | - [API Documentation](https://stephlin.github.io/KCP) 66 | - [Complete result of the experiment of robustness](https://gpl.cs.nycu.edu.tw/Steve-Lin/KCP/robustness.html) 67 | 68 | ## :gear: Installation 69 | 70 | The project is originally developed in **Ubuntu 18.04**, and the following 71 | instruction supposes that you are using Ubuntu 18.04 as well. I am not sure if 72 | it also works with other Ubuntu versions or other Linux distributions, but maybe 73 | you can give it a try :+1: 74 | 75 | Also, please feel free to open an issue if you encounter any problems of the 76 | following instruction. 77 | 78 | ### Step 1. Preparing the Dependencies 79 | 80 | You have to prepare the following packages or libraries used in KCP: 81 | 82 | 1. A C++ compiler supporting C++14 and OpenMP (e.g. [GCC](https://gcc.gnu.org/) 7.5). 83 | 2. [CMake](https://cmake.org/) ≥ **3.11** 84 | 3. [Git](https://git-scm.com/) 85 | 4. [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page) ≥ 3.3 86 | 5. [nanoflann](https://github.com/jlblancoc/nanoflann) 87 | 6. [TEASER++](https://github.com/MIT-SPARK/TEASER-plusplus) ≥ [d79d0c67](https://github.com/MIT-SPARK/TEASER-plusplus/tree/d79d0c67) 88 | 89 | #### GCC, CMake, Git, and Eigen3 90 | 91 | ```bash 92 | sudo apt update 93 | sudo apt install -y g++ build-essential libeigen3-dev git software-properties-common lsb-release 94 | 95 | # If you want to obtain newer version of cmake, run the following commands: (Ref: https://apt.kitware.com/) 96 | # wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null 97 | # echo "deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ $(lsb_release -sc) main" | sudo tee /etc/apt/sources.list.d/kitware.list >/dev/null 98 | # sudo apt update 99 | 100 | sudo apt install cmake 101 | ``` 102 | 103 | #### nanoflann 104 | 105 | ```bash 106 | cd ~ 107 | git clone https://github.com/jlblancoc/nanoflann 108 | cd nanoflann 109 | mkdir build && cd build 110 | cmake .. -DNANOFLANN_BUILD_EXAMPLES=OFF -DNANOFLANN_BUILD_TESTS=OFF 111 | make 112 | sudo make install 113 | ``` 114 | 115 | #### TEASER++ 116 | 117 | ```bash 118 | cd ~ 119 | git clone https://github.com/MIT-SPARK/TEASER-plusplus 120 | cd TEASER-plusplus 121 | git checkout d79d0c67 122 | mkdir build && cd build 123 | cmake .. -DBUILD_TESTS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_DOC=OFF 124 | make 125 | sudo make install 126 | ``` 127 | 128 | ### Step 2. Preparing Dependencies of Python Binding (Optional) 129 | 130 | The Python binding of KCP (pykcp) uses 131 | [pybind11](https://github.com/pybind/pybind11) to achieve operability between 132 | C++ and Python. KCP will automatically download and compile pybind11 during the 133 | compilation stage. However, you need to prepare a runable Python environment 134 | with header files for the Python C API (python3-dev): 135 | 136 | ```bash 137 | sudo apt install -y python3 python3-dev 138 | ``` 139 | 140 | ### Step 3. Building KCP 141 | 142 | Execute the following commands to build KCP: 143 | 144 | #### Without Python Binding 145 | 146 | ```bash 147 | git clone https://github.com/StephLin/KCP 148 | cd KCP 149 | mkdir build && cd build 150 | cmake .. 151 | make 152 | ``` 153 | 154 | #### With Python Binding 155 | 156 | ```bash 157 | git clone https://github.com/StephLin/KCP 158 | cd KCP 159 | mkdir build && cd build 160 | cmake .. -DKCP_BUILD_PYTHON_BINDING=ON -DPYTHON_EXECUTABLE=$(which python3) 161 | make 162 | ``` 163 | 164 | ### Step 4. Installing KCP to the System (Optional) 165 | 166 | This will make the KCP library available in the system, and any C++ (CMake) 167 | project can find the package by `find_package(KCP)`. Think twice before you 168 | enter the following command! 169 | 170 | ```bash 171 | # Under /path/to/KCP/build 172 | sudo make install 173 | ``` 174 | 175 | ## :seedling: Examples 176 | 177 | We provide two examples (one for C++ and the other for Python 3) These examples 178 | take nuScenes' LiDAR data to perform registration. Please check 179 | 180 | - [C++ example with CMake](examples/cpp), and 181 | - [Python example](examples/python) 182 | 183 | for more information. 184 | 185 | ## :memo: Some Remarks 186 | 187 | ### Tuning Parameters 188 | 189 | The major parameters are 190 | 191 | - `kcp::KCP::Params::k` and 192 | - `kcp::KCP::Params::teaser::noise_bound`, 193 | 194 | where `k` is the number of nearest points of each source point selected to be 195 | part of initial correspondences, and `noise_bound` is the criterion to determine 196 | if a correspondence is correct. In our paper, we suggest `k=2` and `noise_bound` 197 | the 3-sigma (we use `noise_bound=0.06` meters for nuScenes data), and those are 198 | default values in the library. 199 | 200 | There is also a boolean parameter to enable debug messages called 201 | `kcp::KCP::Params::verbose` (default: `false`). 202 | 203 | To use different parameters to the KCP solver, please refer to the following 204 | snippets: 205 | 206 | **C++** 207 | 208 | ```cpp 209 | #include 210 | 211 | auto params = kcp::KCP::Params(); 212 | 213 | params.k = 2; 214 | params.verbose = false; 215 | params.teaser.noise_bound = 0.06; 216 | 217 | auto solver = kcp::KCP(params); 218 | ``` 219 | 220 | **Python** 221 | 222 | ```python 223 | import pykcp 224 | 225 | params = pykcp.KCPParams() 226 | params.k = 2 227 | params.verbose = False 228 | params.teaser.noise_bound = 0.06 229 | 230 | solver = pykcp.KCP(params) 231 | ``` 232 | 233 | ### Controlling Computational Cost 234 | 235 | Instead of 236 | [correspondence-free registration in TEASER++](https://github.com/MIT-SPARK/TEASER-plusplus/issues/120), 237 | KCP considers k closest point correspondences to reduce the major computational 238 | cost of the maximum clique algorithm, and we have expressed the ability for 239 | real-world scenarios without any complicate or learning-based feature descriptor 240 | in the paper. However, it is still possible to encounter computational time or 241 | memory issue if there are too many correspondences fed to the solver. 242 | 243 | We suggest controlling your keypoints around 500 for k=2 (in this way the 244 | computational time will be much closer to the one presented in the paper). 245 | 246 | ### Torwarding Global Registration Approaches 247 | 248 | It is promising that KCP can be extended to a global registration approach if a 249 | fast and reliable sparse feature point representation method is employed. 250 | 251 | In this way, the role of RANSAC, a fast registration approach usually used in 252 | learning based approaches, is similar to KCP's, but the computation results of 253 | KCP are deterministic, and also, KCP has better theoretical supports. 254 | 255 | ## :gift: Acknowledgement 256 | 257 | This project refers to the computation of the smoothness term defined in 258 | [LOAM](https://www.ri.cmu.edu/pub_files/2014/7/Ji_LidarMapping_RSS2014_v8.pdf) 259 | (implemented in [Tixiao Shan](https://github.com/TixiaoShan)'s excellent project 260 | [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM), which is licensed under 261 | BSD-3). We modified the definition of the smoothness term (and it is called the 262 | multi-scale curvature in this project). 263 | -------------------------------------------------------------------------------- /cmake/Doxyfile.in: -------------------------------------------------------------------------------- 1 | PROJECT_NAME = KCP 2 | PROJECT_BRIEF = "An efficient and effective 3D laser scan matching" 3 | 4 | OUTPUT_DIRECTORY = @CMAKE_CURRENT_BINARY_DIR@/docs/ 5 | 6 | INPUT += @CMAKE_CURRENT_SOURCE_DIR@/docs/mainpage.md 7 | INPUT += @CMAKE_CURRENT_SOURCE_DIR@/docs/installation.md 8 | INPUT += @CMAKE_CURRENT_SOURCE_DIR@/docs/examples.md 9 | INPUT += @CMAKE_CURRENT_SOURCE_DIR@/docs/remarks.md 10 | INPUT += @CMAKE_CURRENT_SOURCE_DIR@/kcp 11 | RECURSIVE = YES 12 | GENERATE_TREEVIEW = YES 13 | 14 | IMAGE_PATH = @CMAKE_CURRENT_SOURCE_DIR@/docs 15 | 16 | FULL_PATH_NAMES = YES 17 | STRIP_FROM_INC_PATH = @CMAKE_CURRENT_SOURCE_DIR@/kcp/include 18 | 19 | HTML_EXTRA_FILES = @CMAKE_CURRENT_BINARY_DIR@/_deps/doxygentheme-src/doxygen-awesome-darkmode-toggle.js 20 | HTML_EXTRA_STYLESHEET = @CMAKE_CURRENT_BINARY_DIR@/_deps/doxygentheme-src/doxygen-awesome.css \ 21 | @CMAKE_CURRENT_BINARY_DIR@/_deps/doxygentheme-src/doxygen-awesome-sidebar-only.css \ 22 | @CMAKE_CURRENT_BINARY_DIR@/_deps/doxygentheme-src/doxygen-awesome-sidebar-only-darkmode-toggle.css 23 | HTML_HEADER = @CMAKE_CURRENT_SOURCE_DIR@/docs/header.html 24 | HTML_FOOTER = @CMAKE_CURRENT_SOURCE_DIR@/docs/footer.html 25 | 26 | USE_MDFILE_AS_MAINPAGE = @CMAKE_CURRENT_SOURCE_DIR@/docs/mainpage.md 27 | 28 | FULL_PATH_NAMES = YES 29 | STRIP_FROM_PATH = @CMAKE_CURRENT_SOURCE_DIR@ 30 | -------------------------------------------------------------------------------- /cmake/ListPrepend.cmake: -------------------------------------------------------------------------------- 1 | FUNCTION(list_prepend var value) 2 | message("${${var}}") 3 | if("${${var}}" STREQUAL "") 4 | SET(${var} "${value}" PARENT_SCOPE) 5 | else() 6 | SET(${var} "${value}" "${${var}}" PARENT_SCOPE) 7 | endif() 8 | ENDFUNCTION(list_prepend) -------------------------------------------------------------------------------- /cmake/doxygenTheme.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | FetchContent_Declare( 4 | doxygentheme 5 | GIT_REPOSITORY https://github.com/jothepro/doxygen-awesome-css 6 | GIT_TAG v1.5.0 7 | ) 8 | 9 | FetchContent_GetProperties(doxygentheme) 10 | if(NOT doxygentheme_POPULATED) 11 | FetchContent_Populate(doxygentheme) 12 | endif() -------------------------------------------------------------------------------- /cmake/gtest.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | FetchContent_Declare( 4 | googletest 5 | GIT_REPOSITORY https://github.com/google/googletest.git 6 | GIT_TAG release-1.10.0 7 | ) 8 | 9 | if (WIN32) 10 | # For Windows: Prevent overriding the parent project's compiler/linker settings 11 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 12 | endif() 13 | 14 | FetchContent_GetProperties(googletest) 15 | if(NOT googletest_POPULATED) 16 | FetchContent_Populate(googletest) 17 | add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR} EXCLUDE_FROM_ALL) 18 | endif() -------------------------------------------------------------------------------- /cmake/pybind11.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | 3 | FetchContent_Declare( 4 | pybind11 5 | GIT_REPOSITORY https://github.com/pybind/pybind11 6 | GIT_TAG v2.2.3 7 | ) 8 | 9 | FetchContent_GetProperties(pybind11) 10 | if(NOT pybind11_POPULATED) 11 | FetchContent_Populate(pybind11) 12 | add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR} EXCLUDE_FROM_ALL) 13 | endif() -------------------------------------------------------------------------------- /docs/examples.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | 3 | We provide two examples (one for C++ and the other for Python 3) These examples 4 | take nuScenes' LiDAR data to perform registration. Please check 5 | 6 | - [C++ example with CMake](https://github.com/StephLin/KCP/tree/main/examples/cpp), and 7 | - [Python example](https://github.com/StephLin/KCP/tree/main/examples/python) 8 | 9 | for more information. 10 | -------------------------------------------------------------------------------- /docs/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10 | 11 | 12 |
13 | $generatedby doxygen $doxygenversion 14 |
15 | 16 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /docs/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | $projectname: $title 10 | $title 11 | 12 | 13 | 14 | $treeview 15 | $search 16 | $mathjax 17 | 18 | $extrastylesheet 19 | 20 | 21 | 22 | 23 | 24 |
25 | 26 | 27 |
28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 41 | 42 | 43 | 44 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 |
36 |
$projectname 37 |  $projectnumber 38 |
39 |
$projectbrief
40 | 		45 |
$projectbrief
46 | 		$searchbox
57 |
58 | 59 | 60 | -------------------------------------------------------------------------------- /docs/images/snapshot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StephLin/KCP/2386fffb8ccfa9d22cf134ccdc4553a5b1cfc975/docs/images/snapshot.gif -------------------------------------------------------------------------------- /docs/images/snapshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StephLin/KCP/2386fffb8ccfa9d22cf134ccdc4553a5b1cfc975/docs/images/snapshot.png -------------------------------------------------------------------------------- /docs/installation.md: -------------------------------------------------------------------------------- 1 | # Installation 2 | 3 | The project is originally developed in **Ubuntu 18.04**, and the following 4 | instruction supposes that you are using Ubuntu 18.04 as well. I am not sure if 5 | it also works with other Ubuntu versions of other Linux distributions, but maybe 6 | you can give it a try :+1: 7 | 8 | Also, please feel free to open an issue if you encounter any problems of the 9 | following instruction. 10 | 11 | ## Step 1. Preparing the Dependencies 12 | 13 | You have to prepare the following packages or libraries used in KCP: 14 | 15 | 1. A C++ compiler supporting C++14 and OpenMP (e.g. [GCC](https://gcc.gnu.org/) 7.5). 16 | 2. [CMake](https://cmake.org/) ≥ **3.11** 17 | 3. [Git](https://git-scm.com/) 18 | 4. [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page) ≥ 3.3 19 | 5. [nanoflann](https://github.com/jlblancoc/nanoflann) 20 | 6. [TEASER++](https://github.com/MIT-SPARK/TEASER-plusplus) ≥ [d79d0c67](https://github.com/MIT-SPARK/TEASER-plusplus/tree/d79d0c67) 21 | 22 | ### GCC, CMake, Git, and Eigen3 23 | 24 | ```bash 25 | sudo apt update 26 | sudo apt install -y g++ build-essential libeigen3-dev git software-properties-common lsb-release 27 | 28 | # If you want to obtain newer version of cmake, run the following commands: (Ref: https://apt.kitware.com/) 29 | # wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null 30 | # echo "deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ $(lsb_release -sc) main" | sudo tee /etc/apt/sources.list.d/kitware.list >/dev/null 31 | # sudo apt update 32 | 33 | sudo apt install cmake 34 | ``` 35 | 36 | ### nanoflann 37 | 38 | ```bash 39 | cd ~ 40 | git clone https://github.com/jlblancoc/nanoflann 41 | cd nanoflann 42 | mkdir build && cd build 43 | cmake .. -DNANOFLANN_BUILD_EXAMPLES=OFF -DNANOFLANN_BUILD_TESTS=OFF 44 | make 45 | sudo make install 46 | ``` 47 | 48 | ### TEASER++ 49 | 50 | ```bash 51 | cd ~ 52 | git clone https://github.com/MIT-SPARK/TEASER-plusplus 53 | cd TEASER-plusplus 54 | git checkout d79d0c67 55 | mkdir build && cd build 56 | cmake .. -DBUILD_TESTS=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_DOC=OFF 57 | make 58 | sudo make install 59 | ``` 60 | 61 | ## Step 2. Preparing Dependencies of Python Binding (Optional) 62 | 63 | The Python binding of KCP (pykcp) uses 64 | [pybind11](https://github.com/pybind/pybind11) to achieve operability between 65 | C++ and Python. KCP will automatically download and compile pybind11 during the 66 | compilation stage. However, you need to prepare a runable Python environment 67 | with header files for the Python C API (python3-dev): 68 | 69 | ```bash 70 | sudo apt install -y python3 python3-dev 71 | ``` 72 | 73 | ## Step 3. Building KCP 74 | 75 | Execute the following commands to build KCP: 76 | 77 | ### Without Python Binding 78 | 79 | ```bash 80 | git clone https://github.com/StephLin/KCP 81 | cd KCP 82 | mkdir build && cd build 83 | cmake .. 84 | make 85 | ``` 86 | 87 | ### With Python Binding 88 | 89 | ```bash 90 | git clone https://github.com/StephLin/KCP 91 | cd KCP 92 | mkdir build && cd build 93 | cmake .. -DKCP_BUILD_PYTHON_BINDING=ON -DPYTHON_EXECUTABLE=$(which python3) 94 | make 95 | ``` 96 | 97 | ## Step 4. Installing KCP to the System (Optional) 98 | 99 | This will make the KCP library available in the system, and any C++ (CMake) 100 | project can find the package by `find_package(KCP)`. Think twice before you 101 | enter the following command! 102 | 103 | ```bash 104 | # Under /path/to/KCP/build 105 | sudo make install 106 | ``` 107 | -------------------------------------------------------------------------------- /docs/mainpage.md: -------------------------------------------------------------------------------- 1 | # KCP 2 | 3 | [paper](https://doi.org/10.1109/LRA.2021.3140130) | [preprint](https://gpl.cs.nycu.edu.tw/Steve-Lin/KCP/preprint.pdf) | [code](https://github.com/StephLin/KCP) | [video](https://youtu.be/ZaDLEOz_yYc) 4 | 5 | The official implementation of KCP: K-Closest Points and Maximum Clique Pruning 6 | for Efficient and Effective 3D Laser Scan Matching, accepted for publication in 7 | the IEEE Robotics and Automation Letters (RA-L). 8 | 9 | ![](images/snapshot.gif) 10 | 11 | KCP is an efficient and effective local point cloud registration approach 12 | targeting for real-world 3D LiDAR scan matching problem. A simple (and naive) 13 | understanding is: ICP iteratively considers the closest point of each 14 | source point, but KCP considers the k closest points of each 15 | source point in the beginning, and outlier correspondences are mainly rejected 16 | by the maximum clique pruning method. KCP is written in C++ and we also 17 | support Python binding of KCP (pykcp). 18 | 19 | For more, please refer to our paper: 20 | 21 | - Yu-Kai Lin, Wen-Chieh Lin, Chieh-Chih Wang, **K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching**. _IEEE Robotics and Automation Letters (RA-L)_, vol.7, no. 2, pp. 1471 -- 1477, Apr. 2022. ([paper](https://doi.org/10.1109/LRA.2021.3140130)) ([preprint](https://gpl.cs.nycu.edu.tw/Steve-Lin/KCP/preprint.pdf)) ([code](https://github.com/StephLin/KCP)) ([video](https://youtu.be/ZaDLEOz_yYc)) 22 | 23 | If you use this project in your research, please cite: 24 | 25 | ```bibtex 26 | @article{lin2022kcp, 27 | title={K-Closest Points and Maximum Clique Pruning for Efficient and Effective 3-D Laser Scan Matching}, 28 | author={Lin, Yu-Kai and Lin, Wen-Chieh and Wang, Chieh-Chih}, 29 | journal={IEEE Robotics and Automation Letters}, 30 | volume={7}, 31 | number={2}, 32 | pages={1471--1477}, 33 | year={2022}, 34 | doi={10.1109/LRA.2021.3140130}, 35 | } 36 | ``` 37 | 38 | and if you find this project helpful or interesting, please 39 | [⭐Star the repository](https://github.com/StephLin/KCP). Thank you! 40 | -------------------------------------------------------------------------------- /docs/remarks.md: -------------------------------------------------------------------------------- 1 | # Remarks 2 | 3 | ## Tuning Parameters 4 | 5 | The major parameters are 6 | 7 | - `kcp::KCP::Params::k` and 8 | - [`kcp::KCP::Params::teaser::noise_bound`](https://teaser.readthedocs.io/en/latest/api-cpp.html#_CPPv4N6teaser24RobustRegistrationSolver6Params11noise_boundE), 9 | 10 | where `k` is the number of nearest points of each source point selected to be 11 | part of initial correspondences, and `noise_bound` is the criterion to determine 12 | if a correspondence is correct. In our paper, we suggest `k=2` and `noise_bound` 13 | the 3-sigma (we use `noise_bound=0.06` meters for nuScenes data), and those are 14 | default values in the library. 15 | 16 | There is also a boolean parameter to enable debug messages called 17 | `kcp::KCP::Params::verbose` (default: `false`). 18 | 19 | To use different parameters to the KCP solver, please refer to the following 20 | snippets: 21 | 22 | C++ 23 | 24 | ```cpp 25 | #include 26 | 27 | auto params = kcp::KCP::Params(); 28 | 29 | params.k = 2; 30 | params.verbose = false; 31 | params.teaser.noise_bound = 0.06; 32 | 33 | auto solver = kcp::KCP(params); 34 | ``` 35 | 36 | **Python** 37 | 38 | ```python 39 | import pykcp 40 | 41 | params = pykcp.KCPParams() 42 | params.k = 2 43 | params.verbose = False 44 | params.teaser.noise_bound = 0.06 45 | 46 | solver = pykcp.KCP(params) 47 | ``` 48 | 49 | ## Controlling Computational Cost 50 | 51 | Instead of 52 | [correspondence-free registration in TEASER++](https://github.com/MIT-SPARK/TEASER-plusplus/issues/120), 53 | KCP considers k closest point correspondences to reduce the major computational 54 | cost of the maximum clique algorithm, and we have expressed the ability for 55 | real-world scenarios without any complicate or learning-based feature descriptor 56 | in the paper. However, it is still possible to encounter computational time or 57 | memory issue if there are too many correspondences fed to the solver. 58 | 59 | We suggest controlling your keypoints around 500 for k=2 (in this way the 60 | computational time will be much closer to the one presented in the paper). 61 | 62 | ## Torwarding Global Registration Approaches 63 | 64 | It is promising that KCP can be extended to a global registration approach if a 65 | fast and reliable sparse feature point representation method is employed. 66 | 67 | In this way, the role of RANSAC, a fast registration approach usually used in 68 | learning based approaches, is similar to KCP's, but the computation results of 69 | KCP are deterministic, and also, KCP has better theoretical supports. 70 | -------------------------------------------------------------------------------- /examples/cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(kcp_cpp_example VERSION 1.0.0) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | option(FIND_KCP_FROM_SYSTEM "Find KCP from system" OFF) 7 | 8 | if (FIND_KCP_FROM_SYSTEM) 9 | find_package(Eigen3 REQUIRED QUIET) 10 | find_package(nanoflann REQUIRED QUIET) 11 | find_package(teaserpp REQUIRED QUIET) 12 | find_package(KCP REQUIRED) 13 | else() 14 | option(KCP_BUILD_TESTS "" OFF) 15 | option(KCP_BUILD_PYTHON_WRAPPER "" OFF) 16 | option(KCP_BUILD_DOC "" OFF) 17 | add_subdirectory(${CMAKE_SOURCE_DIR}/../.. ${CMAKE_BINARY_DIR}/kcp) 18 | endif() 19 | 20 | # PCL 21 | find_package(PCL 1.8 REQUIRED COMPONENTS io) 22 | 23 | include_directories( 24 | ${PCL_INCLUDE_DIRS} 25 | ) 26 | add_executable(main main.cpp) 27 | target_link_libraries(main PRIVATE KCP::kcp ${PCL_LIBRARIES}) 28 | 29 | add_custom_target(data 30 | COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_SOURCE_DIR}/../data ${CMAKE_BINARY_DIR}/data 31 | ) 32 | add_dependencies(main data) 33 | -------------------------------------------------------------------------------- /examples/cpp/README.md: -------------------------------------------------------------------------------- 1 | # C++ example with CMake 2 | 3 | This is a C++ example with CMake management. Note that this example requires the 4 | [Point Cloud Library (PCL) ≥ 1.8](https://pointclouds.org/) for reading 5 | `.pcd` files. You can install the library by the following command: 6 | 7 | ```bash 8 | sudo apt install -y libpcl-dev 9 | ``` 10 | 11 | ## :gear: Building 12 | 13 | To build the project, please choose a proper one and execute the corresponding 14 | script: 15 | 16 | ### Using `add_subdirectory` (Default) 17 | 18 | - Pros: You don't need to install the KCP library (libkcp) to the system. 19 | - Cons: You have to set a path to the KCP repository, and you need to re-compile 20 | KCP for each project. 21 | 22 | ```bash 23 | mkdir build && cd build 24 | cmake .. 25 | make 26 | ``` 27 | 28 | ### Using `find_package` 29 | 30 | - Pros: You can import the KCP library anywhere, without the path to the KCP 31 | repository. Moreover, you can re-use the shared library. 32 | - Cons: You have to install the KCP library (libkcp) to the system. 33 | 34 | ```bash 35 | mkdir build && cd build 36 | cmake .. -DFIND_KCP_FROM_SYSTEM=ON 37 | make 38 | ``` 39 | 40 | ## :running: Execution 41 | 42 | ```bash 43 | ./main 44 | ``` 45 | 46 | The main function will read two point clouds from [data](../data), and perform 47 | the scan matching using KCP. 48 | -------------------------------------------------------------------------------- /examples/cpp/data: -------------------------------------------------------------------------------- 1 | ../data -------------------------------------------------------------------------------- /examples/cpp/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Yu-Kai Lin. All rights reserved. 2 | // Use of this source code is governed by a BSD-style 3 | // license that can be found in the LICENSE file. 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | /** 13 | * @brief Convert ``pcl::PointCloud`` to ``Eigen::MatrixX3d``. 14 | * 15 | * @param cloud The point cloud of type ``pcl::PointCloud`` 16 | * @param matrix The point cloud of type ``Eigen::MatrixX3d``. 17 | */ 18 | void convert_pcl_point_cloud_to_eigen_matrix(pcl::PointCloud& cloud, 19 | Eigen::MatrixX3d& matrix) { 20 | matrix.setZero(cloud.size(), 3); 21 | for (int idx = 0; idx < cloud.size(); ++idx) { 22 | const auto& point = cloud[idx]; 23 | matrix(idx, 0) = point.x; 24 | matrix(idx, 1) = point.y; 25 | matrix(idx, 2) = point.z; 26 | } 27 | } 28 | 29 | /** 30 | * @brief Point cloud preprocessing for the nuScenes data. 31 | * 32 | * @details This preprocessing removes ground points (using a very simple 33 | * condition that z-axis <= -1.5m) and points coming from the inspector. 34 | * 35 | * @param cloud The point cloud. 36 | */ 37 | void preprocessing(pcl::PointCloud* cloud) { 38 | pcl::PointCloud filtered_cloud; 39 | 40 | for (const auto& point : *cloud) { 41 | // remove points comes from the inspector 42 | bool&& cond_x = point.x > 0.62 || point.x < -0.62; 43 | bool&& cond_y = point.y > 1.87 || point.y < -1.10; 44 | if (!cond_x && !cond_y) continue; 45 | 46 | // remove ground points 47 | bool&& cond_z = point.z > -1.5; 48 | if (!cond_z) continue; 49 | 50 | filtered_cloud.push_back(point); 51 | } 52 | 53 | *cloud = filtered_cloud; 54 | } 55 | 56 | int main() { 57 | /** 58 | * 1. Load point clouds and convert their types to Eigen::MatrixX3d. 59 | */ 60 | std::string source_point_cloud_filename = "data/1531883530.949817000.pcd"; 61 | std::string target_point_cloud_filename = "data/1531883530.449377000.pcd"; 62 | 63 | pcl::PointCloud source_pcl_cloud; 64 | pcl::PointCloud target_pcl_cloud; 65 | 66 | pcl::io::loadPCDFile(source_point_cloud_filename, source_pcl_cloud); 67 | pcl::io::loadPCDFile(target_point_cloud_filename, target_pcl_cloud); 68 | 69 | preprocessing(&source_pcl_cloud); 70 | preprocessing(&target_pcl_cloud); 71 | 72 | Eigen::MatrixX3d source; 73 | Eigen::MatrixX3d target; 74 | 75 | convert_pcl_point_cloud_to_eigen_matrix(source_pcl_cloud, source); 76 | convert_pcl_point_cloud_to_eigen_matrix(target_pcl_cloud, target); 77 | 78 | /** 79 | * 2. Extract corner points with multi-scale curvature. 80 | */ 81 | Eigen::MatrixX3d source_corner_points = kcp::keypoint::MultiScaleCurvature(source).get_corner_points(); 82 | Eigen::MatrixX3d target_corner_points = kcp::keypoint::MultiScaleCurvature(target).get_corner_points(); 83 | 84 | /** 85 | * 3. Estimate the transformation of two clouds with KCP-TEASER. 86 | */ 87 | auto params = kcp::KCP::Params(); 88 | 89 | params.k = 2; 90 | params.verbose = true; 91 | params.teaser.noise_bound = 0.06; 92 | 93 | auto solver = kcp::KCP(params); 94 | solver.solve(source_corner_points, target_corner_points, // source and target point clouds 95 | source_corner_points, target_corner_points); // source and target feature clouds 96 | auto solution = solver.get_solution(); 97 | std::cout << solution << '\n'; 98 | 99 | return 0; 100 | } -------------------------------------------------------------------------------- /examples/data/1531883530.449377000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StephLin/KCP/2386fffb8ccfa9d22cf134ccdc4553a5b1cfc975/examples/data/1531883530.449377000.pcd -------------------------------------------------------------------------------- /examples/data/1531883530.949817000.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/StephLin/KCP/2386fffb8ccfa9d22cf134ccdc4553a5b1cfc975/examples/data/1531883530.949817000.pcd -------------------------------------------------------------------------------- /examples/data/README.md: -------------------------------------------------------------------------------- 1 | # Sample Point Clouds 2 | 3 | These point clouds come from [the nuScenes dataset](https://www.nuscenes.org/), 4 | which is licensed under 5 | [CC BY-NC-SA 4.0](https://creativecommons.org/licenses/by-nc-sa/4.0/deed.en) 6 | with non-commercial use. For more information about nuScenes dataset policy, 7 | please visit [their terms-of-use](https://www.nuscenes.org/terms-of-use). 8 | -------------------------------------------------------------------------------- /examples/python/.gitignore: -------------------------------------------------------------------------------- 1 | snapshot.png 2 | open3d-0.14.1-cp36-cp36m-manylinux_2_27_x86_64.whl -------------------------------------------------------------------------------- /examples/python/README.md: -------------------------------------------------------------------------------- 1 | # Python 3 example 2 | 3 | This is a Python 3 example. Note that this example requires NumPy, SciPy, and 4 | [Open3D](https://github.com/isl-org/Open3D) for IO and visualization. You can 5 | use the following command to install them: 6 | 7 | ```bash 8 | python3 -m pip install -r requirements.txt 9 | ``` 10 | 11 | ## :running: Execution 12 | 13 | ```bash 14 | python3 main.py 15 | ``` 16 | 17 | The main function will read two point clouds from [data](../data), perform the 18 | scan matching using KCP, and visualize the registration result with Open3D. 19 | 20 | If you receive the message: 21 | 22 | ``` 23 | Oops, We cannot import pykcp! 24 | Make sure that you have properly compiled the python binding of KCP. 25 | ``` 26 | 27 | it means that you do not properly build the Python binding of KCP (pykcp) under 28 | `../../build` relative to this directory (there should be a file something likes 29 | `../../build/python/pykcp.cpython-XXX-XXX-XXX-XXX.so`). Please check the 30 | installation guideline and build KCP **with python binding**. 31 | -------------------------------------------------------------------------------- /examples/python/data: -------------------------------------------------------------------------------- 1 | ../data -------------------------------------------------------------------------------- /examples/python/main.py: -------------------------------------------------------------------------------- 1 | # Copyright 2021 Yu-Kai Lin. All rights reserved. 2 | # Use of this source code is governed by a BSD-style 3 | # license that can be found in the LICENSE file. 4 | 5 | import os.path as osp 6 | import sys 7 | from functools import partial 8 | 9 | __dir__ = osp.dirname(osp.abspath(__file__)) 10 | 11 | try: 12 | pykcp_path = osp.join(__dir__, "../../build/python") 13 | sys.path.insert(0, pykcp_path) 14 | import pykcp 15 | except ImportError: 16 | print("Oops, We cannot import pykcp!") 17 | print("Make sure that you have properly compiled the python binding of KCP.") 18 | exit(1) 19 | 20 | import numpy as np 21 | import open3d as o3d 22 | from scipy.spatial.transform.rotation import Rotation as R 23 | 24 | source_point_cloud_filename = osp.join(__dir__, "data/1531883530.949817000.pcd") 25 | target_point_cloud_filename = osp.join(__dir__, "data/1531883530.449377000.pcd") 26 | 27 | snapshot_path = "snapshot.png" 28 | 29 | 30 | def create_line_by_cylinder(start, end, radius, color): 31 | length = np.linalg.norm(start - end) 32 | cylinder = o3d.geometry.TriangleMesh.create_cylinder(radius, length, 8) 33 | translation = (start + end) / 2 34 | axis = np.cross((start - end) / length, np.array([0, 0, 1])) 35 | angle = np.arccos(np.dot((start - end) / length, np.array([0, 0, 1]))) 36 | rotation = R.from_rotvec(angle * axis).as_matrix() 37 | cylinder.rotate(rotation, center=(0, 0, 0)) 38 | cylinder.translate(translation) 39 | cylinder.paint_uniform_color(color) 40 | return cylinder 41 | 42 | 43 | def create_sphere(position, radius, color): 44 | sphere = o3d.geometry.TriangleMesh.create_sphere(radius) 45 | sphere.translate(position) 46 | sphere.paint_uniform_color(color) 47 | return sphere 48 | 49 | 50 | def visualize( 51 | source, 52 | target, 53 | transformation, 54 | initial_correspondences, 55 | inlier_correspondence_indices, 56 | ): 57 | source_o3d = o3d.geometry.PointCloud() 58 | source_o3d.points = o3d.utility.Vector3dVector(source) 59 | source_transformed_o3d = o3d.geometry.PointCloud() 60 | source_transformed_o3d.points = o3d.utility.Vector3dVector(source) 61 | target_o3d = o3d.geometry.PointCloud() 62 | target_o3d.points = o3d.utility.Vector3dVector(target) 63 | 64 | source_transformed_o3d.transform(transformation) 65 | 66 | source_o3d.colors = o3d.utility.Vector3dVector( 67 | np.array([[1.0, 0.2, 0.2] for _ in range(source.shape[0])], dtype=float) 68 | ) 69 | source_transformed_o3d.colors = o3d.utility.Vector3dVector( 70 | np.array([[1.0, 0.2, 0.2] for _ in range(source.shape[0])], dtype=float) 71 | ) 72 | target_o3d.colors = o3d.utility.Vector3dVector( 73 | np.array([[0.2, 0.2, 1.0] for _ in range(target.shape[0])], dtype=float) 74 | ) 75 | 76 | size = initial_correspondences.points[0].shape[1] 77 | points = list(initial_correspondences.points[0].T) 78 | points += list(initial_correspondences.points[1].T) 79 | lines = [[i, i + size] for i in range(size)] 80 | initial_correspondences_o3d = o3d.geometry.LineSet() 81 | initial_correspondences_o3d.points = o3d.utility.Vector3dVector(np.array(points)) 82 | initial_correspondences_o3d.lines = o3d.utility.Vector2iVector( 83 | np.array(lines, dtype=int) 84 | ) 85 | initial_correspondences_o3d.paint_uniform_color(np.array([0.2, 0.2, 0.2])) 86 | inlier_correspondence_lines = [] 87 | inlier_correspondence_spheres = [] 88 | for idx in inlier_correspondence_indices: 89 | start = initial_correspondences.points[0].T[idx] 90 | end = initial_correspondences.points[1].T[idx] 91 | color = np.array([0.0, 0.9, 0.0]) 92 | inlier_correspondence_lines.append( 93 | create_line_by_cylinder(start, end, 0.08, color) 94 | ) 95 | inlier_correspondence_spheres.append(create_sphere(end, 0.2, color)) 96 | 97 | def registration_before_raw(vis): 98 | vis.clear_geometries() 99 | vis.add_geometry(source_o3d, False) 100 | vis.add_geometry(target_o3d, False) 101 | return False 102 | 103 | def registration_before_raw_with_initial_correspondences(vis): 104 | registration_before_raw(vis) 105 | vis.add_geometry(initial_correspondences_o3d, False) 106 | vis.update_renderer() 107 | return False 108 | 109 | def registration_before_raw_with_inlier_correspondences(vis): 110 | registration_before_raw(vis) 111 | vis.add_geometry(initial_correspondences_o3d, False) 112 | for line in inlier_correspondence_lines: 113 | vis.add_geometry(line, False) 114 | vis.update_renderer() 115 | return False 116 | 117 | def registration_result_with_inlier_correspondences(vis): 118 | vis.clear_geometries() 119 | vis.add_geometry(source_transformed_o3d, False) 120 | vis.add_geometry(target_o3d, False) 121 | for sphere in inlier_correspondence_spheres: 122 | vis.add_geometry(sphere, False) 123 | return False 124 | 125 | def registration_result_final(vis): 126 | vis.clear_geometries() 127 | vis.add_geometry(source_transformed_o3d, False) 128 | vis.add_geometry(target_o3d, False) 129 | return False 130 | 131 | def save_snapshot(vis): 132 | vis.capture_screen_image(snapshot_path) 133 | return False 134 | 135 | vis = o3d.visualization.VisualizerWithKeyCallback() 136 | 137 | vis.create_window() 138 | vis.get_render_option().load_from_json("render_option.json") 139 | vis.register_key_callback(ord("S"), partial(save_snapshot)) 140 | vis.register_key_callback(ord("1"), partial(registration_before_raw)) 141 | vis.register_key_callback( 142 | ord("2"), partial(registration_before_raw_with_initial_correspondences) 143 | ) 144 | vis.register_key_callback( 145 | ord("3"), partial(registration_before_raw_with_inlier_correspondences) 146 | ) 147 | vis.register_key_callback( 148 | ord("4"), partial(registration_result_with_inlier_correspondences) 149 | ) 150 | vis.register_key_callback(ord("5"), partial(registration_result_final)) 151 | vis.add_geometry(source_transformed_o3d) 152 | vis.add_geometry(target_o3d) 153 | 154 | print() 155 | print("[Keymap] Key '1'-'5' to see different views, 's' to snapshot, 'q' to quit") 156 | print(" 1: initial clouds") 157 | print(" 2: initial k closet points correspondences") 158 | print(" 3: inlier correspondences by the maximum clique pruning method") 159 | print(" 4: registration result with inlier correspondences") 160 | print(" 5: registration result") 161 | 162 | vis.run() 163 | vis.destroy_window() 164 | 165 | return vis 166 | 167 | 168 | def main(): 169 | # 1. Load point clouds. 170 | source_o3d = o3d.io.read_point_cloud(source_point_cloud_filename) 171 | target_o3d = o3d.io.read_point_cloud(target_point_cloud_filename) 172 | 173 | source = np.asarray(source_o3d.points) 174 | target = np.asarray(target_o3d.points) 175 | 176 | # 2. Extract corner points with multi-scale curvature. 177 | source_corner_points = pykcp.MultiScaleCurvature(source).get_corner_points() 178 | target_corner_points = pykcp.MultiScaleCurvature(target).get_corner_points() 179 | 180 | # 3. Estimate the transformation of two clouds with KCP-TEASER. 181 | params = pykcp.KCPParams() 182 | params.k = 2 183 | params.verbose = True 184 | params.teaser.noise_bound = 0.06 185 | 186 | solver = pykcp.KCP(params) 187 | solver.solve( 188 | source_corner_points, # source point clouds 189 | target_corner_points, # target point clouds 190 | source_corner_points, # source feature clouds 191 | target_corner_points, # target feature clouds 192 | ) 193 | solution = solver.get_solution() 194 | initial_correspondences = solver.get_initial_correspondences() 195 | inlier_correspondence_indices = solver.get_inlier_correspondence_indices() 196 | 197 | print(solution) 198 | visualize( 199 | source, 200 | target, 201 | solution, 202 | initial_correspondences, 203 | inlier_correspondence_indices, 204 | ) 205 | 206 | 207 | if __name__ == "__main__": 208 | main() 209 | -------------------------------------------------------------------------------- /examples/python/render_option.json: -------------------------------------------------------------------------------- 1 | { 2 | "background_color" : [ 1.0, 1.0, 1.0 ], 3 | "class_name" : "RenderOption", 4 | "default_mesh_color" : [ 0.69999999999999996, 0.69999999999999996, 0.69999999999999996 ], 5 | "image_max_depth" : 3000, 6 | "image_stretch_option" : 1, 7 | "interpolation_option" : 0, 8 | "light0_color" : [ 1.0, 1.0, 1.0 ], 9 | "light0_diffuse_power" : 0.66000000000000003, 10 | "light0_position" : [ 0.0, 0.0, 2.0 ], 11 | "light0_specular_power" : 0.20000000000000001, 12 | "light0_specular_shininess" : 100.0, 13 | "light1_color" : [ 1.0, 1.0, 1.0 ], 14 | "light1_diffuse_power" : 0.66000000000000003, 15 | "light1_position" : [ 0.0, 0.0, 2.0 ], 16 | "light1_specular_power" : 0.20000000000000001, 17 | "light1_specular_shininess" : 100.0, 18 | "light2_color" : [ 1.0, 1.0, 1.0 ], 19 | "light2_diffuse_power" : 0.66000000000000003, 20 | "light2_position" : [ 0.0, 0.0, -2.0 ], 21 | "light2_specular_power" : 0.20000000000000001, 22 | "light2_specular_shininess" : 100.0, 23 | "light3_color" : [ 1.0, 1.0, 1.0 ], 24 | "light3_diffuse_power" : 0.66000000000000003, 25 | "light3_position" : [ 0.0, 0.0, -2.0 ], 26 | "light3_specular_power" : 0.20000000000000001, 27 | "light3_specular_shininess" : 100.0, 28 | "light_ambient_color" : [ 0.0, 0.0, 0.0 ], 29 | "light_on" : true, 30 | "line_width" : 5.0, 31 | "mesh_color_option" : 1, 32 | "mesh_shade_option" : 0, 33 | "mesh_show_back_face" : false, 34 | "mesh_show_wireframe" : false, 35 | "point_color_option" : 0, 36 | "point_show_normal" : false, 37 | "point_size" : 2.0, 38 | "show_coordinate_frame" : false, 39 | "version_major" : 1, 40 | "version_minor" : 0 41 | } -------------------------------------------------------------------------------- /examples/python/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | scipy 3 | open3d -------------------------------------------------------------------------------- /kcp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(kcp_src) 2 | 3 | include(GNUInstallDirs) 4 | 5 | add_library(kcp SHARED src/solver.cpp src/keypoint.cpp src/utility.cpp) 6 | target_include_directories(kcp PUBLIC 7 | $ 8 | $ 9 | $ 10 | ) 11 | target_link_libraries(kcp Eigen3::Eigen nanoflann::nanoflann ${TEASER_LIBRARIES}) 12 | add_library(KCP::kcp ALIAS kcp) 13 | 14 | install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ 15 | DESTINATION include 16 | ) 17 | install(TARGETS kcp 18 | EXPORT KCPConfig 19 | LIBRARY DESTINATION lib 20 | ) 21 | 22 | export(TARGETS kcp 23 | NAMESPACE KCP:: 24 | FILE "${CMAKE_CURRENT_BINARY_DIR}/KCPConfig.cmake" 25 | ) 26 | install(EXPORT KCPConfig 27 | DESTINATION "${CMAKE_INSTALL_DATADIR}/KCP/cmake" 28 | NAMESPACE KCP:: 29 | ) -------------------------------------------------------------------------------- /kcp/include/kcp/common.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Yu-Kai Lin. All rights reserved. 2 | // Use of this source code is governed by a BSD-style 3 | // license that can be found in the LICENSE file. 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include 10 | 11 | /** 12 | * @brief Namespace for the KCP library. 13 | * 14 | */ 15 | namespace kcp { 16 | 17 | /** 18 | * @brief Data structure for point correspondences. 19 | * 20 | */ 21 | struct Correspondences { 22 | /** 23 | * @brief Correspondences in terms of position, where the first and the second 24 | * parts correspond to the source and the target clouds respectively. 25 | * 26 | */ 27 | std::pair points; 28 | 29 | /** 30 | * @brief Correspondences in terms of point indices, where the first and the 31 | * second parts correspond to the source and the target clouds respectively. 32 | * 33 | */ 34 | std::pair, std::vector> indices; 35 | }; 36 | 37 | }; // namespace kcp 38 | -------------------------------------------------------------------------------- /kcp/include/kcp/keypoint.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Yu-Kai Lin. All rights reserved. 2 | // Use of this source code is governed by a BSD-style 3 | // license that can be found in the LICENSE file. 4 | 5 | #pragma once 6 | 7 | #include "kcp/common.hpp" 8 | 9 | namespace kcp { 10 | 11 | /** 12 | * @brief Namespace for the keypoint extraction. 13 | * 14 | */ 15 | namespace keypoint { 16 | 17 | /** 18 | * @brief A range image of a point cloud based on the spherical projection. 19 | * 20 | */ 21 | class RangeImage { 22 | protected: 23 | /** 24 | * @brief The point cloud. 25 | * 26 | */ 27 | Eigen::MatrixX3d cloud; 28 | 29 | /** 30 | * @brief Channel indices of raw points. 31 | * 32 | */ 33 | std::vector channel; 34 | 35 | /** 36 | * @brief The number of channels (height of the range image). It is usually 37 | * set to be the number of LiDAR beams. 38 | * 39 | */ 40 | int n_channels; 41 | 42 | /** 43 | * @brief The minimum vertical field of view (V-FOV) of the given point cloud. 44 | * 45 | */ 46 | float min_vfov_deg; 47 | 48 | /** 49 | * @brief The maximum vertical field of view (V-FOV) of the given point cloud. 50 | * 51 | */ 52 | float max_vfov_deg; 53 | 54 | /** 55 | * @brief The resolution of 360-degree horizontal field of view. 56 | * 57 | */ 58 | int hfov_resolution; 59 | 60 | /** 61 | * @brief The range image whose entry indicates the raw index of point. 62 | * 63 | */ 64 | Eigen::MatrixXi image_indices; 65 | 66 | /** 67 | * @brief The depths of points ordered by channels. 68 | * 69 | */ 70 | std::vector image_depth_sequence; 71 | 72 | /** 73 | * @brief The raw indices of points ordered by channels. 74 | * 75 | */ 76 | std::vector image_point_indices_sequence; 77 | 78 | /** 79 | * @brief The column (horizontal) indices of points ordered by channels. 80 | * 81 | */ 82 | std::vector image_col_indices_sequence; 83 | 84 | /** 85 | * @brief The starting index vector of all channels. 86 | * 87 | */ 88 | std::vector channel_start_indices; 89 | 90 | /** 91 | * @brief The ending index vector of all channels. 92 | * 93 | */ 94 | std::vector channel_end_indices; 95 | 96 | /** 97 | * @brief Compute the polar index of each point. 98 | * 99 | */ 100 | void calculate_point_cloud_properties(); 101 | 102 | /** 103 | * @brief Compute the corresponding range image of the given point cloud. 104 | * 105 | */ 106 | void calculate_range_image(); 107 | 108 | public: 109 | /** 110 | * @brief Construct a new RangeImage object. 111 | * 112 | * @param cloud The point cloud. 113 | * @param n_channels The number of channels (height of the range image). It is 114 | * usually set to be the number of LiDAR beams. 115 | * @param min_vfov_deg The minimum vertical field of view (V-FOV) of the given 116 | * point cloud. 117 | * @param max_vfov_deg The maximum vertical field of view (V-FOV) of the given 118 | * point cloud. 119 | * @param hfov_resolution The resolution of 360-degree horizontal field of 120 | * view. 121 | */ 122 | RangeImage(Eigen::MatrixX3d cloud, 123 | int n_channels = 32, 124 | float min_vfov_deg = -30.0, 125 | float max_vfov_deg = 10.0, 126 | int hfov_resolution = 1800); 127 | 128 | /** 129 | * @brief Get the point cloud. 130 | * 131 | * @return const Eigen::MatrixX3d& 132 | */ 133 | const Eigen::MatrixX3d &get_cloud() const { return this->cloud; } 134 | 135 | /** 136 | * @brief Get the number of channels. 137 | * 138 | * @return int 139 | */ 140 | int get_n_channels() const { return this->n_channels; } 141 | 142 | /** 143 | * @brief Get the size of parameterized points for the range image. 144 | * 145 | * @return size_t 146 | */ 147 | size_t get_image_sequence_size() const { return this->image_depth_sequence.size(); } 148 | 149 | /** 150 | * @brief Get the range image whose entry indicates the raw index of point. 151 | * 152 | * @return const Eigen::MatrixXi& 153 | */ 154 | const Eigen::MatrixXi &get_image_indices() const { return this->image_indices; } 155 | 156 | /** 157 | * @brief Get the depths of points ordered by channels. 158 | * 159 | * @return const std::vector& 160 | */ 161 | const std::vector &get_image_depth_sequence() const { return this->image_depth_sequence; } 162 | 163 | /** 164 | * @brief Get the raw indices of points ordered by channels. 165 | * 166 | * @return const std::vector& 167 | */ 168 | const std::vector &get_image_point_indices_sequence() const { return this->image_point_indices_sequence; } 169 | 170 | /** 171 | * @brief Get the column (horizontal) indices of points ordered by channels. 172 | * 173 | * @return const std::vector& 174 | */ 175 | const std::vector &get_image_col_indices_sequence() const { return this->image_col_indices_sequence; } 176 | 177 | /** 178 | * @brief Get the starting index vector of all channels. 179 | * 180 | * @return const std::vector& 181 | */ 182 | const std::vector &get_channel_start_indices() const { return this->channel_start_indices; } 183 | 184 | /** 185 | * @brief Get the ending index vector of all channels. 186 | * 187 | * @return const std::vector& 188 | */ 189 | const std::vector &get_channel_end_indices() const { return this->channel_end_indices; } 190 | }; 191 | 192 | /** 193 | * @brief The multi-scale curvature class for extracting corner points and plane 194 | * points based on the range image. 195 | * 196 | * @see RangeImage The range image class. 197 | * 198 | */ 199 | class MultiScaleCurvature { 200 | public: 201 | /** 202 | * @brief Enum class of point labels. 203 | * 204 | */ 205 | enum class Label { 206 | UNDEFINED, 207 | NORMAL, 208 | OCCLUDED, 209 | PARALLEL, 210 | CORNER, 211 | PLANE, 212 | AMBIGUOUS 213 | }; 214 | 215 | protected: 216 | /** 217 | * @brief The range image. 218 | * 219 | */ 220 | RangeImage range_image; 221 | 222 | /** 223 | * @brief Multi-scale curvatures stored as a vector of {kappa, index}. 224 | * 225 | */ 226 | std::vector> curvature; 227 | 228 | /** 229 | * @brief Labels of the points. 230 | * 231 | */ 232 | std::vector