diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..bfce055 --- /dev/null +++ b/.clang-format @@ -0,0 +1,10 @@ +BasedOnStyle: Google +UseTab: Never +IndentWidth: 4 +AccessModifierOffset: -4 +ColumnLimit: 100 +BinPackParameters: false +SortIncludes: true +Standard: c++17 +DerivePointerAlignment: false +PointerAlignment: Right diff --git a/.cmake-format.yaml b/.cmake-format.yaml new file mode 100644 index 0000000..36140f3 --- /dev/null +++ b/.cmake-format.yaml @@ -0,0 +1,4 @@ +enable_markup: false +line_width: 120 +format: + max_subgroups_hwrap: 5 diff --git a/.gitignore b/.gitignore new file mode 100755 index 0000000..c896448 --- /dev/null +++ b/.gitignore @@ -0,0 +1,137 @@ +*cmake-build-debug*/ +*cmake-build-release*/ +*build*/ + +.idea/ +.vscode/ + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### CMake ### +CMakeLists.txt.user +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +Makefile +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake +_deps + +### CMake Patch ### +# External projects +*-prefix/ + +### Code ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Eclipse ### +.metadata +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.settings/ +.loadpath +.recommenders + +# External tool builders +.externalToolBuilders/ + + +# PyDev specific (Python IDE for Eclipse) +*.pydevproject + +# CDT-specific (C/C++ Development Tooling) +.cproject + +# CDT- autotools +.autotools + +# Java annotation processor (APT) +.factorypath + +# PDT-specific (PHP Development Tools) +.buildpath + +# sbteclipse plugin +.target + +# Tern plugin +.tern-project + +# TeXlipse plugin +.texlipse + +# STS (Spring Tool Suite) +.springBeans + +# Code Recommenders +.recommenders/ + +# Annotation Processing +.apt_generated/ + +# Scala IDE specific (Scala & Java development for Eclipse) +.cache-main +.scala_dependencies +.worksheet + +### Eclipse Patch ### +# Eclipse Core +.project + +# JDT-specific (Eclipse Java Development Tools) +.classpath + +# Annotation Processing +.apt_generated + +.sts4-cache/ + +# Pybinding +python/build/ +*.pyc + +# End of https://www.gitignore.io/api/c++,code,cmake,eclipse \ No newline at end of file diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..ced0246 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,26 @@ +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-added-large-files +- repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.0 + hooks: + - id: clang-format +- repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + args: [--config=python/pyproject.toml] +- repo: https://github.com/ahans/cmake-format-precommit + rev: 8e52fb6506f169dddfaa87f88600d765fca48386 + hooks: + - id: cmake-format +- repo: https://github.com/pycqa/isort + rev: 5.12.0 + hooks: + - id: isort + args: ["--settings-path=python/pyproject.toml"] diff --git a/CITATION.cff b/CITATION.cff new file mode 100644 index 0000000..9405846 --- /dev/null +++ b/CITATION.cff @@ -0,0 +1,15 @@ +cff-version: 1.2.0 +preferred-citation: + title: "GenZ-ICP: Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting" + doi: "10.1109/LRA.2024.3498779" + year: "2024" + type: article + journal: "IEEE Robotics and Automation Letters (RA-L)" + codeurl: https://github.com/cocel-postech/genz-icp + authors: + - family-names: Lee + given-names: Daehan + - family-names: Lim + given-names: Hyungtae + - family-names: Han + given-names: Soohee diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..52551e9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..f03b2f2 --- /dev/null +++ b/README.md @@ -0,0 +1,159 @@ +
+

GenZ-ICP

+ + + + + + +
+
+ Video +   •   + Install +   •   + ROS +   •   + Paper +   •   + Contact Us +
+
+

animated

+ + [GenZ-ICP][arXivlink] is a **Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting** +
+ +[arXivlink]: https://arxiv.org/abs/2411.06766 + +## :gear: How to build & run + +### ROS1 + +#### How to build + +You should not need any extra dependency, just clone and build: + +```sh +mkdir -p ~/catkin_ws/src +cd ~/catkin_ws/src +git clone https://github.com/cocel-postech/genz-icp.git +cd .. +catkin build genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release +source ~/catkin_ws/devel/setup.bash +``` + +#### How to run + +#### Option 1 + +If you want to use a pre-tuned parameter set, you need to provide the **config file** with the **topic name** as arguments: + +```sh +roslaunch genz_icp odometry.launch topic:= config_file:=.yaml +``` +```sh +rosbag play .bag +``` + +For example, + +```sh +roslaunch genz_icp odometry.launch topic:=/velodyne_points config_file:=long_corridor.yaml +``` +```sh +rosbag play subt_mrs_long_corridor.bag +``` + +The original bagfile for the **Long_Corridor** sequence of SubT-MRS dataset can be downloaded from [here][long_corridor_original_link] + +`subt_mrs_long_corridor.bag` includes only the `/velodyne_points` topic and can be downloaded from [here][long_corridor_link] + +[long_corridor_original_link]: https://superodometry.com/iccv23_challenge_Mul +[long_corridor_link]: https://cocel.synology.me:5001/sharing/JZQalfEqQ + + +#### Option 2 + +Otherwise, the only required argument to provide is the **topic name**: + +```sh +roslaunch genz_icp odometry.launch topic:= +``` +```sh +rosbag play .bag +``` + +### ROS2 + +#### How to build + +You should not need any extra dependency, just clone and build: + +```sh +mkdir -p ~/colcon_ws/src +cd ~/colcon_ws/src +git clone https://github.com/cocel-postech/genz-icp.git +cd .. +colcon build --packages-select genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release +source ~/colcon_ws/install/setup.bash +``` + +#### How to run + +The only required argument to provide is the **topic name**: + +```sh +ros2 launch genz_icp odometry.launch.py topic:= +``` + +and then, + +```sh +ros2 bag play .mcap +``` + +### Python + +Will be available in an upcoming update + +## :pushpin: Todo list +- [ ] Python support for GenZ-ICP + +## :pencil: Citation + +If you use our codes, please cite our paper ([arXiv][arXivLink], [IEEE *Xplore*][genzicpIEEElink]) +``` +@ARTICLE{lee2024genzicp, + author={Lee, Daehan and Lim, Hyungtae and Han, Soohee}, + journal={IEEE Robotics and Automation Letters (RA-L)}, + title={{GenZ-ICP: Generalizable and Degeneracy-Robust LiDAR Odometry Using an Adaptive Weighting}}, + year={2024}, + volume={}, + number={}, + pages={1-8}, + keywords={Localization;Mapping;SLAM}, + doi={10.1109/LRA.2024.3498779} +} +``` + +[genzicpIEEElink]: https://ieeexplore.ieee.org/document/10753079 + +## :pray: Acknowledgement + +Many thanks to [Ignacio Vizzo][nacholink] to provide outstanding LiDAR odometry codes! + +Please refer to [KISS-ICP][kissicplink] for more information + +[nacholink]: https://github.com/nachovizzo +[kissicplink]: https://github.com/PRBonn/kiss-icp + + +## :mailbox: Contact information + +If you have any questions, please do not hesitate to contact us +* [Daehan Lee][dhlink] :envelope: daehanlee `at` postech `dot` ac `dot` kr +* [Hyungtae Lim][htlink] :envelope: shapelim `at` mit `dot` edu + +[dhlink]: https://github.com/Daehan2Lee +[htlink]: https://github.com/LimHyungTae diff --git a/cpp/COLCON_IGNORE b/cpp/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/cpp/genz_icp/3rdparty/eigen/LICENSE b/cpp/genz_icp/3rdparty/eigen/LICENSE new file mode 100644 index 0000000..de5b632 --- /dev/null +++ b/cpp/genz_icp/3rdparty/eigen/LICENSE @@ -0,0 +1,18 @@ +Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: + http://www.mozilla.org/MPL/2.0/ + http://www.mozilla.org/MPL/2.0/FAQ.html + +Some files contain third-party code under BSD or LGPL licenses, whence the other +COPYING.* files here. + +All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. +For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. + +If you want to guarantee that the Eigen code that you are #including is licensed +under the MPL2 and possibly more permissive licenses (like BSD), #define this +preprocessor symbol: + EIGEN_MPL2_ONLY +For example, with most compilers, you could add this to your project CXXFLAGS: + -DEIGEN_MPL2_ONLY +This will cause a compilation error to be generated if you #include any code that is +LGPL licensed. diff --git a/cpp/genz_icp/3rdparty/eigen/eigen.cmake b/cpp/genz_icp/3rdparty/eigen/eigen.cmake new file mode 100644 index 0000000..8ad2121 --- /dev/null +++ b/cpp/genz_icp/3rdparty/eigen/eigen.cmake @@ -0,0 +1,46 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# TODO: Yet another manual release dne by nacho. This should be updated whenever the Eigen team +# release a new version that is not 3.4. That version does not include this necessary changes: +# - https://gitlab.com/libeigen/eigen/-/merge_requests/893/diffs + +set(EIGEN_BUILD_DOC OFF CACHE BOOL "Don't build Eigen docs") +set(EIGEN_BUILD_TESTING OFF CACHE BOOL "Don't build Eigen tests") +set(EIGEN_BUILD_PKGCONFIG OFF CACHE BOOL "Don't build Eigen pkg-config") +set(EIGEN_BUILD_BLAS OFF CACHE BOOL "Don't build blas module") +set(EIGEN_BUILD_LAPACK OFF CACHE BOOL "Don't build lapack module") + +include(FetchContent) +FetchContent_Declare(eigen URL https://github.com/nachovizzo/eigen/archive/refs/tags/3.4.90.tar.gz) +if(NOT eigen_POPULATED) + FetchContent_Populate(eigen) + if(${CMAKE_VERSION} GREATER_EQUAL 3.25) + add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) + else() + # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will + # consider this 3rdparty headers as source code and fail due the -Werror flag. + add_subdirectory(${eigen_SOURCE_DIR} ${eigen_BINARY_DIR} EXCLUDE_FROM_ALL) + get_target_property(eigen_include_dirs eigen INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(eigen PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${eigen_include_dirs}") + endif() +endif() diff --git a/cpp/genz_icp/3rdparty/find_dependencies.cmake b/cpp/genz_icp/3rdparty/find_dependencies.cmake new file mode 100644 index 0000000..e9fd9ae --- /dev/null +++ b/cpp/genz_icp/3rdparty/find_dependencies.cmake @@ -0,0 +1,42 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# Silence timestamp warning +if(CMAKE_VERSION VERSION_GREATER 3.24) + cmake_policy(SET CMP0135 OLD) +endif() + +function(find_external_dependecy PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) + string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) + set(USE_FROM_SYSTEM_OPTION "USE_SYSTEM_${PACKAGE_NAME_UP}") + if(${${USE_FROM_SYSTEM_OPTION}}) + find_package(${PACKAGE_NAME} QUIET NO_MODULE) + endif() + if(NOT ${${USE_FROM_SYSTEM_OPTION}} OR NOT TARGET ${TARGET_NAME}) + set(${USE_FROM_SYSTEM_OPTION} OFF PARENT_SCOPE) + include(${INCLUDED_CMAKE_PATH}) + endif() +endfunction() + +find_external_dependecy("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/eigen/eigen.cmake") +find_external_dependecy("Sophus" "Sophus::Sophus" "${CMAKE_CURRENT_LIST_DIR}/sophus/sophus.cmake") +find_external_dependecy("TBB" "TBB::tbb" "${CMAKE_CURRENT_LIST_DIR}/tbb/tbb.cmake") +find_external_dependecy("tsl-robin-map" "tsl::robin_map" "${CMAKE_CURRENT_LIST_DIR}/tsl_robin/tsl_robin.cmake") diff --git a/cpp/genz_icp/3rdparty/sophus/LICENSE b/cpp/genz_icp/3rdparty/sophus/LICENSE new file mode 100644 index 0000000..9f9cc4d --- /dev/null +++ b/cpp/genz_icp/3rdparty/sophus/LICENSE @@ -0,0 +1,19 @@ +Copyright (c) 2008-2015 Jesse Beder. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWAR diff --git a/cpp/genz_icp/3rdparty/sophus/sophus.cmake b/cpp/genz_icp/3rdparty/sophus/sophus.cmake new file mode 100644 index 0000000..4067799 --- /dev/null +++ b/cpp/genz_icp/3rdparty/sophus/sophus.cmake @@ -0,0 +1,30 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +include(FetchContent) + +set(SOPHUS_USE_BASIC_LOGGING ON CACHE BOOL "Don't use fmt for Sophus libraru") +set(BUILD_SOPHUS_TESTS OFF CACHE BOOL "Don't build Sophus tests") +set(BUILD_SOPHUS_EXAMPLES OFF CACHE BOOL "Don't build Sophus Examples") + +# TODO: after https://github.com/strasdat/Sophus/pull/502 gets merged go back to mainstream +FetchContent_Declare(sophus SYSTEM URL https://github.com/nachovizzo/Sophus/archive/refs/tags/1.22.11.tar.gz) +FetchContent_MakeAvailable(sophus) diff --git a/cpp/genz_icp/3rdparty/tbb/LICENSE b/cpp/genz_icp/3rdparty/tbb/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/cpp/genz_icp/3rdparty/tbb/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/cpp/genz_icp/3rdparty/tbb/tbb.cmake b/cpp/genz_icp/3rdparty/tbb/tbb.cmake new file mode 100644 index 0000000..7f4fe42 --- /dev/null +++ b/cpp/genz_icp/3rdparty/tbb/tbb.cmake @@ -0,0 +1,42 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# option(BUILD_SHARED_LIBS ON) +option(BUILD_SHARED_LIBS OFF) +option(TBBMALLOC_BUILD OFF) +option(TBB_EXAMPLES OFF) +option(TBB_STRICT OFF) +option(TBB_TEST OFF) + +include(FetchContent) +FetchContent_Declare(tbb URL https://github.com/oneapi-src/oneTBB/archive/refs/tags/v2021.8.0.tar.gz) +if(NOT tbb_POPULATED) + FetchContent_Populate(tbb) + if(${CMAKE_VERSION} GREATER_EQUAL 3.25) + add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} SYSTEM EXCLUDE_FROM_ALL) + else() + # Emulate the SYSTEM flag introduced in CMake 3.25. Withouth this flag the compiler will + # consider this 3rdparty headers as source code and fail due the -Werror flag. + add_subdirectory(${tbb_SOURCE_DIR} ${tbb_BINARY_DIR} EXCLUDE_FROM_ALL) + get_target_property(tbb_include_dirs tbb INTERFACE_INCLUDE_DIRECTORIES) + set_target_properties(tbb PROPERTIES INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${tbb_include_dirs}") + endif() +endif() diff --git a/cpp/genz_icp/3rdparty/tsl_robin/LICENSE b/cpp/genz_icp/3rdparty/tsl_robin/LICENSE new file mode 100644 index 0000000..e9c5ae9 --- /dev/null +++ b/cpp/genz_icp/3rdparty/tsl_robin/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2017 Thibaut Goetghebuer-Planchon + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/cpp/genz_icp/3rdparty/tsl_robin/tsl_robin.cmake b/cpp/genz_icp/3rdparty/tsl_robin/tsl_robin.cmake new file mode 100644 index 0000000..b8a8565 --- /dev/null +++ b/cpp/genz_icp/3rdparty/tsl_robin/tsl_robin.cmake @@ -0,0 +1,24 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +include(FetchContent) +FetchContent_Declare(tessil SYSTEM URL https://github.com/Tessil/robin-map/archive/refs/tags/v1.2.1.tar.gz) +FetchContent_MakeAvailable(tessil) diff --git a/cpp/genz_icp/CMakeLists.txt b/cpp/genz_icp/CMakeLists.txt new file mode 100644 index 0000000..869dc24 --- /dev/null +++ b/cpp/genz_icp/CMakeLists.txt @@ -0,0 +1,52 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +cmake_minimum_required(VERSION 3.16...3.26) +project(genz_icp_cpp VERSION 0.3.0 LANGUAGES CXX) + +# Setup build options +option(USE_CCACHE "Build using Ccache if found on the path" ON) +option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) +option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) +option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) +option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) + +# ccache setup +if(USE_CCACHE) + find_program(CCACHE_PATH ccache) + if(CCACHE_PATH) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + message(STATUS "Using ccache: ${CCACHE_PATH}") + endif() +endif() + +# Set build type (repeat here for C++ only consumers) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +include(3rdparty/find_dependencies.cmake) +include(cmake/CompilerOptions.cmake) + +add_subdirectory(core) +add_subdirectory(metrics) +add_subdirectory(pipeline) diff --git a/cpp/genz_icp/LICENSE b/cpp/genz_icp/LICENSE new file mode 100644 index 0000000..52551e9 --- /dev/null +++ b/cpp/genz_icp/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/cpp/genz_icp/README.md b/cpp/genz_icp/README.md new file mode 100644 index 0000000..1d5bdaf --- /dev/null +++ b/cpp/genz_icp/README.md @@ -0,0 +1,14 @@ +# GenZ-ICP C++ Library + +## How to build + +```sh +cmake -Bbuild +cmake --build build -j$(nproc --all) +``` + +## Dependencies + +The cmake build system should handle all dependencies for you. In case you have some particular +requirements for the library dependencies, by default the build system will attempt to use the +ones you have installed on your local system. diff --git a/cpp/genz_icp/cmake/CompilerOptions.cmake b/cpp/genz_icp/cmake/CompilerOptions.cmake new file mode 100644 index 0000000..90ce10c --- /dev/null +++ b/cpp/genz_icp/cmake/CompilerOptions.cmake @@ -0,0 +1,52 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +function(set_global_target_properties target) + target_compile_features(${target} PUBLIC cxx_std_17) + target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) + target_compile_options( + ${target} + PRIVATE # MSVC + $<$:/W4> + $<$:/WX> + # Clang/AppleClang + $<$:-fcolor-diagnostics> + $<$:-Werror> + $<$:-Wall> + $<$:-Wextra> + $<$:-Wconversion> + $<$:-Wno-sign-conversion> + # GNU + $<$:-fdiagnostics-color=always> + $<$:-Werror> + $<$:-Wall> + $<$:-Wextra> + $<$:-pedantic> + $<$:-Wcast-align> + $<$:-Wcast-qual> + $<$:-Wconversion> + $<$:-Wdisabled-optimization> + $<$:-Woverloaded-virtual>) + set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) + get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) + target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC $ $) +endfunction() diff --git a/cpp/genz_icp/core/CMakeLists.txt b/cpp/genz_icp/core/CMakeLists.txt new file mode 100644 index 0000000..d5c462a --- /dev/null +++ b/cpp/genz_icp/core/CMakeLists.txt @@ -0,0 +1,26 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +add_library(core STATIC) +add_library(genz_icp::core ALIAS core) +target_sources(core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp) +target_link_libraries(core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus) +set_global_target_properties(core) diff --git a/cpp/genz_icp/core/Deskew.cpp b/cpp/genz_icp/core/Deskew.cpp new file mode 100644 index 0000000..b170188 --- /dev/null +++ b/cpp/genz_icp/core/Deskew.cpp @@ -0,0 +1,48 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "Deskew.hpp" + +#include + +#include +#include +#include + +namespace { +constexpr double mid_pose_timestamp{0.5}; +} // namespace + +namespace genz_icp { +std::vector DeSkewScan(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &start_pose, + const Sophus::SE3d &finish_pose) { + const auto delta_pose = (start_pose.inverse() * finish_pose).log(); + + std::vector corrected_frame(frame.size()); + tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { + const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose); // original + corrected_frame[i] = motion * frame[i]; + }); + return corrected_frame; +} +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Deskew.hpp b/cpp/genz_icp/core/Deskew.hpp new file mode 100644 index 0000000..81bfd2a --- /dev/null +++ b/cpp/genz_icp/core/Deskew.hpp @@ -0,0 +1,36 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include + +namespace genz_icp { + +/// Compensate the frame by estimatng the velocity between the given poses +std::vector DeSkewScan(const std::vector &frame, + const std::vector ×tamps, + const Sophus::SE3d &start_pose, + const Sophus::SE3d &finish_pose); + +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Preprocessing.cpp b/cpp/genz_icp/core/Preprocessing.cpp new file mode 100644 index 0000000..b6a1619 --- /dev/null +++ b/cpp/genz_icp/core/Preprocessing.cpp @@ -0,0 +1,131 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "Preprocessing.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace { +using Voxel = Eigen::Vector3i; +struct VoxelHash { + size_t operator()(const Voxel &voxel) const { + const uint32_t *vec = reinterpret_cast(voxel.data()); + return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791); + } +}; +} // namespace + +namespace genz_icp { + +std::vector VoxelDownsample(const std::vector &frame, + double voxel_size) { + tsl::robin_map grid; + grid.reserve(frame.size()); + + for (const auto &point : frame) { + const auto voxel = Voxel((point / voxel_size).cast()); + if (grid.contains(voxel)) continue; + grid.insert({voxel, point}); + } + + std::vector frame_downsampled; + frame_downsampled.reserve(grid.size()); + for (const auto &[voxel, point] : grid) { + (void)voxel; + frame_downsampled.emplace_back(point); + } + //std::cout<<"Downsampled Map Size: "< VoxelDownsampleForMap(const std::vector &frame, + double voxel_size) { + tsl::robin_map grid; + grid.reserve(frame.size()); + + for (const auto &point : frame) { + const auto voxel = Voxel((point / voxel_size).cast()); + if (grid.contains(voxel)) continue; + grid.insert({voxel, point}); + } + + std::vector frame_downsampled; + frame_downsampled.reserve(grid.size()); + for (const auto &[voxel, point] : grid) { + (void)voxel; + frame_downsampled.emplace_back(point); + } + std::cout<<"Downsampled Map Size: "< VoxelDownsampleForScan(const std::vector &frame, + double voxel_size) { + tsl::robin_map grid; + grid.reserve(frame.size()); + + for (const auto &point : frame) { + const auto voxel = Voxel((point / voxel_size).cast()); + if (grid.contains(voxel)) continue; + grid.insert({voxel, point}); + } + + std::vector frame_downsampled; + frame_downsampled.reserve(grid.size()); + for (const auto &[voxel, point] : grid) { + (void)voxel; + frame_downsampled.emplace_back(point); + } + std::cout<<"Downsampled Scan Size: "< Preprocess(const std::vector &frame, + double max_range, + double min_range) { + std::vector inliers; + std::copy_if(frame.cbegin(), frame.cend(), std::back_inserter(inliers), [&](const auto &pt) { + const double norm = pt.norm(); + return norm < max_range && norm > min_range; + }); + return inliers; +} + +std::vector CorrectKITTIScan(const std::vector &frame) { + constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; + std::vector corrected_frame(frame.size()); + tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) { + const auto &pt = frame[i]; + const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.)); + corrected_frame[i] = + Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt; + }); + return corrected_frame; +} +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Preprocessing.hpp b/cpp/genz_icp/core/Preprocessing.hpp new file mode 100644 index 0000000..8554565 --- /dev/null +++ b/cpp/genz_icp/core/Preprocessing.hpp @@ -0,0 +1,49 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include +#include + +namespace genz_icp { + +/// Crop the frame with max/min ranges +std::vector Preprocess(const std::vector &frame, + double max_range, + double min_range); + +/// This function only applies for the KITTI dataset, and should NOT be used by any other dataset, +/// the original idea and part of the implementation is taking from CT-ICP(Although IMLS-SLAM +/// Originally introduced the calibration factor) +std::vector CorrectKITTIScan(const std::vector &frame); + +std::vector VoxelDownsample(const std::vector &frame, + double voxel_size); + +std::vector VoxelDownsampleForMap(const std::vector &frame, + double voxel_size); + +std::vector VoxelDownsampleForScan(const std::vector &frame, + double voxel_size); +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Registration.cpp b/cpp/genz_icp/core/Registration.cpp new file mode 100644 index 0000000..8e4a4a3 --- /dev/null +++ b/cpp/genz_icp/core/Registration.cpp @@ -0,0 +1,221 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "Registration.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace Eigen { +using Matrix6d = Eigen::Matrix; +using Matrix3_6d = Eigen::Matrix; +using Vector6d = Eigen::Matrix; +} // namespace Eigen + +namespace { + +inline double square(double x) { return x * x; } + +struct ResultTuple { + ResultTuple() { + JTJ.setZero(); + JTr.setZero(); + } + + ResultTuple operator+(const ResultTuple &other) { + this->JTJ += other.JTJ; + this->JTr += other.JTr; + return *this; + } + + Eigen::Matrix6d JTJ; + Eigen::Vector6d JTr; +}; + +void TransformPoints(const Sophus::SE3d &T, std::vector &points) { + std::transform(points.cbegin(), points.cend(), points.begin(), + [&](const auto &point) { return T * point; }); +} + +//Build the linear system for the GenZ-ICP +std::tuple BuildLinearSystem( + const std::vector &src_planar, + const std::vector &tgt_planar, + const std::vector &normals, + const std::vector &src_non_planar, + const std::vector &tgt_non_planar, + double kernel, + double alpha) { + + struct ResultTuple { + Eigen::Matrix6d JTJ; + Eigen::Vector6d JTr; + + ResultTuple() : JTJ(Eigen::Matrix6d::Zero()), JTr(Eigen::Vector6d::Zero()) {} + + ResultTuple operator+(const ResultTuple &other) const { + ResultTuple result; + result.JTJ = JTJ + other.JTJ; + result.JTr = JTr + other.JTr; + return result; + } + }; + + // Point-to-Plane Jacobian and Residual + auto compute_jacobian_and_residual_planar = [&](auto i) { + double r_planar = (src_planar[i] - tgt_planar[i]).dot(normals[i]); // residual + Eigen::Matrix J_planar; // Jacobian matrix + J_planar.block<1, 3>(0, 0) = normals[i].transpose(); + J_planar.block<1, 3>(0, 3) = (src_planar[i].cross(normals[i])).transpose(); + return std::make_tuple(J_planar, r_planar); + }; + + // Point-to-Point Jacobian and Residual + auto compute_jacobian_and_residual_non_planar = [&](auto i) { + const Eigen::Vector3d r_non_planar = src_non_planar[i] - tgt_non_planar[i]; + Eigen::Matrix3_6d J_non_planar; + J_non_planar.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + J_non_planar.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(src_non_planar[i]); + return std::make_tuple(J_non_planar, r_non_planar); + }; + + auto compute = [&](const tbb::blocked_range &r, ResultTuple J) -> ResultTuple { + auto Weight = [&](double residual) { + return square(kernel) / square(kernel + residual); + }; + auto &[JTJ_private, JTr_private] = J; + for (size_t i = r.begin(); i < r.end(); ++i) { + if (i < src_planar.size()) { // Point-to-Plane + const auto &[J_planar, r_planar] = compute_jacobian_and_residual_planar(i); + double w_planar = Weight(r_planar); + JTJ_private.noalias() += alpha * J_planar.transpose() * w_planar * J_planar; + JTr_private.noalias() += alpha * J_planar.transpose() * w_planar * r_planar; + } else { // Point-to-Point + size_t index = i - src_planar.size(); + if (index < src_non_planar.size()) { + const auto &[J_non_planar, r_non_planar] = compute_jacobian_and_residual_non_planar(index); + const double w_non_planar = Weight(r_non_planar.squaredNorm()); + JTJ_private.noalias() += (1 - alpha) * J_non_planar.transpose() * w_non_planar * J_non_planar; + JTr_private.noalias() += (1 - alpha) * J_non_planar.transpose() * w_non_planar * r_non_planar; + } + } + } + return J; + }; + + + size_t total_size = src_planar.size() + src_non_planar.size(); + const auto &[JTJ, JTr] = tbb::parallel_reduce( + tbb::blocked_range(0, total_size), + ResultTuple(), + compute, + [](const ResultTuple &a, const ResultTuple &b) { + return a + b; + }); + + return std::make_tuple(JTJ, JTr); +} +} // namespace + +namespace genz_icp { + +Registration::Registration(int max_num_iteration, double convergence_criterion) + : max_num_iterations_(max_num_iteration), + convergence_criterion_(convergence_criterion) {} + +std::tuple, std::vector> Registration::RegisterFrame(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel) { + + // for visualization + std::vector final_planar_points; + std::vector final_non_planar_points; + final_planar_points.clear(); + final_non_planar_points.clear(); + + if (voxel_map.Empty()) return std::make_tuple(initial_guess, final_planar_points, final_non_planar_points); + + std::vector source = frame; + TransformPoints(initial_guess, source); + + // GenZ-ICP-loop + Sophus::SE3d T_icp = Sophus::SE3d(); + for (int j = 0; j < max_num_iterations_; ++j) { + const auto &[src_planar, tgt_planar, normals, src_non_planar, tgt_non_planar, planar_count, non_planar_count] = voxel_map.GetCorrespondences(source, max_correspondence_distance); + double alpha = static_cast(planar_count) / static_cast(planar_count + non_planar_count); + const auto &[JTJ, JTr] = BuildLinearSystem(src_planar, tgt_planar, normals, src_non_planar, tgt_non_planar, kernel, alpha); + const Eigen::Vector6d dx = JTJ.ldlt().solve(-JTr); + const Sophus::SE3d estimation = Sophus::SE3d::exp(dx); + TransformPoints(estimation, source); + // Update iterations + T_icp = estimation * T_icp; + + // Only for visualization + const auto VisualizeStatus = [](const size_t planar_count, const size_t non_planar_count, const double alpha) { + const int bar_width = 52; // the sizes are parameterized + const std::string planar_color = "\033[1;38;2;0;119;187m"; + const std::string non_planar_color = "\033[1;38;2;238;51;119m"; + const std::string alpha_color = "\033[1;32m"; + printf("\033[2J\033[1;1H"); // Clear terminal + std::cout << "====================== GenZ-ICP ======================\n"; + std::cout << non_planar_color << "# of non-planar points: " << non_planar_count << ", "; + std::cout << planar_color << "# of planar points: " << planar_count << "\033[0m\n"; + + std::cout << "Unstructured <----- "; + std::cout << alpha_color << "alpha: " << std::fixed << std::setprecision(3) << alpha << "\033[0m"; + std::cout << " -----> Structured\n"; + + const int alpha_location = static_cast(bar_width * alpha); + std::cout << "["; + for (int i = 0; i < bar_width; ++i) { + if (i == alpha_location) { + std::cout << "\033[1;32m█\033[0m"; + } else { + std::cout << "-"; + } + } + std::cout << "]\n"; + std::cout.flush(); + }; + + // Termination criteria + if (dx.norm() < convergence_criterion_){ + VisualizeStatus(planar_count, non_planar_count, alpha); + final_planar_points = src_planar; + final_non_planar_points = src_non_planar; + break; + } + } + + // // Spit the final transformation + return std::make_tuple(T_icp * initial_guess, final_planar_points, final_non_planar_points); +} + +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Registration.hpp b/cpp/genz_icp/core/Registration.hpp new file mode 100644 index 0000000..4be03df --- /dev/null +++ b/cpp/genz_icp/core/Registration.hpp @@ -0,0 +1,45 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include +#include + +#include "VoxelHashMap.hpp" + +namespace genz_icp { + +struct Registration { + explicit Registration(int max_num_iteration, double convergence_criterion); + + std::tuple, std::vector> RegisterFrame(const std::vector &frame, + const VoxelHashMap &voxel_map, + const Sophus::SE3d &initial_guess, + double max_correspondence_distance, + double kernel); + + int max_num_iterations_; + double convergence_criterion_; +}; +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Threshold.cpp b/cpp/genz_icp/core/Threshold.cpp new file mode 100644 index 0000000..0283612 --- /dev/null +++ b/cpp/genz_icp/core/Threshold.cpp @@ -0,0 +1,51 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "Threshold.hpp" + +#include +#include + +namespace { +double ComputeModelError(const Sophus::SE3d &model_deviation, double max_range) { + const double theta = Eigen::AngleAxisd(model_deviation.rotationMatrix()).angle(); + const double delta_rot = 2.0 * max_range * std::sin(theta / 2.0); + const double delta_trans = model_deviation.translation().norm(); + return delta_trans + delta_rot; +} +} // namespace + +namespace genz_icp { + +double AdaptiveThreshold::ComputeThreshold() { + double model_error = ComputeModelError(model_deviation_, max_range_); + if (model_error > min_motion_th_) { + model_error_sse2_ += model_error * model_error; + num_samples_++; + } + + if (num_samples_ < 1) { + return initial_threshold_; + } + return std::sqrt(model_error_sse2_ / num_samples_); +} + +} // namespace genz_icp diff --git a/cpp/genz_icp/core/Threshold.hpp b/cpp/genz_icp/core/Threshold.hpp new file mode 100644 index 0000000..1ab10ca --- /dev/null +++ b/cpp/genz_icp/core/Threshold.hpp @@ -0,0 +1,53 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include + +namespace genz_icp { + +struct AdaptiveThreshold { + explicit AdaptiveThreshold(double initial_threshold, double min_motion_th, double max_range) + : initial_threshold_(initial_threshold), + min_motion_th_(min_motion_th), + max_range_(max_range) {} + + /// Update the current belief of the deviation from the prediction model + inline void UpdateModelDeviation(const Sophus::SE3d ¤t_deviation) { + model_deviation_ = current_deviation; + } + + /// Returns the GenZ-ICP adaptive threshold used in registration + double ComputeThreshold(); + + // configurable parameters + double initial_threshold_; + double min_motion_th_; + double max_range_; + + // Local cache for ccomputation + double model_error_sse2_ = 0; + int num_samples_ = 0; + Sophus::SE3d model_deviation_ = Sophus::SE3d(); +}; + +} // namespace genz_icp diff --git a/cpp/genz_icp/core/VoxelHashMap.cpp b/cpp/genz_icp/core/VoxelHashMap.cpp new file mode 100644 index 0000000..846e999 --- /dev/null +++ b/cpp/genz_icp/core/VoxelHashMap.cpp @@ -0,0 +1,235 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "VoxelHashMap.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +// This parameters are not intended to be changed, therefore we do not expose it +namespace { +struct ResultTuple { + ResultTuple(std::size_t n) { + source.reserve(n); + target.reserve(n); + } + std::vector source; + std::vector target; +}; +} // namespace + +namespace genz_icp { + +VoxelHashMap::Vector3dVectorTuple7 VoxelHashMap::GetCorrespondences( + const Vector3dVector &points, double max_correspondance_distance) const { + + struct ResultTuple { + Vector3dVector source; + Vector3dVector target; + Vector3dVector normals; + Vector3dVector non_planar_source; + Vector3dVector non_planar_target; + size_t planar_count = 0; // Count of planar correspondences + size_t non_planar_count = 0; // Count of non-planar correspondences + + ResultTuple() = default; + ResultTuple(size_t n) { + source.reserve(n); + target.reserve(n); + normals.reserve(n); + non_planar_source.reserve(n); + non_planar_target.reserve(n); + } + + ResultTuple operator+(const ResultTuple &other) const { + ResultTuple result(*this); + result.source.insert(result.source.end(), other.source.begin(), other.source.end()); + result.target.insert(result.target.end(), other.target.begin(), other.target.end()); + result.normals.insert(result.normals.end(), other.normals.begin(), other.normals.end()); + result.non_planar_source.insert(result.non_planar_source.end(), other.non_planar_source.begin(), other.non_planar_source.end()); + result.non_planar_target.insert(result.non_planar_target.end(), other.non_planar_target.begin(), other.non_planar_target.end()); + result.planar_count += other.planar_count; + result.non_planar_count += other.non_planar_count; + return result; + } + }; + + auto compute = [&](const tbb::blocked_range &r, ResultTuple result) -> ResultTuple { + for (size_t i = r.begin(); i != r.end(); ++i) { + const Eigen::Vector3d &point = points[i]; + + // Find the closest neighbor + Eigen::Vector3d closest_neighbor = Eigen::Vector3d::Zero(); + double closest_distance2 = std::numeric_limits::max(); + + // Collect neighbors for normal estimation + std::vector neighbors; + neighbors.reserve(27 * max_points_per_voxel_); + + // Search in the neighboring voxels + auto kx = static_cast(point[0] / voxel_size_); + auto ky = static_cast(point[1] / voxel_size_); + auto kz = static_cast(point[2] / voxel_size_); + for (int i = kx - 1; i <= kx + 1; ++i) { + for (int j = ky - 1; j <= ky + 1; ++j) { + for (int k = kz - 1; k <= kz + 1; ++k) { + Voxel voxel(i, j, k); + auto search = map_.find(voxel); + if (search != map_.end()) { + const auto &voxel_points = search->second.points; + for (const auto &voxel_point : voxel_points) { + double distance = (voxel_point - point).norm(); + if (distance < closest_distance2) { + closest_neighbor = voxel_point; + closest_distance2 = distance; + } + neighbors.emplace_back(voxel_point); + } + } + } + } + } + + if (closest_distance2 > max_correspondance_distance) continue; + //if ((closest_neighbor - point).norm() > max_correspondance_distance*2) continue; + + const size_t min_neighbors_for_normal_estimation = 5; + if (neighbors.size() >= min_neighbors_for_normal_estimation){ + + // Estimate normal using neighboring points + Eigen::Vector3d normal = Eigen::Vector3d::Zero(); + Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero(); + Eigen::Vector3d centroid = Eigen::Vector3d::Zero(); + + // Calculate the centroid of the neighbors + for (const auto& neighbor : neighbors) { + centroid += neighbor; + } + centroid /= static_cast(neighbors.size()); + + // Calculate the covariance matrix + for (const auto &neighbor : neighbors) { + Eigen::Vector3d centered = neighbor - centroid; + covariance += centered * centered.transpose(); + } + covariance /= static_cast(neighbors.size()); + + // Compute the normal as the eigenvector of the smallest eigenvalue + Eigen::SelfAdjointEigenSolver solver(covariance, Eigen::ComputeEigenvectors); + normal = solver.eigenvectors().col(0); + normal = normal.normalized(); + + // Planarity check + const auto &eigenvalues = solver.eigenvalues(); + double lambda3 = eigenvalues[0]; + double lambda2 = eigenvalues[1]; + double lambda1 = eigenvalues[2]; + + // Check if the surface is planar + bool is_planar = (lambda3 / (lambda1 + lambda2 + lambda3)) < planarity_threshold_; + + if(is_planar){ + result.source.emplace_back(point); + result.target.emplace_back(closest_neighbor); + result.normals.emplace_back(normal); + result.planar_count++; + } else if (closest_distance2 < max_correspondance_distance){ + result.non_planar_source.emplace_back(point); + result.non_planar_target.emplace_back(closest_neighbor); + result.non_planar_count++; + } + } + else if (closest_distance2 < max_correspondance_distance){ + result.non_planar_source.emplace_back(point); + result.non_planar_target.emplace_back(closest_neighbor); + result.non_planar_count++; + } + + } + return result; + }; + + const auto &[source, target, normals, non_planar_source, non_planar_target, planar_count, non_planar_count] = tbb::parallel_reduce( + tbb::blocked_range(0, points.size()), + ResultTuple(points.size()), + compute, + [&](const ResultTuple &a, const ResultTuple &b) { + return a + b; + }); + + return std::make_tuple(source, target, normals, non_planar_source, non_planar_target, planar_count, non_planar_count); +} + +std::vector VoxelHashMap::Pointcloud() const { + std::vector points; + points.reserve(max_points_per_voxel_ * map_.size()); + for (const auto &[voxel, voxel_block] : map_) { + (void)voxel; + for (const auto &point : voxel_block.points) { + points.emplace_back(point); + } + } + return points; +} + +void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) { + AddPoints(points); + RemovePointsFarFromLocation(origin); +} + +void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) { + Vector3dVector points_transformed(points.size()); + std::transform(points.cbegin(), points.cend(), points_transformed.begin(), + [&](const auto &point) { return pose * point; }); + const Eigen::Vector3d &origin = pose.translation(); + Update(points_transformed, origin); +} + +void VoxelHashMap::AddPoints(const std::vector &points) { + std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { + auto voxel = Voxel((point / voxel_size_).template cast()); + auto search = map_.find(voxel); + if (search != map_.end()) { + auto &voxel_block = search.value(); + voxel_block.AddPoint(point); + } else { + map_.insert({voxel, VoxelBlock{{point}, max_points_per_voxel_}}); + } + }); +} + +void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { + for (const auto &[voxel, voxel_block] : map_) { + const auto &pt = voxel_block.points.front(); + const auto max_distance2 = map_cleanup_radius_ * map_cleanup_radius_; + if ((pt - origin).squaredNorm() > (max_distance2)) { + map_.erase(voxel); + } + } +} +} // namespace genz_icp diff --git a/cpp/genz_icp/core/VoxelHashMap.hpp b/cpp/genz_icp/core/VoxelHashMap.hpp new file mode 100644 index 0000000..52f5fbc --- /dev/null +++ b/cpp/genz_icp/core/VoxelHashMap.hpp @@ -0,0 +1,80 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// NOTE: This implementation is heavily inspired in the original CT-ICP VoxelHashMap implementation, +// although it was heavily modifed and drastically simplified, but if you are using this module you +// should at least acknoowledge the work from CT-ICP by giving a star on GitHub +#pragma once + +#include + +#include +#include +#include + +namespace genz_icp { +struct VoxelHashMap { + using Vector3dVector = std::vector; + using Vector3dVectorTuple = std::tuple; + using Vector3dVectorTuple7 = std::tuple; + using Voxel = Eigen::Vector3i; + struct VoxelBlock { + // buffer of points with a max limit of n_points + std::vector points; + int num_points_; + inline void AddPoint(const Eigen::Vector3d &point) { + if (points.size() < static_cast(num_points_)) points.push_back(point); + } + }; + struct VoxelHash { + size_t operator()(const Voxel &voxel) const { + const uint32_t *vec = reinterpret_cast(voxel.data()); + return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + } + }; + + explicit VoxelHashMap(double voxel_size, double max_distance, double map_cleanup_radius, + double planarity_threshold, int max_points_per_voxel) + : voxel_size_(voxel_size), + max_distance_(max_distance), + map_cleanup_radius_(map_cleanup_radius), + planarity_threshold_(planarity_threshold), + max_points_per_voxel_(max_points_per_voxel) {} + + Vector3dVectorTuple7 GetCorrespondences(const Vector3dVector &points, + double max_correspondance_distance) const; + inline void Clear() { map_.clear(); } + inline bool Empty() const { return map_.empty(); } + void Update(const std::vector &points, const Eigen::Vector3d &origin); + void Update(const std::vector &points, const Sophus::SE3d &pose); + void AddPoints(const std::vector &points); + void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); + std::vector Pointcloud() const; + + double voxel_size_; + double max_distance_; + double map_cleanup_radius_; + double planarity_threshold_; + int max_points_per_voxel_; + tsl::robin_map map_; +}; +} // namespace genz_icp diff --git a/cpp/genz_icp/metrics/CMakeLists.txt b/cpp/genz_icp/metrics/CMakeLists.txt new file mode 100644 index 0000000..15b2c22 --- /dev/null +++ b/cpp/genz_icp/metrics/CMakeLists.txt @@ -0,0 +1,26 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +add_library(metrics STATIC) +add_library(genz_icp::metrics ALIAS metrics) +target_sources(metrics PRIVATE Metrics.cpp) +target_link_libraries(metrics PUBLIC Eigen3::Eigen) +set_global_target_properties(metrics) diff --git a/cpp/genz_icp/metrics/Metrics.cpp b/cpp/genz_icp/metrics/Metrics.cpp new file mode 100644 index 0000000..8ff219e --- /dev/null +++ b/cpp/genz_icp/metrics/Metrics.cpp @@ -0,0 +1,191 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include "Metrics.hpp" + +#include +#include +#include +#include +#include +#include + +// All this not so beatifull C++ functions are taken from kitti-dev-kit +namespace { +double lengths[] = {100, 200, 300, 400, 500, 600, 700, 800}; +int32_t num_lengths = 8; + +struct errors { + int32_t first_frame; + double r_err; + double t_err; + double len; + double speed; + errors(int32_t first_frame, double r_err, double t_err, double len, double speed) + : first_frame(first_frame), r_err(r_err), t_err(t_err), len(len), speed(speed) {} +}; + +std::vector TrajectoryDistances(const std::vector &poses) { + std::vector dist; + dist.push_back(0); + for (uint32_t i = 1; i < poses.size(); i++) { + const Eigen::Matrix4d &P1 = poses[i - 1]; + const Eigen::Matrix4d &P2 = poses[i]; + + double dx = P1(0, 3) - P2(0, 3); + double dy = P1(1, 3) - P2(1, 3); + double dz = P1(2, 3) - P2(2, 3); + + dist.push_back(dist[i - 1] + std::sqrt(dx * dx + dy * dy + dz * dz)); + } + + return dist; +} + +int32_t LastFrameFromSegmentLength(const std::vector &dist, + int32_t first_frame, + double len) { + for (uint32_t i = first_frame; i < dist.size(); i++) { + if (dist[i] > dist[first_frame] + len) { + return i; + } + } + return -1; +} + +double RotationError(const Eigen::Matrix4d &pose_error) { + double a = pose_error(0, 0); + double b = pose_error(1, 1); + double c = pose_error(2, 2); + double d = 0.5 * (a + b + c - 1.0); + return std::acos(std::max(std::min(d, 1.0), -1.0)); +} + +double TranslationError(const Eigen::Matrix4d &pose_error) { + double dx = pose_error(0, 3); + double dy = pose_error(1, 3); + double dz = pose_error(2, 3); + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +std::vector CalcSequenceErrors(const std::vector &poses_gt, + const std::vector &poses_result) { + // error vector + std::vector err; + + // parameters + int32_t step_size = 10; // every second + + // pre-compute distances (from ground truth as reference) + std::vector dist = TrajectoryDistances(poses_gt); + + // for all start positions do + for (uint32_t first_frame = 0; first_frame < poses_gt.size(); first_frame += step_size) { + // for all segment lengths do + for (int32_t i = 0; i < num_lengths; i++) { + // current length + double len = lengths[i]; + + // compute last frame + int32_t last_frame = LastFrameFromSegmentLength(dist, first_frame, len); + + // continue, if sequence not long enough + if (last_frame == -1) { + continue; + } + + // compute rotational and translational errors + Eigen::Matrix4d pose_delta_gt = poses_gt[first_frame].inverse() * poses_gt[last_frame]; + Eigen::Matrix4d pose_delta_result = + poses_result[first_frame].inverse() * poses_result[last_frame]; + Eigen::Matrix4d pose_error = pose_delta_result.inverse() * pose_delta_gt; + double r_err = RotationError(pose_error); + double t_err = TranslationError(pose_error); + + // compute speed + auto num_frames = static_cast(last_frame - first_frame + 1); + double speed = len / (0.1 * num_frames); + + // write to file + err.emplace_back(first_frame, r_err / len, t_err / len, len, speed); + } + } + + // return error vector + return err; +} +} // namespace + +namespace genz_icp::metrics { + +std::tuple SeqError(const std::vector &poses_gt, + const std::vector &poses_result) { + std::vector err = CalcSequenceErrors(poses_gt, poses_result); + double t_err = 0; + double r_err = 0; + + for (const auto &it : err) { + t_err += it.t_err; + r_err += it.r_err; + } + + double avg_trans_error = 100.0 * (t_err / static_cast(err.size())); + double avg_rot_error = (r_err / static_cast(err.size())) / 3.14 * 180.0; + + return std::make_tuple(avg_trans_error, avg_rot_error); +} + +std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, + const std::vector &poses_result) { + assert(poses_gt.size() == poses_result.size() && + "AbsoluteTrajectoryError| Different number of poses in ground truth and estimate"); + Eigen::MatrixXd source(3, poses_gt.size()); + Eigen::MatrixXd target(3, poses_gt.size()); + const size_t num_poses = poses_gt.size(); + // Align the two trajectories using SVD-ICP (Umeyama algorithm) + for (size_t i = 0; i < num_poses; ++i) { + source.block<3, 1>(0, i) = poses_result[i].block<3, 1>(0, 3); + target.block<3, 1>(0, i) = poses_gt[i].block<3, 1>(0, 3); + } + const Eigen::Matrix4d T_align_trajectories = Eigen::umeyama(source, target, false); + // ATE computation + double ATE_rot = 0, ATE_trans = 0; + for (size_t j = 0; j < num_poses; ++j) { + // Apply alignement matrix + const Eigen::Matrix4d T_estimate = T_align_trajectories * poses_result[j]; + const Eigen::Matrix4d &T_ground_truth = poses_gt[j]; + // Compute error in translation and rotation matrix (ungly) + const Eigen::Matrix3d delta_R = + T_ground_truth.block<3, 3>(0, 0) * T_estimate.block<3, 3>(0, 0).transpose(); + const Eigen::Vector3d delta_t = + T_ground_truth.block<3, 1>(0, 3) - delta_R * T_estimate.block<3, 1>(0, 3); + // Get angular error + const double theta = Eigen::AngleAxisd(delta_R).angle(); + // Sum of Squares + ATE_rot += theta * theta; + ATE_trans += delta_t.squaredNorm(); + } + // Get the RMSE + ATE_rot /= static_cast(num_poses); + ATE_trans /= static_cast(num_poses); + return std::make_tuple(std::sqrt(ATE_rot), std::sqrt(ATE_trans)); +} +} // namespace genz_icp::metrics diff --git a/cpp/genz_icp/metrics/Metrics.hpp b/cpp/genz_icp/metrics/Metrics.hpp new file mode 100644 index 0000000..59de35e --- /dev/null +++ b/cpp/genz_icp/metrics/Metrics.hpp @@ -0,0 +1,39 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef KITTI_UTILS_H_ +#define KITTI_UTILS_H_ + +#include +#include +#include + +namespace genz_icp::metrics { + +std::tuple SeqError(const std::vector &poses_gt, + const std::vector &poses_result); + +std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt, + const std::vector &poses_result); +} // namespace genz_icp::metrics + +#endif // KITTI_UTILS_H_ diff --git a/cpp/genz_icp/pipeline/CMakeLists.txt b/cpp/genz_icp/pipeline/CMakeLists.txt new file mode 100644 index 0000000..fcf6005 --- /dev/null +++ b/cpp/genz_icp/pipeline/CMakeLists.txt @@ -0,0 +1,27 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +add_library(pipeline STATIC) +add_library(genz_icp::pipeline ALIAS pipeline) +target_sources(pipeline PRIVATE GenZICP.cpp) +target_link_libraries(pipeline PUBLIC genz_icp::core) +set_global_target_properties(pipeline) diff --git a/cpp/genz_icp/pipeline/GenZICP.cpp b/cpp/genz_icp/pipeline/GenZICP.cpp new file mode 100644 index 0000000..47b1d91 --- /dev/null +++ b/cpp/genz_icp/pipeline/GenZICP.cpp @@ -0,0 +1,121 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include "GenZICP.hpp" + +#include +#include +#include + +#include "genz_icp/core/Deskew.hpp" +#include "genz_icp/core/Preprocessing.hpp" +#include "genz_icp/core/Registration.hpp" +#include "genz_icp/core/VoxelHashMap.hpp" + +namespace genz_icp::pipeline { + +GenZICP::Vector3dVectorTuple GenZICP::RegisterFrame(const std::vector &frame, + const std::vector ×tamps) { + const auto &deskew_frame = [&]() -> std::vector { + if (!config_.deskew || timestamps.empty()) return frame; + // TODO(Nacho) Add some asserts here to sanitize the timestamps + + // If not enough poses for the estimation, do not de-skew + const size_t N = poses().size(); + if (N <= 2) return frame; + + // Estimate linear and angular velocities + const auto &start_pose = poses_[N - 2]; + const auto &finish_pose = poses_[N - 1]; + + return DeSkewScan(frame, timestamps, start_pose, finish_pose); + + }(); + return RegisterFrame(deskew_frame); +} + +GenZICP::Vector3dVectorTuple GenZICP::RegisterFrame(const std::vector &frame) { + // Preprocess the input cloud + const auto &cropped_frame = Preprocess(frame, config_.max_range, config_.min_range); + + static double voxel_size = config_.voxel_size; + + // Voxelize + const auto &[source, frame_downsample] = Voxelize(cropped_frame, voxel_size); + + // Adapt voxel size + if (static_cast(source.size()) > config_.max_points_per_voxelized_scan){ + voxel_size += 0.01; + } + else if (static_cast(source.size()) < config_.min_points_per_voxelized_scan){ + voxel_size -= 0.01; + } + + // Get motion prediction and adaptive_threshold + const double sigma = GetAdaptiveThreshold(); + + // Compute initial_guess for ICP + const auto prediction = GetPredictionModel(); + const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d(); + const auto initial_guess = last_pose * prediction; + + // Run GenZ-ICP + const auto &[new_pose, planar_points, non_planar_points] = registration_.RegisterFrame(source, // + local_map_, // + initial_guess, // + 3.0 * sigma, // + sigma / 3.0); + const auto model_deviation = initial_guess.inverse() * new_pose; + adaptive_threshold_.UpdateModelDeviation(model_deviation); + local_map_.Update(frame_downsample, new_pose); + poses_.push_back(new_pose); + return {planar_points, non_planar_points}; +} + +GenZICP::Vector3dVectorTuple GenZICP::Voxelize(const std::vector &frame, double voxel_size) const { + //const auto voxel_size = config_.voxel_size; + const auto frame_downsample = genz_icp::VoxelDownsample(frame, voxel_size * 0.5); // localmap update + const auto source = genz_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5); // registration + return {source, frame_downsample}; +} + +double GenZICP::GetAdaptiveThreshold() { + if (!HasMoved()) { + return config_.initial_threshold; + } + return adaptive_threshold_.ComputeThreshold(); +} + +Sophus::SE3d GenZICP::GetPredictionModel() const { + Sophus::SE3d pred = Sophus::SE3d(); + const size_t N = poses_.size(); + if (N < 2) return pred; + return poses_[N - 2].inverse() * poses_[N - 1]; +} + +bool GenZICP::HasMoved() { + if (poses_.empty()) return false; + const double motion = (poses_.front().inverse() * poses_.back()).translation().norm(); + return motion > 5.0 * config_.min_motion_th; +} + +} // namespace genz_icp::pipeline diff --git a/cpp/genz_icp/pipeline/GenZICP.hpp b/cpp/genz_icp/pipeline/GenZICP.hpp new file mode 100644 index 0000000..0ac78ac --- /dev/null +++ b/cpp/genz_icp/pipeline/GenZICP.hpp @@ -0,0 +1,97 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include + +#include "genz_icp/core/Deskew.hpp" +#include "genz_icp/core/Threshold.hpp" +#include "genz_icp/core/VoxelHashMap.hpp" +#include "genz_icp/core/Registration.hpp" + +namespace genz_icp::pipeline { + +struct GenZConfig { + // map params + double max_range = 100.0; + double min_range = 0.5; + double map_cleanup_radius = 400.0; + int max_points_per_voxel = 1; + + // voxelize params + double voxel_size = 0.25; + int max_points_per_voxelized_scan = 2000; + int min_points_per_voxelized_scan = 1800; + + // th parms + double min_motion_th = 0.1; + double initial_threshold = 2.0; + double planarity_threshold = 0.1; + + // Motion compensation + bool deskew = false; + + // registration params + int max_num_iterations = 150; + double convergence_criterion = 0.0001; +}; + +class GenZICP { +public: + using Vector3dVector = std::vector; + using Vector3dVectorTuple = std::tuple; + +public: + explicit GenZICP(const GenZConfig &config) + : config_(config), + registration_(config.max_num_iterations, config.convergence_criterion), + local_map_(config.voxel_size, config.max_range, config.map_cleanup_radius, config.planarity_threshold, config.max_points_per_voxel), + adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {} + + GenZICP() : GenZICP(GenZConfig{}) {} + +public: + Vector3dVectorTuple RegisterFrame(const std::vector &frame); + Vector3dVectorTuple RegisterFrame(const std::vector &frame, + const std::vector ×tamps); + Vector3dVectorTuple Voxelize(const std::vector &frame, double voxel_size) const; + double GetAdaptiveThreshold(); + Sophus::SE3d GetPredictionModel() const; + bool HasMoved(); + +public: + // Extra C++ API to facilitate ROS debugging + std::vector LocalMap() const { return local_map_.Pointcloud(); }; + std::vector poses() const { return poses_; }; + +private: + // GenZ-ICP pipeline modules + std::vector poses_; + GenZConfig config_; + Registration registration_; + VoxelHashMap local_map_; + AdaptiveThreshold adaptive_threshold_; +}; + +} // namespace genz_icp::pipeline diff --git a/pictures/GenZ-ICP.gif b/pictures/GenZ-ICP.gif new file mode 100644 index 0000000..738d5b5 Binary files /dev/null and b/pictures/GenZ-ICP.gif differ diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt new file mode 100644 index 0000000..f8763c2 --- /dev/null +++ b/ros/CMakeLists.txt @@ -0,0 +1,101 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +cmake_minimum_required(VERSION 3.16...3.26) +project(genz_icp VERSION 0.0.0 LANGUAGES CXX) + +#add_compile_options(-Werror=unused-variable) + +set(ignore ${CATKIN_INSTALL_INTO_PREFIX_ROOT}) +set(CMAKE_BUILD_TYPE Release) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/genz_icp/) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/genz_icp ${CMAKE_CURRENT_BINARY_DIR}/genz_icp) +else() + cmake_minimum_required(VERSION 3.18) + message(STATUS "Performing out-of-tree build, fetching GenZ-ICP v${CMAKE_PROJECT_VERSION} Release from Github") + include(FetchContent) + FetchContent_Declare( + ext_genz_icp_core PREFIX genz_icp_core + URL https://github.com/cocel-postech/genz-icp/archive/refs/tags/v${CMAKE_PROJECT_VERSION}.tar.gz SOURCE_SUBDIR + cpp/genz_icp) + FetchContent_MakeAvailable(ext_genz_icp_core) +endif() + +if("$ENV{ROS_VERSION}" STREQUAL "1") + message(STATUS "GenZ-ICP ROS 1 wrapper will be compiled") + find_package( + catkin REQUIRED + COMPONENTS geometry_msgs + nav_msgs + sensor_msgs + geometry_msgs + roscpp + rosbag + std_msgs + tf2 + tf2_ros) + catkin_package() + + # ROS 1 node + add_executable(odometry_node ros1/OdometryServer.cpp) + target_compile_features(odometry_node PUBLIC cxx_std_20) + target_include_directories(odometry_node PUBLIC include ${catkin_INCLUDE_DIRS}) + target_link_libraries(odometry_node genz_icp::pipeline ${catkin_LIBRARIES}) + install(TARGETS odometry_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +elseif("$ENV{ROS_VERSION}" STREQUAL "2") + message(STATUS "GenZ-ICP ROS 2 wrapper will be compiled") + + find_package(ament_cmake REQUIRED) + find_package(nav_msgs REQUIRED) + find_package(rcutils REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rclcpp_components REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(tf2_ros REQUIRED) + + # ROS 2 node + add_library(odometry_component SHARED ros2/OdometryServer.cpp) + target_compile_features(odometry_component PUBLIC cxx_std_20) + target_include_directories(odometry_component PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(odometry_component genz_icp::pipeline) + ament_target_dependencies( + odometry_component + rcutils + rclcpp + rclcpp_components + nav_msgs + sensor_msgs + geometry_msgs + tf2_ros) + + rclcpp_components_register_node(odometry_component PLUGIN "genz_icp_ros::OdometryServer" EXECUTABLE odometry_node) + + install(TARGETS odometry_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}/) + + ament_package() + +else() + message(FATAL_ERROR "catkin or colcon not found GenZ-ICP-ROS disabled") +endif() diff --git a/ros/LICENSE b/ros/LICENSE new file mode 100644 index 0000000..52551e9 --- /dev/null +++ b/ros/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/ros/README.md b/ros/README.md new file mode 100644 index 0000000..9efe8bf --- /dev/null +++ b/ros/README.md @@ -0,0 +1,116 @@ +# GenZ-ICP ROS wrappers + +## :gear: How to build & Run + +### ROS1 + +#### How to build + +You should not need any extra dependency, just clone and build: + +```sh +mkdir -p ~/catkin_ws/src +cd ~/catkin_ws/src +git clone https://github.com/cocel-postech/genz-icp.git +cd .. +catkin build genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release +source ~/catkin_ws/devel/setup.bash +``` + +#### How to run + +#### Option 1 + +If you want to use a pre-tuned parameter set, you need to provide the **config file** with the **topic name** as arguments: + +```sh +roslaunch genz_icp odometry.launch topic:= config_file:=.yaml +``` +```sh +rosbag play .bag +``` + +For example, + +1. **Long_Corridor** sequence of SubT-MRS dataset + +```sh +roslaunch genz_icp odometry.launch topic:=/velodyne_points config_file:=long_corridor.yaml +``` +```sh +rosbag play subt_mrs_long_corridor.bag +``` + +The original bagfile for the **Long_Corridor** sequence of SubT-MRS dataset can be downloaded from [here][long_corridor_original_link] + +`subt_mrs_long_corridor.bag` includes only the `/velodyne_points` topic and can be downloaded from [here][long_corridor_link] + +[long_corridor_original_link]: https://superodometry.com/iccv23_challenge_Mul +[long_corridor_link]: https://cocel.synology.me:5001/sharing/JZQalfEqQ + + +2. **Exp07** Long Corridor sequence of HILTI-Oxford dataset + +```sh +roslaunch genz_icp odometry.launch topic:=/hesai/pandar config_file:=exp07.yaml +``` +```sh +rosbag play exp07_long_corridor.bag +``` + +The bagfile for the **Exp07** Long Corridor sequence of HILTI-Oxford dataset can be downloaded from [here][exp07_link] + +[exp07_link]: https://hilti-challenge.com/dataset-2022.html + +3. **Corridor1** and **Corridor2** sequences of Ground-Challenge dataset + +```sh +roslaunch genz_icp odometry.launch topic:=/velodyne_points config_file:=corridor.yaml +``` +```sh +rosbag play corridor1.bag +``` + +The bagfile for the **Corridor1** and **Corridor2** sequences of Ground-Challenge dataset can be downloaded from [here][ground_challenge_link] + +[ground_challenge_link]: https://github.com/sjtuyinjie/Ground-Challenge + +#### Option 2 + +Otherwise, the only required argument to provide is the **topic name**: + +```sh +roslaunch genz_icp odometry.launch topic:= +``` +```sh +rosbag play .bag +``` + +### ROS2 + +#### How to build + +You should not need any extra dependency, just clone and build: + +```sh +mkdir -p ~/colcon_ws/src +cd ~/colcon_ws/src +git clone https://github.com/cocel-postech/genz-icp.git +cd .. +colcon build --packages-select genz_icp --cmake-args -DCMAKE_BUILD_TYPE=Release +source ~/colcon_ws/install/setup.bash +``` + +#### How to run + +The only required argument to provide is the **topic name**: + +```sh +ros2 launch genz_icp odometry.launch.py topic:= +``` + +and then, + +```sh +ros2 bag play .mcap +``` diff --git a/ros/config/corridor.yaml b/ros/config/corridor.yaml new file mode 100644 index 0000000..aaa3bbb --- /dev/null +++ b/ros/config/corridor.yaml @@ -0,0 +1,15 @@ +# Parameter set for Corridor1 and Corridor2 sequences of Ground-Challegne dataset + +# topic: /velodyne_points (set in launch file or command line) +deskew: false +max_range: 100.0 +min_range: 0.5 +voxel_size: 0.2 +max_points_per_voxelized_scan: 1800 +min_points_per_voxelized_scan: 1500 +planarity_threshold: 0.15 +max_points_per_voxel: 1 +initial_threshold: 2.0 +min_motion_th: 0.1 +max_num_iterations: 150 +convergence_criterion: 0.0001 \ No newline at end of file diff --git a/ros/config/exp07.yaml b/ros/config/exp07.yaml new file mode 100644 index 0000000..8ff7367 --- /dev/null +++ b/ros/config/exp07.yaml @@ -0,0 +1,15 @@ +# Parameter set for Exp07 sequence of HILTI-Oxford dataset + +# topic: /hesai/pandar (set in launch file or command line) +deskew: false +max_range: 120.0 +min_range: 0.5 +voxel_size: 0.2 +max_points_per_voxelized_scan: 1800 +min_points_per_voxelized_scan: 1500 +planarity_threshold: 0.17 +max_points_per_voxel: 1 +initial_threshold: 2.0 +min_motion_th: 0.1 +max_num_iterations: 150 +convergence_criterion: 0.0001 \ No newline at end of file diff --git a/ros/config/long_corridor.yaml b/ros/config/long_corridor.yaml new file mode 100644 index 0000000..bdf903f --- /dev/null +++ b/ros/config/long_corridor.yaml @@ -0,0 +1,16 @@ +# Parameter set for Long_Corridor sequence of SubT-MRS dataset + +# topic: /velodyne_points (set in launch file or command line) +deskew: false +max_range: 100.0 +min_range: 0.5 +voxel_size: 0.25 +map_cleanup_radius: 300 +max_points_per_voxelized_scan: 1800 +min_points_per_voxelized_scan: 1500 +planarity_threshold: 0.17 +max_points_per_voxel: 1 +initial_threshold: 2.0 +min_motion_th: 0.1 +max_num_iterations: 200 +convergence_criterion: 0.0001 \ No newline at end of file diff --git a/ros/launch/odometry.launch b/ros/launch/odometry.launch new file mode 100644 index 0000000..48e41fa --- /dev/null +++ b/ros/launch/odometry.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/launch/odometry.launch.py b/ros/launch/odometry.launch.py new file mode 100644 index 0000000..cdb5956 --- /dev/null +++ b/ros/launch/odometry.launch.py @@ -0,0 +1,105 @@ +# MIT License +# +# Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + current_pkg = FindPackageShare("genz_icp") + return LaunchDescription( + [ + # ROS 2 parameters + DeclareLaunchArgument("topic", description="sensor_msg/PointCloud2 topic to process"), + DeclareLaunchArgument("bagfile", default_value=""), + DeclareLaunchArgument("visualize", default_value="true"), + DeclareLaunchArgument("odom_frame", default_value="odom"), + DeclareLaunchArgument("base_frame", default_value=""), + DeclareLaunchArgument("publish_odom_tf", default_value="true"), + # GenZ-ICP parameters + DeclareLaunchArgument("deskew", default_value="false"), + DeclareLaunchArgument("max_range", default_value="100.0"), + DeclareLaunchArgument("min_range", default_value="0.5"), + # This thing is still not suported: https://github.com/ros2/launch/issues/290#issuecomment-1438476902 + # DeclareLaunchArgument("voxel_size", default_value=None), + DeclareLaunchArgument("voxel_size", default_value="0.25"), + DeclareLaunchArgument("map_cleanup_radius", default_value="300.0"), + DeclareLaunchArgument("max_points_per_voxelized_scan", default_value="1800"), + DeclareLaunchArgument("min_points_per_voxelized_scan", default_value="1500"), + DeclareLaunchArgument("planarity_threshold", default_value="0.17"), + DeclareLaunchArgument("max_points_per_voxel", default_value="1"), + DeclareLaunchArgument("max_num_iterations", default_value="200"), + DeclareLaunchArgument("convergence_criterion", default_value="0.0001"), + DeclareLaunchArgument("initial_threshold", default_value="2.0"), + DeclareLaunchArgument("min_motion_th", default_value="0.1"), + Node( + package="genz_icp", + executable="odometry_node", + name="odometry_node", + output="screen", + remappings=[("pointcloud_topic", LaunchConfiguration("topic"))], + parameters=[ + { + "odom_frame": LaunchConfiguration("odom_frame"), + "base_frame": LaunchConfiguration("base_frame"), + "deskew": LaunchConfiguration("deskew"), + "max_range": LaunchConfiguration("max_range"), + "min_range": LaunchConfiguration("min_range"), + "voxel_size": LaunchConfiguration("voxel_size"), + "map_cleanup_radius": LaunchConfiguration("map_cleanup_radius"), + "max_points_per_voxelized_scan": LaunchConfiguration("max_points_per_voxelized_scan"), + "min_points_per_voxelized_scan": LaunchConfiguration("min_points_per_voxelized_scan"), + "planarity_threshold": LaunchConfiguration("planarity_threshold"), + "max_points_per_voxel": LaunchConfiguration("max_points_per_voxel"), + "max_num_iterations": LaunchConfiguration("max_num_iterations"), + "convergence_criterion": LaunchConfiguration("convergence_criterion"), + "initial_threshold": 2.0, + "min_motion_th": 0.1, + "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), + "visualize": LaunchConfiguration("visualize"), + } + ], + ), + Node( + package="rviz2", + executable="rviz2", + output={"both": "log"}, + arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "genz_icp_ros2.rviz"])], + condition=IfCondition(LaunchConfiguration("visualize")), + ), + ExecuteProcess( + cmd=["ros2", "bag", "play", LaunchConfiguration("bagfile")], + output="screen", + condition=IfCondition( + PythonExpression(["'", LaunchConfiguration("bagfile"), "' != ''"]) + ), + ), + ] + ) diff --git a/ros/package.xml b/ros/package.xml new file mode 100644 index 0000000..23ac1ed --- /dev/null +++ b/ros/package.xml @@ -0,0 +1,58 @@ + + + + genz_icp + 0.0.0 + GenZ-ICP ROS Wrappers + Daehan Lee + MIT + + + ros_environment + + + catkin + roscpp + + + ament_cmake + rcutils + rclcpp + rclcpp_components + ros2launch + + + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + tf2 + tf2_ros + + + catkin + ament_cmake + + diff --git a/ros/ros1/OdometryServer.cpp b/ros/ros1/OdometryServer.cpp new file mode 100644 index 0000000..c873eb2 --- /dev/null +++ b/ros/ros1/OdometryServer.cpp @@ -0,0 +1,228 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include +#include +#include +#include + +// GenZ-ICP-ROS +#include "OdometryServer.hpp" +#include "Utils.hpp" + +// GenZ-ICP +#include "genz_icp/pipeline/GenZICP.hpp" + +// ROS 1 headers +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace genz_icp_ros { + +using utils::EigenToPointCloud2; +using utils::GetTimestamps; +using utils::PointCloud2ToEigen; + +OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh) + : nh_(nh), pnh_(pnh), tf2_listener_(tf2_ros::TransformListener(tf2_buffer_)) { + pnh_.param("base_frame", base_frame_, base_frame_); + pnh_.param("odom_frame", odom_frame_, odom_frame_); + pnh_.param("publish_odom_tf", publish_odom_tf_, false); + pnh_.param("visualize", publish_debug_clouds_, publish_debug_clouds_); + pnh_.param("max_range", config_.max_range, config_.max_range); + pnh_.param("min_range", config_.min_range, config_.min_range); + pnh_.param("deskew", config_.deskew, config_.deskew); + pnh_.param("voxel_size", config_.voxel_size, config_.max_range / 100.0); + pnh_.param("map_cleanup_radius", config_.map_cleanup_radius, config_.max_range); + pnh_.param("planarity_threshold", config_.planarity_threshold, config_.planarity_threshold); + pnh_.param("max_points_per_voxel", config_.max_points_per_voxel, config_.max_points_per_voxel); + pnh_.param("max_points_per_voxelized_scan", config_.max_points_per_voxelized_scan, config_.max_points_per_voxelized_scan); + pnh_.param("min_points_per_voxelized_scan", config_.min_points_per_voxelized_scan, config_.min_points_per_voxelized_scan); + pnh_.param("initial_threshold", config_.initial_threshold, config_.initial_threshold); + pnh_.param("min_motion_th", config_.min_motion_th, config_.min_motion_th); + pnh_.param("max_num_iterations", config_.max_num_iterations, config_.max_num_iterations); + pnh_.param("convergence_criterion", config_.convergence_criterion, config_.convergence_criterion); + if (config_.max_range < config_.min_range) { + ROS_WARN("[WARNING] max_range is smaller than min_range, setting min_range to 0.0"); + config_.min_range = 0.0; + } + + // Construct the main GenZ-ICP odometry node + odometry_ = genz_icp::pipeline::GenZICP(config_); + + // Initialize subscribers + pointcloud_sub_ = nh_.subscribe("pointcloud_topic", queue_size_, + &OdometryServer::RegisterFrame, this); + + // Initialize publishers + odom_publisher_ = pnh_.advertise("/genz/odometry", queue_size_); + traj_publisher_ = pnh_.advertise("/genz/trajectory", queue_size_); + if (publish_debug_clouds_) { + map_publisher_ = pnh_.advertise("/genz/local_map", queue_size_); + planar_points_publisher_ = pnh_.advertise("/genz/planar_points", queue_size_); + non_planar_points_publisher_ = pnh_.advertise("/genz/non_planar_points", queue_size_); + } + // Initialize the transform buffer + tf2_buffer_.setUsingDedicatedThread(true); + path_msg_.header.frame_id = odom_frame_; + + // publish odometry msg + ROS_INFO("GenZ-ICP ROS 1 Odometry Node Initialized"); +} + +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_._frameExists(source_frame) && // + tf2_buffer_._frameExists(target_frame) && // + tf2_buffer_.canTransform(target_frame, source_frame, ros::Time(0), &err_msg)) { + try { + auto tf = tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0)); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + } + } + ROS_WARN("Failed to find tf between %s and %s. Reason=%s", target_frame.c_str(), + source_frame.c_str(), err_msg.c_str()); + return {}; +} + +void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; + const auto points = PointCloud2ToEigen(msg); + const auto timestamps = [&]() -> std::vector { + if (!config_.deskew) return {}; + return GetTimestamps(msg); + }(); + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); + + // Register frame, main entry point to GenZ-ICP pipeline + const auto &[planar_points, non_planar_points] = odometry_.RegisterFrame(points, timestamps); + + // Compute the pose using GenZ, ego-centric to the LiDAR + const Sophus::SE3d genz_pose = odometry_.poses().back(); + + // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame + const auto pose = [&]() -> Sophus::SE3d { + if (egocentric_estimation) return genz_pose; + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * genz_pose * cloud2base.inverse(); + }(); + + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(msg->header.stamp, cloud_frame_id, planar_points, non_planar_points); + } +} + +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id) { + // Header for point clouds and stuff seen from desired odom_frame + + // Broadcast the tf + if (publish_odom_tf_) { + geometry_msgs::TransformStamped transform_msg; + transform_msg.header.stamp = stamp; + transform_msg.header.frame_id = odom_frame_; + transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.transform = tf2::sophusToTransform(pose); + tf_broadcaster_.sendTransform(transform_msg); + } + + // publish trajectory msg + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = stamp; + pose_msg.header.frame_id = odom_frame_; + pose_msg.pose = tf2::sophusToPose(pose); + path_msg_.poses.push_back(pose_msg); + traj_publisher_.publish(path_msg_); + + // publish odometry msg + nav_msgs::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); + odom_publisher_.publish(odom_msg); +} + +void OdometryServer::PublishClouds(const ros::Time &stamp, + const std::string &cloud_frame_id, + const std::vector &planar_points, + const std::vector &non_planar_points) { + std_msgs::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto genz_map = odometry_.LocalMap(); + + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; + + map_publisher_.publish(*EigenToPointCloud2(genz_map, odom_header)); + planar_points_publisher_.publish(*EigenToPointCloud2(planar_points, cloud_header)); + non_planar_points_publisher_.publish(*EigenToPointCloud2(non_planar_points, cloud_header)); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + planar_points_publisher_.publish(*EigenToPointCloud2(planar_points, odom_header)); + non_planar_points_publisher_.publish(*EigenToPointCloud2(non_planar_points, odom_header)); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_.publish(*EigenToPointCloud2(genz_map, cloud2base, odom_header)); + } else { + map_publisher_.publish(*EigenToPointCloud2(genz_map, odom_header)); + } +} + +} // namespace genz_icp_ros + +int main(int argc, char **argv) { + ros::init(argc, argv, "genz_icp"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + + genz_icp_ros::OdometryServer node(nh, nh_private); + + ros::spin(); + + return 0; +} diff --git a/ros/ros1/OdometryServer.hpp b/ros/ros1/OdometryServer.hpp new file mode 100644 index 0000000..b06d32a --- /dev/null +++ b/ros/ros1/OdometryServer.hpp @@ -0,0 +1,95 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +// GenZ-ICP +#include "genz_icp/pipeline/GenZICP.hpp" + +// ROS +#include +#include +#include +#include +#include +#include + +#include + +namespace genz_icp_ros { + +class OdometryServer { +public: + /// OdometryServer constructor + OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle &pnh); + +private: + /// Register new frame + void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg); + + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const ros::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const ros::Time &stamp, + const std::string &cloud_frame_id, + const std::vector &planar_points, + const std::vector &non_planar_points); + + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + + /// Ros node stuff + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + int queue_size_{1}; + + /// Tools for broadcasting TFs. + tf2_ros::TransformBroadcaster tf_broadcaster_; + tf2_ros::Buffer tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + bool publish_odom_tf_; + bool publish_debug_clouds_; + + /// Data subscribers. + ros::Subscriber pointcloud_sub_; + + /// Data publishers. + ros::Publisher odom_publisher_; + ros::Publisher map_publisher_; + ros::Publisher traj_publisher_; + ros::Publisher planar_points_publisher_; + ros::Publisher non_planar_points_publisher_; + nav_msgs::Path path_msg_; + + /// GenZ-ICP + genz_icp::pipeline::GenZICP odometry_; + genz_icp::pipeline::GenZConfig config_; + + /// Global/map coordinate frame. + std::string odom_frame_{"odom"}; + std::string base_frame_{}; +}; + +} // namespace genz_icp_ros diff --git a/ros/ros1/Utils.hpp b/ros/ros1/Utils.hpp new file mode 100644 index 0000000..2db4125 --- /dev/null +++ b/ros/ros1/Utils.hpp @@ -0,0 +1,235 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 1 headers +#include +#include +#include +#include +#include + +namespace tf2 { + +inline geometry_msgs::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::Transform t; + t.translation.x = T.translation().x(); + t.translation.y = T.translation().y(); + t.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.rotation.w = q.w(); + + return t; +} + +inline geometry_msgs::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::Pose t; + t.position.x = T.translation().x(); + t.position.y = T.translation().y(); + t.position.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.orientation.x = q.x(); + t.orientation.y = q.y(); + t.orientation.z = q.z(); + t.orientation.w = q.w(); + + return t; +} + +inline Sophus::SE3d transformToSophus(const geometry_msgs::TransformStamped &transform) { + const auto &t = transform.transform; + return Sophus::SE3d( + Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), + Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); +} +} // namespace tf2 + +namespace genz_icp_ros::utils { +using PointCloud2 = sensor_msgs::PointCloud2; +using PointField = sensor_msgs::PointField; +using Header = std_msgs::Header; + +inline std::string FixFrameId(const std::string &frame_id) { + return std::regex_replace(frame_id, std::regex("^/"), ""); +} + +inline auto GetTimestampField(const PointCloud2::ConstPtr msg) { + PointField timestamp_field; + for (const auto &field : msg->fields) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { + timestamp_field = field; + } + } + if (!timestamp_field.count) { + throw std::runtime_error("Field 't', 'timestamp', or 'time' does not exist"); + } + return timestamp_field; +} + +// Normalize timestamps from 0.0 to 1.0 +inline auto NormalizeTimestamps(const std::vector ×tamps) { + const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const double min_timestamp = *min_it; + const double max_timestamp = *max_it; + + std::vector timestamps_normalized(timestamps.size()); + std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), + [&](const auto ×tamp) { + return (timestamp - min_timestamp) / (max_timestamp - min_timestamp); + }); + return timestamps_normalized; +} + +inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstPtr msg, const PointField &field) { + auto extract_timestamps = + [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { + const size_t n_points = msg->height * msg->width; + std::vector timestamps; + timestamps.reserve(n_points); + for (size_t i = 0; i < n_points; ++i, ++it) { + timestamps.emplace_back(static_cast(*it)); + } + return NormalizeTimestamps(timestamps); + }; + + // Get timestamp field that must be one of the following : {t, timestamp, time} + auto timestamp_field = GetTimestampField(msg); + + // According to the type of the timestamp == type, return a PointCloud2ConstIterator + using sensor_msgs::PointCloud2ConstIterator; + if (timestamp_field.datatype == PointField::UINT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT64) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } + + // timestamp type not supported, please open an issue :) + throw std::runtime_error("timestamp field type not supported"); +} + +inline std::unique_ptr CreatePointCloud2Msg(const size_t n_points, + const Header &header, + bool timestamp = false) { + auto cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*cloud_msg); + cloud_msg->header = header; + cloud_msg->header.frame_id = FixFrameId(cloud_msg->header.frame_id); + cloud_msg->fields.clear(); + int offset = 0; + offset = addPointField(*cloud_msg, "x", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "y", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "z", 1, PointField::FLOAT32, offset); + offset += sizeOfPointField(PointField::FLOAT32); + if (timestamp) { + // assuming timestamp on a velodyne fashion for now (between 0.0 and 1.0) + offset = addPointField(*cloud_msg, "time", 1, PointField::FLOAT64, offset); + offset += sizeOfPointField(PointField::FLOAT64); + } + + // Resize the point cloud accordingly + cloud_msg->point_step = offset; + cloud_msg->row_step = cloud_msg->width * cloud_msg->point_step; + cloud_msg->data.resize(cloud_msg->height * cloud_msg->row_step); + modifier.resize(n_points); + return cloud_msg; +} + +inline void FillPointCloud2XYZ(const std::vector &points, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); + sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); + sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); + for (size_t i = 0; i < points.size(); i++, ++msg_x, ++msg_y, ++msg_z) { + const Eigen::Vector3d &point = points[i]; + *msg_x = point.x(); + *msg_y = point.y(); + *msg_z = point.z(); + } +} + +inline void FillPointCloud2Timestamp(const std::vector ×tamps, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_t(msg, "time"); + for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i]; +} + +inline std::vector GetTimestamps(const PointCloud2::ConstPtr msg) { + auto timestamp_field = GetTimestampField(msg); + + // Extract timestamps from cloud_msg + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field); + + return timestamps; +} + +inline std::vector PointCloud2ToEigen(const PointCloud2::ConstPtr msg) { + std::vector points; + points.reserve(msg->height * msg->width); + sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z"); + for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) { + points.emplace_back(*msg_x, *msg_y, *msg_z); + } + return points; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header); + FillPointCloud2XYZ(points, *msg); + return msg; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector points_t; + points_t.resize(points.size()); + std::transform(points.cbegin(), points.cend(), points_t.begin(), + [&](const auto &point) { return T * point; }); + return EigenToPointCloud2(points_t, header); +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const std::vector ×tamps, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header, true); + FillPointCloud2XYZ(points, *msg); + FillPointCloud2Timestamp(timestamps, *msg); + return msg; +} +} // namespace genz_icp_ros::utils diff --git a/ros/ros2/OdometryServer.cpp b/ros/ros2/OdometryServer.cpp new file mode 100644 index 0000000..1d76a46 --- /dev/null +++ b/ros/ros2/OdometryServer.cpp @@ -0,0 +1,223 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#include +#include +#include +#include +#include + +// GenZ-ICP-ROS +#include "OdometryServer.hpp" +#include "Utils.hpp" + +// GenZ-ICP +#include "genz_icp/pipeline/GenZICP.hpp" + +// ROS 2 headers +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace genz_icp_ros { + +using utils::EigenToPointCloud2; +using utils::GetTimestamps; +using utils::PointCloud2ToEigen; + +OdometryServer::OdometryServer(const rclcpp::NodeOptions &options) + : rclcpp::Node("odometry_node", options) { + // clang-format off + base_frame_ = declare_parameter("base_frame", base_frame_); + odom_frame_ = declare_parameter("odom_frame", odom_frame_); + publish_odom_tf_ = declare_parameter("publish_odom_tf", publish_odom_tf_); + publish_debug_clouds_ = declare_parameter("visualize", publish_debug_clouds_); + config_.max_range = declare_parameter("max_range", config_.max_range); + config_.min_range = declare_parameter("min_range", config_.min_range); + config_.deskew = declare_parameter("deskew", config_.deskew); + config_.voxel_size = declare_parameter("voxel_size", config_.max_range); + config_.map_cleanup_radius = declare_parameter("map_cleanup_radius", config_.map_cleanup_radius); + config_.planarity_threshold = declare_parameter("planarity_threshold", config_.planarity_threshold); + config_.max_points_per_voxel = declare_parameter("max_points_per_voxel", config_.max_points_per_voxel); + config_.max_points_per_voxelized_scan = declare_parameter("max_points_per_voxelized_scan", config_.max_points_per_voxelized_scan); + config_.min_points_per_voxelized_scan = declare_parameter("min_points_per_voxelized_scan", config_.min_points_per_voxelized_scan); + config_.max_num_iterations = declare_parameter("max_num_iterations", config_.max_num_iterations); + config_.convergence_criterion = declare_parameter("convergence_criterion", config_.convergence_criterion); + config_.initial_threshold = declare_parameter("initial_threshold", config_.initial_threshold); + config_.min_motion_th = declare_parameter("min_motion_th", config_.min_motion_th); + if (config_.max_range < config_.min_range) { + RCLCPP_WARN(get_logger(), "[WARNING] max_range is smaller than min_range, settng min_range to 0.0"); + config_.min_range = 0.0; + } + // clang-format on + + // Construct the main GenZ-ICP odometry node + odometry_ = genz_icp::pipeline::GenZICP(config_); + + // Initialize subscribers + pointcloud_sub_ = create_subscription( + "pointcloud_topic", rclcpp::SensorDataQoS(), + std::bind(&OdometryServer::RegisterFrame, this, std::placeholders::_1)); + + // Initialize publishers + rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); + odom_publisher_ = create_publisher("/genz/odometry", qos); + traj_publisher_ = create_publisher("/genz/trajectory", qos); + path_msg_.header.frame_id = odom_frame_; + if (publish_debug_clouds_) { + map_publisher_ = create_publisher("/genz/local_map", qos); + planar_points_publisher_ = create_publisher("/genz/planar_points", qos); + non_planar_points_publisher_ = create_publisher("/genz/non_planar_points", qos); + } + + // Initialize the transform broadcaster + tf_broadcaster_ = std::make_unique(*this); + tf2_buffer_ = std::make_unique(this->get_clock()); + tf2_buffer_->setUsingDedicatedThread(true); + tf2_listener_ = std::make_unique(*tf2_buffer_); + + RCLCPP_INFO(this->get_logger(), "GenZ-ICP ROS 2 odometry node initialized"); +} + +Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame, + const std::string &source_frame) const { + std::string err_msg; + if (tf2_buffer_->_frameExists(source_frame) && // + tf2_buffer_->_frameExists(target_frame) && // + tf2_buffer_->canTransform(target_frame, source_frame, tf2::TimePointZero, &err_msg)) { + try { + auto tf = tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + return tf2::transformToSophus(tf); + } catch (tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + } + } + RCLCPP_WARN(this->get_logger(), "Failed to find tf. Reason=%s", err_msg.c_str()); + return {}; +} + +void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { + const auto cloud_frame_id = msg->header.frame_id; + const auto points = PointCloud2ToEigen(msg); + const auto timestamps = [&]() -> std::vector { + if (!config_.deskew) return {}; + return GetTimestamps(msg); + }(); + const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id); + + // Register frame, main entry point to GenZ-ICP pipeline + const auto &[planar_points, non_planar_points] = odometry_.RegisterFrame(points, timestamps); + + // Compute the pose using GenZ, ego-centric to the LiDAR + const Sophus::SE3d genz_pose = odometry_.poses().back(); + + // If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame + const auto pose = [&]() -> Sophus::SE3d { + if (egocentric_estimation) return genz_pose; + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + return cloud2base * genz_pose * cloud2base.inverse(); + }(); + + // Spit the current estimated pose to ROS msgs + PublishOdometry(pose, msg->header.stamp, cloud_frame_id); + // Publishing this clouds is a bit costly, so do it only if we are debugging + if (publish_debug_clouds_) { + PublishClouds(msg->header.stamp, cloud_frame_id, planar_points, non_planar_points); + } +} + +void OdometryServer::PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id) { + // Broadcast the tf --- + if (publish_odom_tf_) { + geometry_msgs::msg::TransformStamped transform_msg; + transform_msg.header.stamp = stamp; + transform_msg.header.frame_id = odom_frame_; + transform_msg.child_frame_id = base_frame_.empty() ? cloud_frame_id : base_frame_; + transform_msg.transform = tf2::sophusToTransform(pose); + tf_broadcaster_->sendTransform(transform_msg); + } + + // publish trajectory msg + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = stamp; + pose_msg.header.frame_id = odom_frame_; + pose_msg.pose = tf2::sophusToPose(pose); + path_msg_.poses.push_back(pose_msg); + traj_publisher_->publish(path_msg_); + + // publish odometry msg + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = stamp; + odom_msg.header.frame_id = odom_frame_; + odom_msg.pose.pose = tf2::sophusToPose(pose); + odom_publisher_->publish(std::move(odom_msg)); +} + +void OdometryServer::PublishClouds(const rclcpp::Time &stamp, + const std::string &cloud_frame_id, + const std::vector &planar_points, + const std::vector &non_planar_points) { + std_msgs::msg::Header odom_header; + odom_header.stamp = stamp; + odom_header.frame_id = odom_frame_; + + // Publish map + const auto genz_map = odometry_.LocalMap(); + + if (!publish_odom_tf_) { + // debugging happens in an egocentric world + std_msgs::msg::Header cloud_header; + cloud_header.stamp = stamp; + cloud_header.frame_id = cloud_frame_id; + + map_publisher_->publish(std::move(EigenToPointCloud2(genz_map, odom_header))); + planar_points_publisher_->publish(std::move(EigenToPointCloud2(planar_points, cloud_header))); + non_planar_points_publisher_->publish(std::move(EigenToPointCloud2(non_planar_points, cloud_header))); + + return; + } + + // If transmitting to tf tree we know where the clouds are exactly + const auto cloud2odom = LookupTransform(odom_frame_, cloud_frame_id); + planar_points_publisher_->publish(std::move(EigenToPointCloud2(planar_points, odom_header))); + non_planar_points_publisher_->publish(std::move(EigenToPointCloud2(non_planar_points, odom_header))); + + if (!base_frame_.empty()) { + const Sophus::SE3d cloud2base = LookupTransform(base_frame_, cloud_frame_id); + map_publisher_->publish(std::move(EigenToPointCloud2(genz_map, cloud2base, odom_header))); + } else { + map_publisher_->publish(std::move(EigenToPointCloud2(genz_map, odom_header))); + } +} +} // namespace genz_icp_ros + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(genz_icp_ros::OdometryServer) diff --git a/ros/ros2/OdometryServer.hpp b/ros/ros2/OdometryServer.hpp new file mode 100644 index 0000000..b162db9 --- /dev/null +++ b/ros/ros2/OdometryServer.hpp @@ -0,0 +1,95 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +// GenZ-ICP +#include "genz_icp/pipeline/GenZICP.hpp" + +// ROS 2 +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace genz_icp_ros { + +class OdometryServer : public rclcpp::Node { +public: + /// OdometryServer constructor + OdometryServer() = delete; + explicit OdometryServer(const rclcpp::NodeOptions &options); + +private: + /// Register new frame + void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); + + /// Stream the estimated pose to ROS + void PublishOdometry(const Sophus::SE3d &pose, + const rclcpp::Time &stamp, + const std::string &cloud_frame_id); + + /// Stream the debugging point clouds for visualization (if required) + void PublishClouds(const rclcpp::Time &stamp, + const std::string &cloud_frame_id, + const std::vector &planar_points, + const std::vector &non_planar_points); + + /// Utility function to compute transformation using tf tree + Sophus::SE3d LookupTransform(const std::string &target_frame, + const std::string &source_frame) const; + +private: + /// Tools for broadcasting TFs. + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf2_buffer_; + std::unique_ptr tf2_listener_; + bool publish_odom_tf_; + bool publish_debug_clouds_; + + /// Data subscribers. + rclcpp::Subscription::SharedPtr pointcloud_sub_; + + /// Data publishers. + rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Publisher::SharedPtr map_publisher_; + rclcpp::Publisher::SharedPtr planar_points_publisher_; + rclcpp::Publisher::SharedPtr non_planar_points_publisher_; + + /// Path publisher + nav_msgs::msg::Path path_msg_; + rclcpp::Publisher::SharedPtr traj_publisher_; + + /// GenZ-ICP + genz_icp::pipeline::GenZICP odometry_; + genz_icp::pipeline::GenZConfig config_; + + /// Global/map coordinate frame. + std::string odom_frame_{"odom"}; + std::string base_frame_{}; +}; + +} // namespace genz_icp_ros diff --git a/ros/ros2/Utils.hpp b/ros/ros2/Utils.hpp new file mode 100644 index 0000000..8e008c3 --- /dev/null +++ b/ros/ros2/Utils.hpp @@ -0,0 +1,237 @@ +// MIT License +// +// Copyright (c) 2024 Daehan Lee, Hyungtae Lim, Soohee Han. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 +#include +#include +#include +#include +#include + +namespace tf2 { + +inline geometry_msgs::msg::Transform sophusToTransform(const Sophus::SE3d &T) { + geometry_msgs::msg::Transform t; + t.translation.x = T.translation().x(); + t.translation.y = T.translation().y(); + t.translation.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.rotation.x = q.x(); + t.rotation.y = q.y(); + t.rotation.z = q.z(); + t.rotation.w = q.w(); + + return t; +} + +inline geometry_msgs::msg::Pose sophusToPose(const Sophus::SE3d &T) { + geometry_msgs::msg::Pose t; + t.position.x = T.translation().x(); + t.position.y = T.translation().y(); + t.position.z = T.translation().z(); + + Eigen::Quaterniond q(T.so3().unit_quaternion()); + t.orientation.x = q.x(); + t.orientation.y = q.y(); + t.orientation.z = q.z(); + t.orientation.w = q.w(); + + return t; +} + +inline Sophus::SE3d transformToSophus(const geometry_msgs::msg::TransformStamped &transform) { + const auto &t = transform.transform; + return Sophus::SE3d( + Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), + Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); +} +} // namespace tf2 + +namespace genz_icp_ros::utils { + +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using Header = std_msgs::msg::Header; + +inline std::string FixFrameId(const std::string &frame_id) { + return std::regex_replace(frame_id, std::regex("^/"), ""); +} + +inline auto GetTimestampField(const PointCloud2::ConstSharedPtr msg) { + PointField timestamp_field; + for (const auto &field : msg->fields) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { + timestamp_field = field; + } + } + if (!timestamp_field.count) { + throw std::runtime_error("Field 't', 'timestamp', or 'time' does not exist"); + } + return timestamp_field; +} + +// Normalize timestamps from 0.0 to 1.0 +inline auto NormalizeTimestamps(const std::vector ×tamps) { + const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const double min_timestamp = *min_it; + const double max_timestamp = *max_it; + + std::vector timestamps_normalized(timestamps.size()); + std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), + [&](const auto ×tamp) { + return (timestamp - min_timestamp) / (max_timestamp - min_timestamp); + }); + return timestamps_normalized; +} + +inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField &field) { + auto extract_timestamps = + [&msg](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { + const size_t n_points = msg->height * msg->width; + std::vector timestamps; + timestamps.reserve(n_points); + for (size_t i = 0; i < n_points; ++i, ++it) { + timestamps.emplace_back(static_cast(*it)); + } + return NormalizeTimestamps(timestamps); + }; + + // Get timestamp field that must be one of the following : {t, timestamp, time} + auto timestamp_field = GetTimestampField(msg); + + // According to the type of the timestamp == type, return a PointCloud2ConstIterator + using sensor_msgs::PointCloud2ConstIterator; + if (timestamp_field.datatype == PointField::UINT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT64) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } + + // timestamp type not supported, please open an issue :) + throw std::runtime_error("timestamp field type not supported"); +} + +inline std::unique_ptr CreatePointCloud2Msg(const size_t n_points, + const Header &header, + bool timestamp = false) { + auto cloud_msg = std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*cloud_msg); + cloud_msg->header = header; + cloud_msg->header.frame_id = FixFrameId(cloud_msg->header.frame_id); + cloud_msg->fields.clear(); + int offset = 0; + offset = addPointField(*cloud_msg, "x", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "y", 1, PointField::FLOAT32, offset); + offset = addPointField(*cloud_msg, "z", 1, PointField::FLOAT32, offset); + offset += sizeOfPointField(PointField::FLOAT32); + if (timestamp) { + // assuming timestamp on a velodyne fashion for now (between 0.0 and 1.0) + offset = addPointField(*cloud_msg, "time", 1, PointField::FLOAT64, offset); + offset += sizeOfPointField(PointField::FLOAT64); + } + + // Resize the point cloud accordingly + cloud_msg->point_step = offset; + cloud_msg->row_step = cloud_msg->width * cloud_msg->point_step; + cloud_msg->data.resize(cloud_msg->height * cloud_msg->row_step); + modifier.resize(n_points); + return cloud_msg; +} + +inline void FillPointCloud2XYZ(const std::vector &points, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_x(msg, "x"); + sensor_msgs::PointCloud2Iterator msg_y(msg, "y"); + sensor_msgs::PointCloud2Iterator msg_z(msg, "z"); + for (size_t i = 0; i < points.size(); i++, ++msg_x, ++msg_y, ++msg_z) { + const Eigen::Vector3d &point = points[i]; + *msg_x = point.x(); + *msg_y = point.y(); + *msg_z = point.z(); + } +} + +inline void FillPointCloud2Timestamp(const std::vector ×tamps, PointCloud2 &msg) { + sensor_msgs::PointCloud2Iterator msg_t(msg, "time"); + for (size_t i = 0; i < timestamps.size(); i++, ++msg_t) *msg_t = timestamps[i]; +} + +inline std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { + auto timestamp_field = GetTimestampField(msg); + + // Extract timestamps from cloud_msg + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field); + + return timestamps; +} + +inline std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg) { + std::vector points; + points.reserve(msg->height * msg->width); + sensor_msgs::PointCloud2ConstIterator msg_x(*msg, "x"); + sensor_msgs::PointCloud2ConstIterator msg_y(*msg, "y"); + sensor_msgs::PointCloud2ConstIterator msg_z(*msg, "z"); + for (size_t i = 0; i < msg->height * msg->width; ++i, ++msg_x, ++msg_y, ++msg_z) { + points.emplace_back(*msg_x, *msg_y, *msg_z); + } + return points; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header); + FillPointCloud2XYZ(points, *msg); + return msg; +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const Sophus::SE3d &T, + const Header &header) { + std::vector points_t; + points_t.resize(points.size()); + std::transform(points.cbegin(), points.cend(), points_t.begin(), + [&](const auto &point) { return T * point; }); + return EigenToPointCloud2(points_t, header); +} + +inline std::unique_ptr EigenToPointCloud2(const std::vector &points, + const std::vector ×tamps, + const Header &header) { + auto msg = CreatePointCloud2Msg(points.size(), header, true); + FillPointCloud2XYZ(points, *msg); + FillPointCloud2Timestamp(timestamps, *msg); + return msg; +} +} // namespace genz_icp_ros::utils diff --git a/ros/rviz/genz_icp_ros1.rviz b/ros/rviz/genz_icp_ros1.rviz new file mode 100644 index 0000000..70efe4b --- /dev/null +++ b/ros/rviz/genz_icp_ros1.rviz @@ -0,0 +1,250 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /PointCloud22 + - /local_map1/Autocompute Value Bounds1 + - /Odometry1/Shape1 + Splitter Ratio: 0.5 + Tree Height: 824 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 119; 187 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /genz/planar_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 238; 51; 119 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /genz/non_planar_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.30000001192092896 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 30 + Min Value: -3 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 136; 138; 133 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: local_map + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 1 + Size (m): 0.10000000149011612 + Style: Points + Topic: /genz/local_map + Unreliable: true + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 3 + Name: odometry_frame + Radius: 0.5 + Reference Frame: odom + Show Trail: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 1 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 134; 44; 134 + Pose Style: None + Queue Size: 10 + Radius: 0.20000000298023224 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /genz/trajectory + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 2 + Shape: + Alpha: 1 + Axes Length: 3 + Axes Radius: 0.6000000238418579 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /genz/odometry + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -0.10000009089708328 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 7.632486343383789 + Target Frame: odom + X: 48.279117584228516 + Y: -10.058450698852539 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016800000375fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001e3000001b40000000000000000fb0000000a0049006d0061006700650000000295000001020000000000000000fb0000000a0049006d0061006700650100000281000001160000000000000000000000010000010000000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000007440000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2488 + X: 1512 + Y: 759 diff --git a/ros/rviz/genz_icp_ros2.rviz b/ros/rviz/genz_icp_ros2.rviz new file mode 100644 index 0000000..8ac650a --- /dev/null +++ b/ros/rviz/genz_icp_ros2.rviz @@ -0,0 +1,297 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /point_clouds1 + - /point_clouds1/planar_points1 + - /point_clouds1/non_planar_points1 + - /odometry1/odometry1/Shape1 + Splitter Ratio: 0.5 + Tree Height: 719 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: local_map +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 4.475373268127441 + Min Value: -4.334714412689209 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 136; 138; 133 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: local_map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.07000000029802322 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /genz/local_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 119; 187 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: planar_points + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /genz/planar_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 238; 51; 119 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: non_planar_points + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /genz/non_planar_points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: point_clouds + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 3 + Name: odom_frame + Radius: 0.5 + Reference Frame: odom + Value: true + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 3 + Axes Radius: 0.6000000238418579 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /genz/odometry + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 245 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /genz/trajectory + Value: true + Enabled: true + Name: odometry + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0.06999997049570084 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 6.211388111114502 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 59.806007385253906 + Y: -4.69886589050293 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000007490000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 1510 + Y: 792