├── .clang-format ├── .cmake-format.yaml ├── .github ├── CODEOWNERS └── workflows │ ├── cpp.yml │ ├── pre-commit.yml │ └── ros.yml ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── Makefile ├── README.md ├── cpp ├── COLCON_IGNORE └── kinematic_icp │ ├── CMakeLists.txt │ ├── LICENSE │ ├── cmake │ └── CompilerOptions.cmake │ ├── correspondence_threshold │ ├── CMakeLists.txt │ ├── CorrespondenceThreshold.cpp │ └── CorrespondenceThreshold.hpp │ ├── kiss_icp │ ├── LICENSE │ └── kiss-icp.cmake │ ├── pipeline │ ├── CMakeLists.txt │ ├── KinematicICP.cpp │ └── KinematicICP.hpp │ └── registration │ ├── CMakeLists.txt │ ├── Registration.cpp │ └── Registration.hpp └── ros ├── CMakeLists.txt ├── LICENSE ├── config └── kinematic_icp_ros.yaml ├── include └── kinematic_icp_ros │ ├── nodes │ ├── offline_node.hpp │ └── online_node.hpp │ ├── server │ └── LidarOdometryServer.hpp │ └── utils │ ├── RosUtils.hpp │ ├── RosbagUtils.hpp │ ├── TimeStampHandler.hpp │ └── indicators.hpp ├── launch ├── common_args.launch.py ├── offline_node.launch.py └── online_node.launch.py ├── package.xml ├── rviz └── kinematic_icp.rviz └── src └── kinematic_icp_ros ├── nodes ├── offline_node.cpp └── online_node.cpp ├── server └── LidarOdometryServer.cpp └── utils ├── RosUtils.cpp ├── RosbagUtils.cpp └── TimeStampHandler.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | UseTab: Never 3 | IndentWidth: 4 4 | AccessModifierOffset: -4 5 | ColumnLimit: 100 6 | BinPackParameters: false 7 | SortIncludes: true 8 | Standard: c++17 9 | DerivePointerAlignment: false 10 | PointerAlignment: Right 11 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | enable_markup: false 2 | line_width: 120 3 | format: 4 | max_subgroups_hwrap: 5 5 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @nachovizzo @benemer @tizianoGuadagnino 2 | -------------------------------------------------------------------------------- /.github/workflows/cpp.yml: -------------------------------------------------------------------------------- 1 | name: C++ API 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | env: 10 | BUILD_TYPE: Release 11 | 12 | jobs: 13 | cpp_api: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] 18 | steps: 19 | - uses: actions/checkout@v3 20 | - name: Setup cmake 21 | uses: jwlawson/actions-setup-cmake@v1.13 22 | with: 23 | cmake-version: "3.25.x" 24 | - name: Configure CMake 25 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp 26 | - name: Build 27 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 28 | 29 | # As the previous job will always install the dependencies from cmake, and this is guaranteed to 30 | # work, we also want to support dev sandboxes where the main dependencies are already 31 | # pre-installed in the system. For now, we only support dev machines under a GNU/Linux 32 | # environmnets. If you are reading this and need the same functionallity in Windows/macOS please 33 | # open a ticket. 34 | cpp_api_dev: 35 | runs-on: ${{ matrix.os }} 36 | strategy: 37 | matrix: 38 | os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04] 39 | 40 | steps: 41 | - uses: actions/checkout@v3 42 | - name: Cache dependencies 43 | uses: actions/cache@v2 44 | with: 45 | path: ~/.apt/cache 46 | key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }} 47 | restore-keys: | 48 | ${{ runner.os }}-apt- 49 | - name: Install dependencies 50 | run: | 51 | sudo apt-get update 52 | sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev 53 | - name: Configure CMake 54 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp 55 | - name: Build 56 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 57 | -------------------------------------------------------------------------------- /.github/workflows/pre-commit.yml: -------------------------------------------------------------------------------- 1 | name: Style Check 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | jobs: 10 | pre-commit: 11 | name: Pre-commit checks 12 | runs-on: ubuntu-latest 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: actions/setup-python@v4 16 | with: 17 | python-version: "3.10" 18 | - uses: pre-commit/action@v3.0.0 19 | -------------------------------------------------------------------------------- /.github/workflows/ros.yml: -------------------------------------------------------------------------------- 1 | name: ROS nodes 2 | 3 | on: 4 | push: 5 | branches: ["main"] 6 | pull_request: 7 | branches: ["main"] 8 | 9 | jobs: 10 | ros2_node: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | release: [humble, iron, jazzy, rolling] 15 | container: osrf/ros:${{ matrix.release }}-desktop 16 | steps: 17 | - name: Setup cmake 18 | uses: jwlawson/actions-setup-cmake@v1.13 19 | with: 20 | cmake-version: "3.25.x" 21 | - uses: actions/checkout@v3 22 | - name: Run colcon 23 | run: source /opt/ros/${{ matrix.release }}/setup.bash && colcon build --event-handlers console_direct+ 24 | shell: bash 25 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.toptal.com/developers/gitignore/api/c++ 2 | # Edit at https://www.toptal.com/developers/gitignore?templates=c++ 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | # End of https://www.toptal.com/developers/gitignore/api/c++ 39 | n# Created by https://www.toptal.com/developers/gitignore/api/cmake 40 | # Edit at https://www.toptal.com/developers/gitignore?templates=cmake 41 | 42 | ### CMake ### 43 | CMakeLists.txt.user 44 | CMakeCache.txt 45 | CMakeFiles 46 | CMakeScripts 47 | Testing 48 | Makefile 49 | cmake_install.cmake 50 | install_manifest.txt 51 | compile_commands.json 52 | CTestTestfile.cmake 53 | _deps 54 | 55 | ### CMake Patch ### 56 | # External projects 57 | *-prefix/ 58 | 59 | # End of https://www.toptal.com/developers/gitignore/api/cmake 60 | n# Created by https://www.toptal.com/developers/gitignore/api/ros 61 | # Edit at https://www.toptal.com/developers/gitignore?templates=ros 62 | 63 | ### ROS ### 64 | devel/ 65 | logs/ 66 | build/ 67 | bin/ 68 | lib/ 69 | msg_gen/ 70 | srv_gen/ 71 | msg/*Action.msg 72 | msg/*ActionFeedback.msg 73 | msg/*ActionGoal.msg 74 | msg/*ActionResult.msg 75 | msg/*Feedback.msg 76 | msg/*Goal.msg 77 | msg/*Result.msg 78 | msg/_*.py 79 | build_isolated/ 80 | devel_isolated/ 81 | 82 | # Generated by dynamic reconfigure 83 | *.cfgc 84 | /cfg/cpp/ 85 | /cfg/*.py 86 | 87 | # Ignore generated docs 88 | *.dox 89 | *.wikidoc 90 | 91 | # eclipse stuff 92 | .project 93 | .cproject 94 | 95 | # qcreator stuff 96 | CMakeLists.txt.user 97 | 98 | srv/_*.py 99 | *.pcd 100 | *.pyc 101 | qtcreator-* 102 | *.user 103 | 104 | /planning/cfg 105 | /planning/docs 106 | /planning/src 107 | 108 | *~ 109 | 110 | # Emacs 111 | .#* 112 | 113 | # Catkin custom files 114 | CATKIN_IGNORE 115 | 116 | # End of https://www.toptal.com/developers/gitignore/api/ros 117 | n 118 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.4.0 4 | hooks: 5 | - id: trailing-whitespace 6 | exclude: \.patch$ 7 | - id: end-of-file-fixer 8 | exclude: \.patch$ 9 | - id: check-yaml 10 | - id: check-added-large-files 11 | - repo: https://github.com/pre-commit/mirrors-clang-format 12 | rev: v14.0.0 13 | hooks: 14 | - id: clang-format 15 | - repo: https://github.com/psf/black 16 | rev: 23.1.0 17 | hooks: 18 | - id: black 19 | - repo: https://github.com/ahans/cmake-format-precommit 20 | rev: 8e52fb6506f169dddfaa87f88600d765fca48386 21 | hooks: 22 | - id: cmake-format 23 | - repo: https://github.com/pycqa/isort 24 | rev: 5.12.0 25 | hooks: 26 | - id: isort 27 | args: ["--profile", "black", "--filter-files"] 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: cpp 2 | 3 | cpp: 4 | @cmake -Bbuild cpp/kinematic_icp/ 5 | @cmake --build build -j$(nproc --all) 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

Kinematic-ICP

3 | 4 | 5 | 6 |
7 |
8 | Paper 9 |   •   10 | Contact Us 11 |
12 |
13 | 14 | [Kinematic-ICP](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf) is a LiDAR odometry approach that explicitly incorporates the kinematic constraints of mobile robots into the classic point-to-point ICP algorithm. 15 | 16 | Kinematic-ICP 17 | 18 |
19 | 20 | # How to Build 21 | 22 | Our system operates on ROS2, supporting **ROS Humble**, **Iron**, and **Jazzy**. To build and run Kinematic-ICP, follow these steps: 23 | 24 | 1. **Clone the Repository**: 25 | ```sh 26 | cd /src 27 | git clone https://github.com/PRBonn/kinematic-icp 28 | cd .. 29 | ``` 30 | 31 | 2. **Ensure all Dependencies are Installed**: 32 | ```sh 33 | rosdep install --from-paths src --ignore-src -r -y 34 | ``` 35 | 36 | 3. **Build the Workspace**: 37 | ```sh 38 | colcon build 39 | ``` 40 | 41 | 4. **Source the Setup Script**: 42 | ```sh 43 | source ./install/setup.bash 44 | ``` 45 | 46 | 47 | # TF Requirements 48 | 49 | Kinematic ICP can enhance existing odometry using a 3D LiDAR. However, there are specific requirements regarding motion and transformations since we use a kinematic motion model for the pose correction. Below are the key requirements: 50 | 51 | 1. **Planar Movement**: The robot is expected to move primarily on a planar surface. 52 | 53 | 2. **Existing Odometry**: An existing odometry source must be provided, such as the platform's wheel odometry. In the ROS ecosystem, this means that another node must publish the `tf` transformation between `base_link` and `odom`. (Note: The names may vary and can be adjusted in the pipeline parameters.) 54 | 55 | 3. **Static Transform for LiDAR**: To utilize the platform's motion model effectively, the system needs to compute the pose in `base_link`. Therefore, a static `tf` transform between `base_link` and the LiDAR frame (extrinsic calibration) is required. If this calibration is significantly inaccurate, it may compromise system performance. 56 | 57 | Finally, Kinematic ICP will publish a new `tf` transformation between `base_link` and `odom_lidar`. 58 | 59 | # Running the System 60 | 61 | This system offers two entry points for deployment, depending on your use case: one for real-time operation and one for offline processing. 62 | 63 | ## 1. Real-Time Deployment: `online_node` 64 | 65 | Use the `online_node` to run the system on a robotics platform. The only required parameter is the **lidar_topic**. You can start the system using the following command: 66 | 67 | ```sh 68 | ros2 launch kinematic_icp online_node.launch.py lidar_topic:= 69 | ``` 70 | 71 | To enable simultaneous visualization through RViz, use the `visualize` flag set to `true`: 72 | 73 | ```sh 74 | ros2 launch kinematic_icp online_node.launch.py lidar_topic:= visualize:=true 75 | ``` 76 | 77 | ## 2. Offline Processing: `offline_node` 78 | 79 | For post-processing and analysis, the `offline_node` processes a ROS bag file at CPU speed, ensuring no frames are dropped. This mode is ideal for reviewing trajectory results, debugging, and speeding up bag file processing. You can launch the offline node with the following command: 80 | 81 | ```sh 82 | ros2 launch kinematic_icp offline_node.launch.py lidar_topic:= bag_filename:= 83 | ``` 84 | 85 | RViz can also be used in this mode by setting the `visualize` flag to `true`. Additionally, the system will output a file in TUM format containing the estimated poses, named **\_kinematic_poses_tum.txt**. This file is saved in the same directory as the ROS bag file by default. 86 | 87 | To specify a custom directory for the output file, use the `output_dir` parameter: 88 | 89 | ```sh 90 | ros2 launch kinematic_icp offline_node.launch.py lidar_topic:= bag_filename:= output_dir:= 91 | ``` 92 | 93 | ## 2D LiDAR support 94 | 95 | You can run both the `online_node` and the `offline_node` on a 2D Laser topic (with message type `LaserScan`) by setting the `use_2d_lidar` flag to `true`. For example: 96 | 97 | ```sh 98 | ros2 launch kinematic_icp online_node.launch.py lidar_topic:= use_2d_lidar:=true 99 | ``` 100 | ## Sample data 101 | 102 | If you want to test the code on a sample bag file, just download it [here](https://www.ipb.uni-bonn.de/html/projects/kinematic-icp/2024-08-23-11-05-41_0_clipped.mcap) and run 103 | 104 | ```sh 105 | ros2 launch kinematic_icp offline_node.launch.py lidar_topic:=/lidar_points bag_filename:=./2024-08-23-11-05-41_0_clipped.mcap 106 | ``` 107 | 108 | or for the 2D LiDAR 109 | 110 | ```sh 111 | ros2 launch kinematic_icp offline_node.launch.py lidar_topic:=/front_scan bag_filename:=./2024-08-23-11-05-41_0_clipped.mcap use_2d_lidar:=true 112 | ``` 113 | 114 | ## Citation 115 | 116 | If you use this library for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf). 117 | 118 | ```bibtex 119 | @article{kissteam2024arxiv, 120 | author = {Guadagnino ,Tiziano and Mersch, Benedikt and Vizzo, Ignacio and Gupta, Saurabh and Malladi, Meher V.R. and Lobefaro, Luca and Doisy, Guillaume and Stachniss, Cyrill}, 121 | title = {{Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints for Wheeled Mobile Robots Moving on Planar Surfaces}}, 122 | journal = arXiv Preprint, 123 | year = {2024}, 124 | volume = {arXiv:2410.10277}, 125 | url = {https://arxiv.org/pdf/2410.10277}, 126 | } 127 | ``` 128 | -------------------------------------------------------------------------------- /cpp/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PRBonn/kinematic-icp/7a761dd16cd6106c621e2e12bc3cda7a85903da5/cpp/COLCON_IGNORE -------------------------------------------------------------------------------- /cpp/kinematic_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kinematic_icp_cpp VERSION 0.1.1 LANGUAGES CXX) 25 | 26 | # Setup build options for the underlying kiss dependency 27 | option(USE_CCACHE "Build using Ccache if found on the path" ON) 28 | option(USE_SYSTEM_EIGEN3 "Use system pre-installed Eigen" ON) 29 | option(USE_SYSTEM_SOPHUS "Use system pre-installed Sophus" ON) 30 | option(USE_SYSTEM_TSL-ROBIN-MAP "Use system pre-installed tsl_robin" ON) 31 | option(USE_SYSTEM_TBB "Use system pre-installed oneAPI/tbb" ON) 32 | include(kiss_icp/kiss-icp.cmake) 33 | 34 | # KISS dependecies that are also used in Kinematic. 35 | # If the internal FetchContent from kiss is in use (through the above options) 36 | # find_package will use those dependencies, otherwise it will search system-wise 37 | find_package(Eigen3 QUIET NO_MODULE) 38 | find_package(Sophus QUIET NO_MODULE) 39 | find_package(TBB QUIET NO_MODULE) 40 | # ccache setup 41 | if(USE_CCACHE) 42 | find_program(CCACHE_PATH ccache) 43 | if(CCACHE_PATH) 44 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 45 | set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 46 | message(STATUS "Using ccache: ${CCACHE_PATH}") 47 | endif() 48 | endif() 49 | 50 | # Set build type (repeat here for C++ only consumers) 51 | set(CMAKE_BUILD_TYPE Release) 52 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 53 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 54 | 55 | include(cmake/CompilerOptions.cmake) 56 | 57 | add_subdirectory(correspondence_threshold) 58 | add_subdirectory(registration) 59 | add_subdirectory(pipeline) 60 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/cmake/CompilerOptions.cmake: -------------------------------------------------------------------------------- 1 | # We ackowledge the original license for this file 2 | # MIT License 3 | # 4 | # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 5 | # Stachniss. 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | function(set_global_target_properties target) 25 | target_compile_features(${target} PUBLIC cxx_std_17) 26 | target_compile_definitions(${target} PUBLIC $<$:_USE_MATH_DEFINES>) 27 | target_compile_options( 28 | ${target} 29 | PRIVATE # MSVC 30 | $<$:/W4> 31 | $<$:/WX> 32 | # Clang/AppleClang 33 | $<$:-fcolor-diagnostics> 34 | $<$:-Werror> 35 | $<$:-Wall> 36 | $<$:-Wextra> 37 | $<$:-Wconversion> 38 | $<$:-Wno-sign-conversion> 39 | # GNU 40 | $<$:-fdiagnostics-color=always> 41 | $<$:-Wall> 42 | $<$:-Wextra> 43 | $<$:-Wcast-align> 44 | $<$:-Wcast-qual> 45 | $<$:-Wconversion> 46 | $<$:-Wdisabled-optimization> 47 | $<$:-Woverloaded-virtual>) 48 | set(INCLUDE_DIRS ${PROJECT_SOURCE_DIR}) 49 | get_filename_component(INCLUDE_DIRS ${INCLUDE_DIRS} PATH) 50 | target_include_directories(${target} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 51 | PUBLIC $ $) 52 | endfunction() 53 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/correspondence_threshold/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(kinematic_icp_threshold STATIC) 24 | target_sources(kinematic_icp_threshold PRIVATE CorrespondenceThreshold.cpp) 25 | target_link_libraries(kinematic_icp_threshold PUBLIC Sophus::Sophus) 26 | set_global_target_properties(kinematic_icp_threshold) 27 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "CorrespondenceThreshold.hpp" 24 | 25 | #include 26 | #include 27 | 28 | namespace { 29 | double OdometryErrorInPointSpace(const Sophus::SE3d &pose, const double max_range) { 30 | const double &theta = pose.so3().logAndTheta().theta; 31 | const double &delta_rot = 2.0 * max_range * std::sin(theta / 2.0); 32 | const double &delta_trans = pose.translation().norm(); 33 | return delta_trans + delta_rot; 34 | }; 35 | } // namespace 36 | 37 | namespace kinematic_icp { 38 | CorrespondenceThreshold::CorrespondenceThreshold(const double map_discretization_error, 39 | const double max_range, 40 | const bool use_adaptive_threshold, 41 | const double fixed_threshold) 42 | : map_discretization_error_(map_discretization_error), 43 | max_range_(max_range), 44 | use_adaptive_threshold_(use_adaptive_threshold), 45 | fixed_threshold_(fixed_threshold), 46 | odom_sse_(0.0), 47 | num_samples_(1e-8) {} 48 | 49 | double CorrespondenceThreshold::ComputeThreshold() const { 50 | if (!use_adaptive_threshold_) return fixed_threshold_; 51 | 52 | const double sigma_odom = std::sqrt(odom_sse_ / num_samples_); 53 | const double &sigma_map = map_discretization_error_; // <-- Renaming for clarity 54 | const double adaptive_threshold = 3.0 * (sigma_map + sigma_odom); 55 | return adaptive_threshold; 56 | } 57 | 58 | void CorrespondenceThreshold::UpdateOdometryError(const Sophus::SE3d &odometry_error) { 59 | if (!use_adaptive_threshold_) return; 60 | 61 | const double &odom_error_in_point_space = OdometryErrorInPointSpace(odometry_error, max_range_); 62 | odom_sse_ += odom_error_in_point_space * odom_error_in_point_space; 63 | num_samples_ += 1.0; 64 | } 65 | 66 | } // namespace kinematic_icp 67 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | // 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | // 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | // 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace kinematic_icp { 29 | 30 | struct CorrespondenceThreshold { 31 | explicit CorrespondenceThreshold(const double map_discretization_error, 32 | const double max_range, 33 | const bool use_adaptive_threshold, 34 | const double fixed_threshold); 35 | 36 | void UpdateOdometryError(const Sophus::SE3d &odometry_error); 37 | 38 | double ComputeThreshold() const; 39 | 40 | inline void Reset() { 41 | odom_sse_ = 0.0; 42 | num_samples_ = 1e-8; 43 | } 44 | 45 | // configurable parameters 46 | double map_discretization_error_; // <-- Error introduced by the fact that we have a discrete 47 | // set of points of the surface we are measuring 48 | double max_range_; 49 | bool use_adaptive_threshold_; 50 | double fixed_threshold_; // <-- Ignored if use_adaptive_threshold_ = true 51 | 52 | // Local cache for computation 53 | double odom_sse_; 54 | double num_samples_; 55 | }; 56 | } // namespace kinematic_icp 57 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/kiss_icp/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/kiss_icp/kiss-icp.cmake: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | # Silence timestamp warning 24 | if(CMAKE_VERSION VERSION_GREATER 3.24) 25 | cmake_policy(SET CMP0135 OLD) 26 | endif() 27 | 28 | include(FetchContent) 29 | FetchContent_Declare(kiss_icp URL https://github.com/PRBonn/kiss-icp/archive/refs/tags/v1.2.0.tar.gz SOURCE_SUBDIR 30 | cpp/kiss_icp) 31 | FetchContent_MakeAvailable(kiss_icp) 32 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/pipeline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(kinematic_icp_pipeline STATIC) 24 | target_sources(kinematic_icp_pipeline PRIVATE KinematicICP.cpp) 25 | target_link_libraries(kinematic_icp_pipeline PUBLIC kinematic_icp_registration kinematic_icp_threshold 26 | kiss_icp_pipeline) 27 | set_global_target_properties(kinematic_icp_pipeline) 28 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/pipeline/KinematicICP.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "KinematicICP.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace { 31 | auto transform_points(const std::vector &points, const Sophus::SE3d &pose) { 32 | std::vector points_transformed(points.size()); 33 | std::transform(points.cbegin(), points.cend(), points_transformed.begin(), 34 | [&](const auto &point) { return pose * point; }); 35 | return points_transformed; 36 | } 37 | 38 | auto Voxelize(const std::vector &frame, const double voxel_size) { 39 | const std::vector &frame_downsample = 40 | kiss_icp::VoxelDownsample(frame, voxel_size * 0.5); 41 | const std::vector &source = 42 | kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5); 43 | return std::make_tuple(source, frame_downsample); 44 | } 45 | } // namespace 46 | namespace kinematic_icp::pipeline { 47 | 48 | KinematicICP::Vector3dVectorTuple KinematicICP::RegisterFrame( 49 | const std::vector &frame, 50 | const std::vector ×tamps, 51 | const Sophus::SE3d &lidar_to_base, 52 | const Sophus::SE3d &relative_odometry) { 53 | // Need to deskew in lidar frame 54 | const Sophus::SE3d &relative_odometry_in_lidar = 55 | lidar_to_base.inverse() * relative_odometry * lidar_to_base; 56 | const auto &preprocessed_frame = 57 | preprocessor_.Preprocess(frame, timestamps, relative_odometry_in_lidar); 58 | // Give the frame in base frame 59 | const auto &preprocessed_frame_in_base = transform_points(preprocessed_frame, lidar_to_base); 60 | // Voxelize 61 | const auto &[source, frame_downsample] = 62 | Voxelize(preprocessed_frame_in_base, config_.voxel_size); 63 | 64 | // Get adaptive_threshold 65 | const double tau = correspondence_threshold_.ComputeThreshold(); 66 | 67 | // Run ICP 68 | const auto new_pose = registration_.ComputeRobotMotion(source, // frame 69 | local_map_, // voxel_map 70 | last_pose_, // last_pose 71 | relative_odometry, // robot_motion 72 | tau); // max_correspondence_dist 73 | 74 | // Compute the difference between the prediction and the actual estimate 75 | const auto odometry_error = (last_pose_ * relative_odometry).inverse() * new_pose; 76 | 77 | // Update step: threshold, local map and the last pose 78 | correspondence_threshold_.UpdateOdometryError(odometry_error); 79 | local_map_.Update(frame_downsample, new_pose); 80 | last_pose_ = new_pose; 81 | 82 | // Return the (deskew) input raw scan (frame) and the points used for 83 | // registration (source) 84 | return {preprocessed_frame_in_base, source}; 85 | } 86 | } // namespace kinematic_icp::pipeline 87 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/pipeline/KinematicICP.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "kinematic_icp/correspondence_threshold/CorrespondenceThreshold.hpp" 34 | #include "kinematic_icp/registration/Registration.hpp" 35 | 36 | namespace kinematic_icp::pipeline { 37 | 38 | struct Config { 39 | // Preprocessing 40 | double max_range = 100.0; 41 | double min_range = 0.0; 42 | // Mapping parameters 43 | double voxel_size = 1.0; 44 | unsigned int max_points_per_voxel = 20; 45 | // Derived parameter, will be computed from other parts of the configuration 46 | constexpr double map_resolution() const { return voxel_size / std::sqrt(max_points_per_voxel); } 47 | // Correspondence threshold parameters 48 | bool use_adaptive_threshold = true; 49 | double fixed_threshold = 1.0; // <-- Ignored if use_adaptive_threshold = true 50 | 51 | // Registration Parameters 52 | int max_num_iterations = 10; 53 | double convergence_criterion = 0.001; 54 | int max_num_threads = 1; 55 | bool use_adaptive_odometry_regularization = true; 56 | double fixed_regularization = 0.0; // <-- Ignored if use_adaptive_threshold = true 57 | 58 | // Motion compensation 59 | bool deskew = false; 60 | }; 61 | 62 | class KinematicICP { 63 | public: 64 | using Vector3dVector = std::vector; 65 | using Vector3dVectorTuple = std::tuple; 66 | 67 | explicit KinematicICP(const Config &config) 68 | : registration_(config.max_num_iterations, 69 | config.convergence_criterion, 70 | config.max_num_threads, 71 | config.use_adaptive_odometry_regularization, 72 | config.fixed_regularization), 73 | correspondence_threshold_(config.map_resolution(), 74 | config.max_range, 75 | config.use_adaptive_threshold, 76 | config.fixed_threshold), 77 | config_(config), 78 | preprocessor_(config.max_range, config.min_range, config.deskew, config.max_num_threads), 79 | local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel) {} 80 | 81 | Vector3dVectorTuple RegisterFrame(const std::vector &frame, 82 | const std::vector ×tamps, 83 | const Sophus::SE3d &lidar_to_base, 84 | const Sophus::SE3d &relative_odometry); 85 | 86 | inline void SetPose(const Sophus::SE3d &pose) { 87 | last_pose_ = pose; 88 | local_map_.Clear(); 89 | correspondence_threshold_.Reset(); 90 | }; 91 | 92 | std::vector LocalMap() const { return local_map_.Pointcloud(); }; 93 | 94 | const kiss_icp::VoxelHashMap &VoxelMap() const { return local_map_; }; 95 | kiss_icp::VoxelHashMap &VoxelMap() { return local_map_; }; 96 | 97 | const Sophus::SE3d &pose() const { return last_pose_; } 98 | Sophus::SE3d &pose() { return last_pose_; } 99 | 100 | protected: 101 | Sophus::SE3d last_pose_; 102 | // Kinematic module 103 | KinematicRegistration registration_; 104 | CorrespondenceThreshold correspondence_threshold_; 105 | Config config_; 106 | // KISS-ICP pipeline modules 107 | kiss_icp::Preprocessor preprocessor_; 108 | kiss_icp::VoxelHashMap local_map_; 109 | }; 110 | 111 | } // namespace kinematic_icp::pipeline 112 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/registration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | add_library(kinematic_icp_registration STATIC) 24 | target_sources(kinematic_icp_registration PRIVATE Registration.cpp) 25 | target_link_libraries(kinematic_icp_registration PUBLIC kiss_icp_core Eigen3::Eigen TBB::tbb Sophus::Sophus) 26 | set_global_target_properties(kinematic_icp_registration) 27 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/registration/Registration.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "Registration.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | using LinearSystem = std::pair; 43 | using Correspondences = tbb::concurrent_vector>; 44 | 45 | namespace { 46 | constexpr double epsilon = std::numeric_limits::min(); 47 | 48 | double ComputeOdometryRegularization(const Correspondences &associations, 49 | const Sophus::SE3d &odometry_initial_guess) { 50 | const double sum_of_squared_residuals = 51 | std::transform_reduce(associations.cbegin(), associations.cend(), 0.0, std::plus(), 52 | [&](const auto &association) { 53 | const auto &[source, target] = association; 54 | return (odometry_initial_guess * source - target).squaredNorm(); 55 | }); 56 | const double N = static_cast(associations.size()); 57 | const double mean_squared_residual = sum_of_squared_residuals / N; 58 | const double beta = 1.0 / (mean_squared_residual + epsilon); 59 | return beta; 60 | } 61 | 62 | Correspondences DataAssociation(const std::vector &points, 63 | const kiss_icp::VoxelHashMap &voxel_map, 64 | const Sophus::SE3d &T, 65 | const double max_correspondance_distance) { 66 | using points_iterator = std::vector::const_iterator; 67 | Correspondences correspondences; 68 | correspondences.reserve(points.size()); 69 | tbb::parallel_for( 70 | // Range 71 | tbb::blocked_range{points.cbegin(), points.cend()}, 72 | [&](const tbb::blocked_range &r) { 73 | std::for_each(r.begin(), r.end(), [&](const auto &point) { 74 | const auto &[closest_neighbor, distance] = voxel_map.GetClosestNeighbor(T * point); 75 | if (distance < max_correspondance_distance) { 76 | correspondences.emplace_back(point, closest_neighbor); 77 | } 78 | }); 79 | }); 80 | return correspondences; 81 | } 82 | 83 | Eigen::Vector2d ComputePerturbation(const Correspondences &correspondences, 84 | const Sophus::SE3d ¤t_estimate, 85 | const double beta) { 86 | auto compute_jacobian_and_residual = [&](const auto &correspondence) { 87 | const auto &[source, target] = correspondence; 88 | const Eigen::Vector3d residual = current_estimate * source - target; 89 | Eigen::Matrix J; 90 | J.col(0) = current_estimate.so3() * Eigen::Vector3d::UnitX(); 91 | J.col(1) = current_estimate.so3() * Eigen::Vector3d(-source.y(), source.x(), 0.0); 92 | return std::make_tuple(J, residual); 93 | }; 94 | 95 | auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) { 96 | a.first += b.first; 97 | a.second += b.second; 98 | return a; 99 | }; 100 | 101 | using correspondence_iterator = Correspondences::const_iterator; 102 | auto [JTJ, JTr] = tbb::parallel_reduce( 103 | // Range 104 | tbb::blocked_range{correspondences.cbegin(), 105 | correspondences.cend()}, 106 | // Identity 107 | LinearSystem(Eigen::Matrix2d::Zero(), Eigen::Vector2d::Zero()), 108 | // 1st Lambda: Parallel computation 109 | [&](const tbb::blocked_range &r, LinearSystem J) -> LinearSystem { 110 | return std::transform_reduce( 111 | r.begin(), r.end(), J, sum_linear_systems, [&](const auto &correspondence) { 112 | const auto &[J_r, residual] = compute_jacobian_and_residual(correspondence); 113 | return LinearSystem(J_r.transpose() * J_r, // JTJ 114 | J_r.transpose() * residual); // JTr 115 | }); 116 | }, 117 | // 2nd Lambda: Parallel reduction of the private Jacboians 118 | sum_linear_systems); 119 | const double num_correspondences = static_cast(correspondences.size()); 120 | 121 | const Eigen::Matrix2d Omega = Eigen::Vector2d(beta, 0).asDiagonal(); 122 | JTJ /= num_correspondences; 123 | JTr /= num_correspondences; 124 | JTJ += Omega; 125 | return -(JTJ.inverse() * JTr); 126 | } 127 | 128 | } // namespace 129 | 130 | namespace kinematic_icp { 131 | 132 | KinematicRegistration::KinematicRegistration(const int max_num_iteration, 133 | const double convergence_criterion, 134 | const int max_num_threads, 135 | const bool use_adaptive_odometry_regularization, 136 | const double fixed_regularization) 137 | : max_num_iterations_(max_num_iteration), 138 | convergence_criterion_(convergence_criterion), 139 | // Only manipulate the number of threads if the user specifies something 140 | // greater than 0 141 | max_num_threads_(max_num_threads > 0 ? max_num_threads 142 | : tbb::this_task_arena::max_concurrency()), 143 | use_adaptive_odometry_regularization_(use_adaptive_odometry_regularization), 144 | fixed_regularization_(fixed_regularization) { 145 | // This global variable requires static duration storage to be able to 146 | // manipulate the max concurrency from TBB across the entire class 147 | static const auto tbb_control_settings = tbb::global_control( 148 | tbb::global_control::max_allowed_parallelism, static_cast(max_num_threads_)); 149 | } 150 | 151 | Sophus::SE3d KinematicRegistration::ComputeRobotMotion(const std::vector &frame, 152 | const kiss_icp::VoxelHashMap &voxel_map, 153 | const Sophus::SE3d &last_robot_pose, 154 | const Sophus::SE3d &relative_wheel_odometry, 155 | const double max_correspondence_distance) { 156 | Sophus::SE3d current_estimate = last_robot_pose * relative_wheel_odometry; 157 | if (voxel_map.Empty()) return current_estimate; 158 | 159 | auto motion_model = [](const Eigen::Vector2d &integrated_controls) { 160 | Sophus::SE3d::Tangent dx = Sophus::SE3d::Tangent::Zero(); 161 | const double &displacement = integrated_controls(0); 162 | const double &theta = integrated_controls(1); 163 | dx(0) = displacement * std::sin(theta) / (theta + epsilon); 164 | dx(1) = displacement * (1.0 - std::cos(theta)) / (theta + epsilon); 165 | dx(5) = theta; 166 | return Sophus::SE3d::exp(dx); 167 | }; 168 | auto correspondences = 169 | DataAssociation(frame, voxel_map, current_estimate, max_correspondence_distance); 170 | 171 | const double regularization_term = [&]() { 172 | if (use_adaptive_odometry_regularization_) { 173 | return ComputeOdometryRegularization(correspondences, current_estimate); 174 | } else { 175 | return fixed_regularization_; 176 | } 177 | }(); 178 | // ICP-loop 179 | for (int j = 0; j < max_num_iterations_; ++j) { 180 | const auto dx = ComputePerturbation(correspondences, current_estimate, regularization_term); 181 | const auto delta_motion = motion_model(dx); 182 | current_estimate = current_estimate * delta_motion; 183 | // Break loop 184 | if (dx.norm() < convergence_criterion_) break; 185 | correspondences = 186 | DataAssociation(frame, voxel_map, current_estimate, max_correspondence_distance); 187 | } 188 | // Spit the final transformation 189 | return current_estimate; 190 | } 191 | } // namespace kinematic_icp 192 | -------------------------------------------------------------------------------- /cpp/kinematic_icp/registration/Registration.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace kinematic_icp { 31 | 32 | struct KinematicRegistration { 33 | explicit KinematicRegistration(const int max_num_iteration, 34 | const double convergence_criterion, 35 | const int max_num_threads, 36 | const bool use_adaptive_odometry_regularization, 37 | const double fixed_regularization); 38 | 39 | Sophus::SE3d ComputeRobotMotion(const std::vector &frame, 40 | const kiss_icp::VoxelHashMap &voxel_map, 41 | const Sophus::SE3d &last_robot_pose, 42 | const Sophus::SE3d &relative_wheel_odometry, 43 | const double max_correspondence_distance); 44 | 45 | int max_num_iterations_; 46 | double convergence_criterion_; 47 | int max_num_threads_; 48 | bool use_adaptive_odometry_regularization_; 49 | double fixed_regularization_; 50 | }; 51 | } // namespace kinematic_icp 52 | -------------------------------------------------------------------------------- /ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | # Stachniss. 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in all 14 | # copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | # SOFTWARE. 23 | cmake_minimum_required(VERSION 3.16...3.26) 24 | project(kinematic_icp VERSION 0.1.1 LANGUAGES CXX) 25 | 26 | set(CMAKE_BUILD_TYPE Release) 27 | 28 | add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp/kinematic_icp ${CMAKE_CURRENT_BINARY_DIR}/kinematic_icp) 29 | 30 | find_package(ament_cmake REQUIRED) 31 | find_package(geometry_msgs REQUIRED) 32 | find_package(nav_msgs REQUIRED) 33 | find_package(rclcpp REQUIRED) 34 | find_package(rclcpp_components REQUIRED) 35 | find_package(rcutils REQUIRED) 36 | find_package(rosbag2_cpp REQUIRED) 37 | find_package(rosbag2_storage REQUIRED) 38 | find_package(sensor_msgs REQUIRED) 39 | find_package(laser_geometry REQUIRED) 40 | find_package(std_msgs REQUIRED) 41 | find_package(std_srvs REQUIRED) 42 | find_package(tf2_ros REQUIRED) 43 | find_package(visualization_msgs REQUIRED) 44 | 45 | set(dependencies 46 | geometry_msgs 47 | nav_msgs 48 | rclcpp 49 | rclcpp_components 50 | rcutils 51 | rosbag2_cpp 52 | rosbag2_storage 53 | sensor_msgs 54 | laser_geometry 55 | std_msgs 56 | std_srvs 57 | tf2_ros 58 | visualization_msgs) 59 | 60 | add_library( 61 | kinematic_icp_ros SHARED 62 | src/kinematic_icp_ros/server/LidarOdometryServer.cpp src/kinematic_icp_ros/utils/RosUtils.cpp 63 | src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/utils/TimeStampHandler.cpp 64 | src/kinematic_icp_ros/nodes/online_node.cpp) # Adding it here for composition 65 | target_compile_features(kinematic_icp_ros PUBLIC cxx_std_17) 66 | target_include_directories(kinematic_icp_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 67 | target_link_libraries(kinematic_icp_ros kinematic_icp_pipeline "${cpp_typesupport_target}") 68 | ament_target_dependencies(kinematic_icp_ros ${dependencies}) 69 | 70 | add_executable(kinematic_icp_offline_node src/kinematic_icp_ros/nodes/offline_node.cpp) 71 | target_link_libraries(kinematic_icp_offline_node PUBLIC kinematic_icp_ros) 72 | 73 | rclcpp_components_register_node(kinematic_icp_ros PLUGIN "kinematic_icp_ros::OnlineNode" EXECUTABLE 74 | kinematic_icp_online_node) 75 | 76 | install(TARGETS kinematic_icp_ros kinematic_icp_online_node kinematic_icp_offline_node LIBRARY DESTINATION lib 77 | RUNTIME DESTINATION lib/${PROJECT_NAME}) 78 | 79 | install(DIRECTORY config launch rviz DESTINATION share/${PROJECT_NAME}/) 80 | 81 | ament_package() 82 | -------------------------------------------------------------------------------- /ros/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | Stachniss. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /ros/config/kinematic_icp_ros.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # Preprocessing 4 | max_range: 100.0 5 | min_range: 0.0 6 | # Mapping parameters 7 | voxel_size: 1.0 8 | max_points_per_voxel: 20 9 | 10 | # Correspondence threshold parameters 11 | use_adaptive_threshold: true 12 | fixed_threshold: 1.0 # Ignored if use_adaptive_threshold=true 13 | 14 | # Registration Parameters 15 | max_num_iterations: 10 16 | convergence_criterion: 0.001 17 | max_num_threads: 1 18 | use_adaptive_odometry_regularization: true 19 | fixed_regularization: 0.0 # Ignored if use_adaptive_odometry_regularization=true 20 | 21 | # Motion Compensation 22 | deskew: true 23 | 24 | # Covariance diagonal values 25 | orientation_covariance: 0.1 26 | position_covariance: 0.1 27 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/nodes/offline_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | // ROS 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" 36 | #include "kinematic_icp_ros/utils/RosbagUtils.hpp" 37 | 38 | namespace kinematic_icp_ros { 39 | 40 | class OfflineNode { 41 | public: 42 | OfflineNode() = delete; 43 | explicit OfflineNode(const rclcpp::NodeOptions &options); 44 | 45 | void writePosesInTumFormat(); 46 | void Run(); 47 | 48 | private: 49 | // Common for offline/online nodes 50 | std::string lidar_topic_; 51 | bool use_2d_lidar_; 52 | std::shared_ptr odometry_server_; 53 | rclcpp::Node::SharedPtr node_; 54 | // store data for the experiments 55 | using PoseWithStamp = std::pair; 56 | std::vector poses_with_timestamps_; 57 | std::filesystem::path output_pose_file_; 58 | laser_geometry::LaserProjection laser_projector_; 59 | 60 | // Offline node specifics 61 | BagMultiplexer bag_multiplexer_; 62 | }; 63 | 64 | } // namespace kinematic_icp_ros 65 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/nodes/online_node.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | // ROS 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" 35 | 36 | namespace kinematic_icp_ros { 37 | 38 | class OnlineNode { 39 | public: 40 | OnlineNode() = delete; 41 | explicit OnlineNode(const rclcpp::NodeOptions &options); 42 | 43 | // Neccesary for ROS 2 composition 44 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface(); 45 | 46 | private: 47 | // Common for offline/online nodes 48 | std::string lidar_topic_; 49 | std::shared_ptr odometry_server_; 50 | rclcpp::Node::SharedPtr node_; 51 | laser_geometry::LaserProjection laser_projector_; 52 | 53 | // Online node specifics 54 | rclcpp::Subscription::SharedPtr pointcloud_sub_; 55 | rclcpp::Subscription::SharedPtr laser_scan_sub_; 56 | }; 57 | 58 | } // namespace kinematic_icp_ros 59 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include "kinematic_icp/pipeline/KinematicICP.hpp" 26 | #include "kinematic_icp_ros/utils/TimeStampHandler.hpp" 27 | 28 | // ROS 2 C 29 | #include 30 | #include 31 | #include 32 | 33 | // ROS 2 C++ 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | // STL 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | namespace kinematic_icp_ros { 50 | 51 | class LidarOdometryServer { 52 | public: 53 | /// LidarOdometryServer constructor 54 | LidarOdometryServer() = delete; 55 | explicit LidarOdometryServer(rclcpp::Node::SharedPtr node); 56 | 57 | /// Register new frame 58 | void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 59 | std::unique_ptr kinematic_icp_; 60 | utils::TimeStampHandler timestamps_handler_; 61 | 62 | private: 63 | /// Stream the estimated pose to ROS 64 | void PublishOdometryMsg(const Sophus::SE3d &pose, const Sophus::SE3d::Tangent &velocity); 65 | 66 | /// Stream the debugging point clouds for visualization (if required) 67 | void PublishClouds(const std::vector frame, 68 | const std::vector keypoints); 69 | 70 | // Temporal initializaiton strattegy until we convert the odometry server to life cycle 71 | void InitializePoseAndExtrinsic(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); 72 | 73 | /// Tools for broadcasting TFs. 74 | std::unique_ptr tf_broadcaster_; 75 | std::unique_ptr tf2_buffer_; 76 | std::unique_ptr tf2_listener_; 77 | std::chrono::milliseconds tf_timeout_; 78 | bool publish_odom_tf_; 79 | bool invert_odom_tf_; 80 | bool publish_debug_clouds_; 81 | Sophus::SE3d sensor_to_base_footprint_; 82 | 83 | /// Data publishers. 84 | rclcpp::Publisher::SharedPtr odom_publisher_; 85 | rclcpp::Publisher::SharedPtr frame_publisher_; 86 | rclcpp::Publisher::SharedPtr kpoints_publisher_; 87 | rclcpp::Publisher::SharedPtr map_publisher_; 88 | rclcpp::Publisher::SharedPtr voxel_grid_pub_; 89 | 90 | /// SetPose service 91 | rclcpp::Service::SharedPtr set_pose_srv_; 92 | 93 | /// Internal messages, models the current state that will be published by this node. 94 | nav_msgs::msg::Odometry odom_msg_; 95 | geometry_msgs::msg::TransformStamped tf_msg_; 96 | 97 | /// Global/map coordinate frame. 98 | std::string lidar_odom_frame_{"odom_lidar"}; 99 | std::string wheel_odom_frame_{"odom"}; 100 | std::string base_frame_{"base_link"}; 101 | // TF frame initialization flag 102 | bool initialize_odom_node{false}; 103 | 104 | rclcpp::Node::SharedPtr node_; 105 | }; 106 | 107 | } // namespace kinematic_icp_ros 108 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/utils/RosUtils.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | // tf2 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | // ROS 2 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace tf2 { 49 | 50 | inline geometry_msgs::msg::Transform sophusToTransform(const Sophus::SE3d &T) { 51 | geometry_msgs::msg::Transform t; 52 | t.translation.x = T.translation().x(); 53 | t.translation.y = T.translation().y(); 54 | t.translation.z = T.translation().z(); 55 | 56 | Eigen::Quaterniond q(T.so3().unit_quaternion()); 57 | t.rotation.x = q.x(); 58 | t.rotation.y = q.y(); 59 | t.rotation.z = q.z(); 60 | t.rotation.w = q.w(); 61 | 62 | return t; 63 | } 64 | 65 | inline geometry_msgs::msg::Pose sophusToPose(const Sophus::SE3d &T) { 66 | geometry_msgs::msg::Pose t; 67 | t.position.x = T.translation().x(); 68 | t.position.y = T.translation().y(); 69 | t.position.z = T.translation().z(); 70 | 71 | Eigen::Quaterniond q(T.so3().unit_quaternion()); 72 | t.orientation.x = q.x(); 73 | t.orientation.y = q.y(); 74 | t.orientation.z = q.z(); 75 | t.orientation.w = q.w(); 76 | 77 | return t; 78 | } 79 | 80 | inline Sophus::SE3d transformToSophus(const geometry_msgs::msg::TransformStamped &transform) { 81 | const auto &t = transform.transform; 82 | return Sophus::SE3d( 83 | Sophus::SE3d::QuaternionType(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z), 84 | Sophus::SE3d::Point(t.translation.x, t.translation.y, t.translation.z)); 85 | } 86 | 87 | inline Sophus::SE3d poseToSophus(const geometry_msgs::msg::Pose &pose) { 88 | const auto &T = pose; 89 | return Sophus::SE3d(Sophus::SE3d::QuaternionType(T.orientation.w, T.orientation.x, 90 | T.orientation.y, T.orientation.z), 91 | Sophus::SE3d::Point(T.position.x, T.position.y, T.position.z)); 92 | } 93 | 94 | } // namespace tf2 95 | 96 | namespace kinematic_icp_ros::utils { 97 | using PointCloud2 = sensor_msgs::msg::PointCloud2; 98 | using PointField = sensor_msgs::msg::PointField; 99 | using Header = std_msgs::msg::Header; 100 | 101 | inline Sophus::SE3d LookupTransform( 102 | const std::string &target_frame, 103 | const std::string &source_frame, 104 | const std::unique_ptr &tf2_buffer, 105 | const rclcpp::Time &time = tf2_ros::toRclcpp(tf2::TimePointZero)) { 106 | try { 107 | auto tf = tf2_buffer->lookupTransform(target_frame, source_frame, time); 108 | return tf2::transformToSophus(tf); 109 | } catch (tf2::TransformException &ex) { 110 | RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what()); 111 | return {}; 112 | } 113 | } 114 | 115 | inline Sophus::SE3d LookupDeltaTransform(const std::string &target_frame, 116 | const rclcpp::Time &target_time, 117 | const std::string &source_frame, 118 | const rclcpp::Time &source_time, 119 | const std::string &fixed_frame, 120 | const rclcpp::Duration timeout, 121 | const std::unique_ptr &tf2_buffer) { 122 | try { 123 | auto tf = tf2_buffer->lookupTransform(target_frame, target_time, source_frame, source_time, 124 | fixed_frame, timeout); 125 | return tf2::transformToSophus(tf); 126 | } catch (tf2::TransformException &ex) { 127 | RCLCPP_WARN(rclcpp::get_logger("LookupTransform"), "%s", ex.what()); 128 | return {}; 129 | } 130 | } 131 | 132 | std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, 133 | const Sophus::SE3d &T = {}); 134 | 135 | std::unique_ptr EigenToPointCloud2(const std::vector &points, 136 | const Header &header); 137 | 138 | visualization_msgs::msg::Marker VoxelsToMarker(const std::vector &voxels, 139 | const double voxel_size, 140 | const Header &header); 141 | } // namespace kinematic_icp_ros::utils 142 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/utils/RosbagUtils.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // tf2 33 | #include 34 | #include 35 | 36 | // ROS 37 | #include 38 | #include 39 | 40 | // rosbag headers 41 | #include 42 | #include 43 | 44 | class BufferableBag { 45 | public: 46 | // Wrapper node to process the transforamtions present in the bagfile 47 | struct TFBridge { 48 | TFBridge(rclcpp::Node::SharedPtr node); 49 | void ProcessTFMessage(std::shared_ptr msg) const; 50 | std::unique_ptr tf_broadcaster; 51 | std::unique_ptr tf_static_broadcaster; 52 | rclcpp::Serialization serializer; 53 | }; 54 | 55 | BufferableBag(const std::string &bag_filename, 56 | const std::shared_ptr tf_bridge, 57 | const std::string &topic, 58 | const std::chrono::seconds buffer_size = std::chrono::seconds(1)); 59 | 60 | std::size_t message_count() const; 61 | void BufferMessages(); 62 | rosbag2_storage::SerializedBagMessage PopNextMessage(); 63 | bool finished() const; 64 | 65 | private: 66 | std::shared_ptr tf_bridge_; 67 | std::unique_ptr bag_reader_; 68 | std::queue buffer_; 69 | std::chrono::seconds buffer_size_; 70 | std::string topic_; 71 | std::size_t message_count_{0}; 72 | }; 73 | 74 | class BagMultiplexer { 75 | public: 76 | // assume we feed this bags in ordered fashion, plesae behave 77 | void AddBag(BufferableBag &&bag); 78 | 79 | // How many messages in total in all the bagfiles 80 | std::size_t message_count() const; 81 | 82 | rosbag2_storage::SerializedBagMessage GetNextMessage(); 83 | bool IsMessageAvailable() const; 84 | 85 | private: 86 | std::vector bags_; 87 | std::size_t current_index_{0}; 88 | std::size_t message_count_{0}; 89 | }; 90 | -------------------------------------------------------------------------------- /ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #pragma once 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace kinematic_icp_ros::utils { 34 | 35 | using StampType = builtin_interfaces::msg::Time; 36 | 37 | struct TimeStampHandler { 38 | std::tuple> ProcessTimestamps( 39 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); 40 | // From double to ROS TimeStamp 41 | inline StampType toStamp(const double &time_in_seconds) const { 42 | return rclcpp::Time(tf2::durationFromSec(time_in_seconds).count()); 43 | }; 44 | // From ROS TimeStamp to double 45 | inline double toTime(const StampType &stamp) const { 46 | return rclcpp::Time(stamp).nanoseconds() * 1e-9; 47 | }; 48 | 49 | StampType last_processed_stamp_; 50 | }; 51 | 52 | } // namespace kinematic_icp_ros::utils 53 | -------------------------------------------------------------------------------- /ros/launch/common_args.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | 4 | 5 | def generate_launch_description(): 6 | return LaunchDescription( 7 | [ 8 | DeclareLaunchArgument( 9 | "lidar_topic", 10 | description="", 11 | ), 12 | DeclareLaunchArgument( 13 | "use_2d_lidar", 14 | default_value="false", 15 | description="", 16 | choices=["true", "false"], 17 | ), 18 | DeclareLaunchArgument( 19 | "lidar_odometry_topic", 20 | default_value="lidar_odometry", 21 | description="", 22 | ), 23 | DeclareLaunchArgument( 24 | "lidar_odom_frame", 25 | default_value="odom_lidar", 26 | description="", 27 | ), 28 | DeclareLaunchArgument( 29 | "wheel_odom_frame", 30 | default_value="odom", 31 | description="", 32 | ), 33 | DeclareLaunchArgument( 34 | "base_frame", 35 | default_value="base_footprint", 36 | description="", 37 | ), 38 | DeclareLaunchArgument( 39 | "publish_odom_tf", 40 | default_value="true", 41 | description="", 42 | choices=["true", "false"], 43 | ), 44 | DeclareLaunchArgument( 45 | "invert_odom_tf", 46 | default_value="true", 47 | description="", 48 | choices=["true", "false"], 49 | ), 50 | DeclareLaunchArgument( 51 | "visualize", 52 | default_value="false", 53 | description="", 54 | choices=["true", "false"], 55 | ), 56 | ] 57 | ) 58 | -------------------------------------------------------------------------------- /ros/launch/offline_node.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | import launch 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 8 | from launch.conditions import IfCondition 9 | from launch.launch_description_sources import ( 10 | get_launch_description_from_python_launch_file, 11 | ) 12 | from launch.substitutions import LaunchConfiguration 13 | from launch_ros.actions import Node 14 | 15 | 16 | def _generate_launch_description(context: launch.LaunchContext, *args, **kwargs): 17 | use_sim_time = True 18 | tf_timeout = 0.0 # tf buffer is always filled before processing any msg 19 | 20 | bag_filename = DeclareLaunchArgument(name="bag_filename", description="") 21 | file = LaunchConfiguration("bag_filename") 22 | output_dir_default = os.path.dirname(file.perform(context=context)) 23 | output_dir = DeclareLaunchArgument( 24 | name="output_dir", default_value=output_dir_default, description="" 25 | ) 26 | common_launch_args = get_launch_description_from_python_launch_file( 27 | get_package_share_directory("kinematic_icp") + "/launch/common_args.launch.py", 28 | ) 29 | 30 | kinematic_icp_offline_node = Node( 31 | package="kinematic_icp", 32 | executable="kinematic_icp_offline_node", 33 | name="offline_node", 34 | namespace="kinematic_icp", 35 | output="screen", 36 | emulate_tty=True, 37 | remappings=[ 38 | ("lidar_odometry", LaunchConfiguration("lidar_odometry_topic")), 39 | ], 40 | parameters=[ 41 | # KISS-ICP configuration 42 | get_package_share_directory("kinematic_icp") 43 | + "/config/kinematic_icp_ros.yaml", 44 | { 45 | # Input topic, is not a remap to marry API with offline node 46 | "lidar_topic": LaunchConfiguration("lidar_topic"), 47 | "use_2d_lidar": LaunchConfiguration("use_2d_lidar"), 48 | # ROS node configuration 49 | "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), 50 | "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), 51 | "base_frame": LaunchConfiguration("base_frame"), 52 | "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), 53 | "invert_odom_tf": LaunchConfiguration("invert_odom_tf"), 54 | "tf_timeout": tf_timeout, 55 | # ROS CLI arguments 56 | "use_sim_time": use_sim_time, 57 | # Offline node specific configuration 58 | "bag_filename": file, 59 | "output_dir": LaunchConfiguration("output_dir"), 60 | "max_num_threads": 0, 61 | }, 62 | ], 63 | ) 64 | 65 | rviz_node = Node( 66 | package="rviz2", 67 | executable="rviz2", 68 | output="screen", 69 | arguments=[ 70 | "-d", 71 | get_package_share_directory("kinematic_icp") + "/rviz/kinematic_icp.rviz", 72 | ], 73 | condition=IfCondition(LaunchConfiguration("visualize")), 74 | ) 75 | return [ 76 | bag_filename, 77 | output_dir, 78 | common_launch_args, 79 | kinematic_icp_offline_node, 80 | rviz_node, 81 | ] 82 | 83 | 84 | def generate_launch_description(): 85 | return LaunchDescription([OpaqueFunction(function=_generate_launch_description)]) 86 | -------------------------------------------------------------------------------- /ros/launch/online_node.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.conditions import IfCondition 6 | from launch.launch_description_sources import ( 7 | get_launch_description_from_python_launch_file, 8 | ) 9 | from launch.substitutions import LaunchConfiguration, PythonExpression 10 | from launch_ros.actions import Node 11 | from launch_ros.substitutions.find_package import get_package_share_directory 12 | 13 | 14 | def generate_launch_description(): 15 | use_sim_time = LaunchConfiguration( 16 | "use_sim_time", default=(os.getenv("SIMULATION") == "true") 17 | ) 18 | tf_timeout = LaunchConfiguration( 19 | "tf_timeout", 20 | default=PythonExpression(["'0.1' if ", use_sim_time, " else '0.0'"]), 21 | ) 22 | common_launch_args = get_launch_description_from_python_launch_file( 23 | get_package_share_directory("kinematic_icp") + "/launch/common_args.launch.py", 24 | ) 25 | 26 | kinematic_icp_online_node = Node( 27 | package="kinematic_icp", 28 | executable="kinematic_icp_online_node", 29 | name="online_node", 30 | namespace="kinematic_icp", 31 | output="screen", 32 | remappings=[ 33 | ("lidar_odometry", LaunchConfiguration("lidar_odometry_topic")), 34 | ], 35 | parameters=[ 36 | # KISS-ICP configuration 37 | get_package_share_directory("kinematic_icp") 38 | + "/config/kinematic_icp_ros.yaml", 39 | { 40 | # Input topic, is not a remap to marry API with offline node 41 | "lidar_topic": LaunchConfiguration("lidar_topic"), 42 | "use_2d_lidar": LaunchConfiguration("use_2d_lidar"), 43 | # ROS node configuration 44 | "lidar_odom_frame": LaunchConfiguration("lidar_odom_frame"), 45 | "wheel_odom_frame": LaunchConfiguration("wheel_odom_frame"), 46 | "base_frame": LaunchConfiguration("base_frame"), 47 | "publish_odom_tf": LaunchConfiguration("publish_odom_tf"), 48 | "invert_odom_tf": LaunchConfiguration("invert_odom_tf"), 49 | "tf_timeout": tf_timeout, 50 | # ROS CLI arguments 51 | "use_sim_time": use_sim_time, 52 | }, 53 | ], 54 | ) 55 | 56 | rviz_node = Node( 57 | package="rviz2", 58 | executable="rviz2", 59 | output="screen", 60 | arguments=[ 61 | "-d", 62 | get_package_share_directory("kinematic_icp") + "/rviz/kinematic_icp.rviz", 63 | ], 64 | condition=IfCondition(LaunchConfiguration("visualize")), 65 | ) 66 | 67 | return LaunchDescription([common_launch_args, kinematic_icp_online_node, rviz_node]) 68 | -------------------------------------------------------------------------------- /ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | kinematic_icp 28 | 0.1.1 29 | ROS 2 Wrapper 30 | frevo137 31 | MIT 32 | 33 | ament_cmake 34 | 35 | geometry_msgs 36 | nav_msgs 37 | rclcpp 38 | rclcpp_components 39 | rcutils 40 | sensor_msgs 41 | laser_geometry 42 | std_msgs 43 | std_srvs 44 | tf2_ros 45 | visualization_msgs 46 | 47 | 48 | rosbag2_cpp 49 | rosbag2_storage 50 | 51 | eigen 52 | sophus 53 | tbb 54 | 55 | ros2launch 56 | 57 | 58 | 59 | ament_cmake 60 | 61 | 62 | -------------------------------------------------------------------------------- /ros/rviz/kinematic_icp.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /point_clouds1 9 | - /point_clouds1/frame_deskew1 10 | - /point_clouds1/kiss_keypoints1 11 | - /point_clouds1/local_map1 12 | - /Odometry1 13 | - /Odometry2 14 | Splitter Ratio: 0.5 15 | Tree Height: 782 16 | - Class: rviz_common/Selection 17 | Name: Selection 18 | - Class: rviz_common/Tool Properties 19 | Expanded: 20 | - /2D Goal Pose1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz_common/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz_common/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: frame_deskew 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Class: rviz_common/Group 38 | Displays: 39 | - Alpha: 0.5 40 | Autocompute Intensity Bounds: true 41 | Autocompute Value Bounds: 42 | Max Value: 13.624558448791504 43 | Min Value: 0.5628786683082581 44 | Value: true 45 | Axis: Z 46 | Channel Name: intensity 47 | Class: rviz_default_plugins/PointCloud2 48 | Color: 255; 255; 0 49 | Color Transformer: AxisColor 50 | Decay Time: 10 51 | Enabled: true 52 | Invert Rainbow: false 53 | Max Color: 255; 255; 255 54 | Max Intensity: 4096 55 | Min Color: 0; 0; 0 56 | Min Intensity: 0 57 | Name: frame_deskew 58 | Position Transformer: XYZ 59 | Selectable: true 60 | Size (Pixels): 1 61 | Size (m): 0.019999999552965164 62 | Style: Points 63 | Topic: 64 | Depth: 5 65 | Durability Policy: Volatile 66 | Filter size: 10 67 | History Policy: Keep Last 68 | Reliability Policy: Best Effort 69 | Value: /kinematic_icp/frame 70 | Use Fixed Frame: true 71 | Use rainbow: true 72 | Value: true 73 | - Alpha: 1 74 | Autocompute Intensity Bounds: true 75 | Autocompute Value Bounds: 76 | Max Value: 10 77 | Min Value: -10 78 | Value: true 79 | Axis: Z 80 | Channel Name: intensity 81 | Class: rviz_default_plugins/PointCloud2 82 | Color: 61; 229; 255 83 | Color Transformer: FlatColor 84 | Decay Time: 0 85 | Enabled: false 86 | Invert Rainbow: false 87 | Max Color: 255; 255; 255 88 | Max Intensity: 4096 89 | Min Color: 0; 0; 0 90 | Min Intensity: 0 91 | Name: kiss_keypoints 92 | Position Transformer: XYZ 93 | Selectable: true 94 | Size (Pixels): 3 95 | Size (m): 0.15000000596046448 96 | Style: Points 97 | Topic: 98 | Depth: 5 99 | Durability Policy: Volatile 100 | Filter size: 10 101 | History Policy: Keep Last 102 | Reliability Policy: Best Effort 103 | Value: /kinematic_icp/keypoints 104 | Use Fixed Frame: true 105 | Use rainbow: true 106 | Value: false 107 | - Alpha: 1 108 | Autocompute Intensity Bounds: true 109 | Autocompute Value Bounds: 110 | Max Value: 94.97420501708984 111 | Min Value: 0.734070897102356 112 | Value: true 113 | Axis: Z 114 | Channel Name: intensity 115 | Class: rviz_default_plugins/PointCloud2 116 | Color: 100; 100; 100 117 | Color Transformer: FlatColor 118 | Decay Time: 0 119 | Enabled: true 120 | Invert Rainbow: false 121 | Max Color: 255; 255; 255 122 | Max Intensity: 4096 123 | Min Color: 0; 0; 0 124 | Min Intensity: 0 125 | Name: local_map 126 | Position Transformer: XYZ 127 | Selectable: true 128 | Size (Pixels): 3 129 | Size (m): 0.05000000074505806 130 | Style: Flat Squares 131 | Topic: 132 | Depth: 5 133 | Durability Policy: Volatile 134 | Filter size: 10 135 | History Policy: Keep Last 136 | Reliability Policy: Best Effort 137 | Value: /kinematic_icp/local_map 138 | Use Fixed Frame: true 139 | Use rainbow: true 140 | Value: true 141 | Enabled: true 142 | Name: point_clouds 143 | - Class: rviz_common/Group 144 | Displays: 145 | - Angle Tolerance: 0.10000000149011612 146 | Class: rviz_default_plugins/Odometry 147 | Covariance: 148 | Orientation: 149 | Alpha: 0.5 150 | Color: 255; 255; 127 151 | Color Style: Unique 152 | Frame: Local 153 | Offset: 1 154 | Scale: 1 155 | Value: true 156 | Position: 157 | Alpha: 0.30000001192092896 158 | Color: 204; 51; 204 159 | Scale: 1 160 | Value: true 161 | Value: false 162 | Enabled: true 163 | Keep: 100 164 | Name: LiDAR Odometry 165 | Position Tolerance: 0.10000000149011612 166 | Shape: 167 | Alpha: 1 168 | Axes Length: 1 169 | Axes Radius: 0.10000000149011612 170 | Color: 255; 25; 0 171 | Head Length: 0.30000001192092896 172 | Head Radius: 0.10000000149011612 173 | Shaft Length: 1 174 | Shaft Radius: 0.05000000074505806 175 | Value: Axes 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | Filter size: 10 180 | History Policy: Keep Last 181 | Reliability Policy: Reliable 182 | Value: /husky_velocity_controller/odom 183 | Value: true 184 | Enabled: true 185 | Name: Odometry 186 | - Class: rviz_default_plugins/Marker 187 | Enabled: false 188 | Name: VoxelHashMap 189 | Namespaces: 190 | {} 191 | Topic: 192 | Depth: 1 193 | Durability Policy: System Default 194 | Filter size: 1 195 | History Policy: Keep Last 196 | Reliability Policy: Best Effort 197 | Value: /kiss_icp/voxel_grid 198 | Value: false 199 | - Angle Tolerance: 0.10000000149011612 200 | Class: rviz_default_plugins/Odometry 201 | Covariance: 202 | Orientation: 203 | Alpha: 0.5 204 | Color: 255; 255; 127 205 | Color Style: Unique 206 | Frame: Local 207 | Offset: 1 208 | Scale: 1 209 | Value: true 210 | Position: 211 | Alpha: 0.30000001192092896 212 | Color: 204; 51; 204 213 | Scale: 1 214 | Value: true 215 | Value: false 216 | Enabled: true 217 | Keep: 100 218 | Name: Odometry 219 | Position Tolerance: 0.10000000149011612 220 | Shape: 221 | Alpha: 1 222 | Axes Length: 1 223 | Axes Radius: 0.10000000149011612 224 | Color: 255; 25; 0 225 | Head Length: 0.30000001192092896 226 | Head Radius: 0.10000000149011612 227 | Shaft Length: 1 228 | Shaft Radius: 0.05000000074505806 229 | Value: Axes 230 | Topic: 231 | Depth: 5 232 | Durability Policy: Volatile 233 | Filter size: 10 234 | History Policy: Keep Last 235 | Reliability Policy: Reliable 236 | Value: /kinematic_icp/lidar_odometry 237 | Value: true 238 | - Class: rviz_default_plugins/TF 239 | Enabled: true 240 | Frame Timeout: 15 241 | Frames: 242 | All Enabled: true 243 | back_depth_base_link: 244 | Value: true 245 | back_depth_imu_frame: 246 | Value: true 247 | back_depth_left_camera_frame: 248 | Value: true 249 | back_depth_left_camera_optical_frame: 250 | Value: true 251 | back_depth_model_origin: 252 | Value: true 253 | back_depth_rgb_camera_frame: 254 | Value: true 255 | back_depth_rgb_camera_optical_frame: 256 | Value: true 257 | back_depth_right_camera_frame: 258 | Value: true 259 | back_depth_right_camera_optical_frame: 260 | Value: true 261 | base_bpearl_back_left_link: 262 | Value: true 263 | base_bpearl_front_right_link: 264 | Value: true 265 | base_cover: 266 | Value: true 267 | base_footprint: 268 | Value: true 269 | base_hokuyo_back_right_link: 270 | Value: true 271 | base_hokuyo_front_left_link: 272 | Value: true 273 | base_link: 274 | Value: true 275 | base_section_1: 276 | Value: true 277 | base_section_2: 278 | Value: true 279 | base_section_3: 280 | Value: true 281 | base_section_4: 282 | Value: true 283 | base_section_5: 284 | Value: true 285 | base_section_6: 286 | Value: true 287 | base_tower: 288 | Value: true 289 | bpearl_back_left_frame: 290 | Value: true 291 | bpearl_front_right_frame: 292 | Value: true 293 | caster_0_link: 294 | Value: true 295 | caster_1_link: 296 | Value: true 297 | caster_2_link: 298 | Value: true 299 | caster_3_link: 300 | Value: true 301 | cover_top: 302 | Value: true 303 | elp_back_base_camera_frame: 304 | Value: true 305 | elp_back_base_camera_link: 306 | Value: true 307 | elp_front_base_camera_frame: 308 | Value: true 309 | elp_front_base_camera_link: 310 | Value: true 311 | front_depth_base_link: 312 | Value: true 313 | front_depth_imu_frame: 314 | Value: true 315 | front_depth_left_camera_frame: 316 | Value: true 317 | front_depth_left_camera_optical_frame: 318 | Value: true 319 | front_depth_model_origin: 320 | Value: true 321 | front_depth_rgb_camera_frame: 322 | Value: true 323 | front_depth_rgb_camera_optical_frame: 324 | Value: true 325 | front_depth_right_camera_frame: 326 | Value: true 327 | front_depth_right_camera_optical_frame: 328 | Value: true 329 | hokuyo_back_right_frame: 330 | Value: true 331 | hokuyo_front_left_frame: 332 | Value: true 333 | left_wheel_link: 334 | Value: true 335 | mobile_coil: 336 | Value: true 337 | odom: 338 | Value: true 339 | odom_lidar: 340 | Value: true 341 | right_wheel_link: 342 | Value: true 343 | robot_top: 344 | Value: true 345 | tower_top: 346 | Value: true 347 | vn100_link: 348 | Value: true 349 | Marker Scale: 1 350 | Name: TF 351 | Show Arrows: true 352 | Show Axes: true 353 | Show Names: false 354 | Tree: 355 | odom: 356 | base_footprint: 357 | back_depth_base_link: 358 | back_depth_imu_frame: 359 | {} 360 | back_depth_left_camera_frame: 361 | back_depth_left_camera_optical_frame: 362 | {} 363 | back_depth_model_origin: 364 | {} 365 | back_depth_rgb_camera_frame: 366 | back_depth_rgb_camera_optical_frame: 367 | {} 368 | back_depth_right_camera_frame: 369 | back_depth_right_camera_optical_frame: 370 | {} 371 | base_bpearl_back_left_link: 372 | bpearl_back_left_frame: 373 | {} 374 | base_bpearl_front_right_link: 375 | bpearl_front_right_frame: 376 | {} 377 | base_hokuyo_back_right_link: 378 | hokuyo_back_right_frame: 379 | {} 380 | base_hokuyo_front_left_link: 381 | hokuyo_front_left_frame: 382 | {} 383 | base_link: 384 | caster_0_link: 385 | {} 386 | caster_1_link: 387 | {} 388 | caster_2_link: 389 | {} 390 | caster_3_link: 391 | {} 392 | left_wheel_link: 393 | {} 394 | right_wheel_link: 395 | {} 396 | base_tower: 397 | base_cover: 398 | cover_top: 399 | {} 400 | base_section_1: 401 | base_section_2: 402 | base_section_3: 403 | base_section_4: 404 | base_section_5: 405 | base_section_6: 406 | {} 407 | tower_top: 408 | {} 409 | elp_back_base_camera_link: 410 | elp_back_base_camera_frame: 411 | {} 412 | elp_front_base_camera_link: 413 | elp_front_base_camera_frame: 414 | {} 415 | front_depth_base_link: 416 | front_depth_imu_frame: 417 | {} 418 | front_depth_left_camera_frame: 419 | front_depth_left_camera_optical_frame: 420 | {} 421 | front_depth_model_origin: 422 | {} 423 | front_depth_rgb_camera_frame: 424 | front_depth_rgb_camera_optical_frame: 425 | {} 426 | front_depth_right_camera_frame: 427 | front_depth_right_camera_optical_frame: 428 | {} 429 | mobile_coil: 430 | {} 431 | odom_lidar: 432 | {} 433 | robot_top: 434 | {} 435 | vn100_link: 436 | {} 437 | Update Interval: 0 438 | Value: true 439 | Enabled: true 440 | Global Options: 441 | Background Color: 0; 0; 0 442 | Fixed Frame: odom_lidar 443 | Frame Rate: 30 444 | Name: root 445 | Tools: 446 | - Class: rviz_default_plugins/Interact 447 | Hide Inactive Objects: true 448 | - Class: rviz_default_plugins/MoveCamera 449 | - Class: rviz_default_plugins/Select 450 | - Class: rviz_default_plugins/FocusCamera 451 | - Class: rviz_default_plugins/Measure 452 | Line color: 128; 128; 0 453 | - Class: rviz_default_plugins/SetInitialPose 454 | Covariance x: 0.25 455 | Covariance y: 0.25 456 | Covariance yaw: 0.06853891909122467 457 | Topic: 458 | Depth: 5 459 | Durability Policy: Volatile 460 | History Policy: Keep Last 461 | Reliability Policy: Reliable 462 | Value: /initialpose 463 | - Class: rviz_default_plugins/SetGoal 464 | Topic: 465 | Depth: 5 466 | Durability Policy: Volatile 467 | History Policy: Keep Last 468 | Reliability Policy: Reliable 469 | Value: /goal_pose 470 | - Class: rviz_default_plugins/PublishPoint 471 | Single click: true 472 | Topic: 473 | Depth: 5 474 | Durability Policy: Volatile 475 | History Policy: Keep Last 476 | Reliability Policy: Reliable 477 | Value: /clicked_point 478 | Transformation: 479 | Current: 480 | Class: rviz_default_plugins/TF 481 | Value: true 482 | Views: 483 | Current: 484 | Class: rviz_default_plugins/ThirdPersonFollower 485 | Distance: 76.9117660522461 486 | Enable Stereo Rendering: 487 | Stereo Eye Separation: 0.05999999865889549 488 | Stereo Focal Distance: 1 489 | Swap Stereo Eyes: false 490 | Value: false 491 | Focal Point: 492 | X: 3.210723876953125 493 | Y: -1.7889020442962646 494 | Z: -3.2766297408670653e-07 495 | Focal Shape Fixed Size: false 496 | Focal Shape Size: 0.05000000074505806 497 | Invert Z Axis: false 498 | Name: Current View 499 | Near Clip Distance: 0.009999999776482582 500 | Pitch: 1.5253978967666626 501 | Target Frame: base_footprint 502 | Value: ThirdPersonFollower (rviz_default_plugins) 503 | Yaw: 4.713582515716553 504 | Saved: ~ 505 | Window Geometry: 506 | Displays: 507 | collapsed: true 508 | Height: 1403 509 | Hide Left Dock: true 510 | Hide Right Dock: false 511 | QMainWindow State: 000000ff00000000fd00000004000000000000048d000004cafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000003d5000000c900fffffffb0000000a005600690065007700730000000418000000ef000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014d0000069ffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000051fc0100000002fb0000000800540069006d0065010000000000000a000000027900fffffffb0000000800540069006d0065010000000000000450000000000000000000000a00000004ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 512 | Selection: 513 | collapsed: false 514 | Time: 515 | collapsed: false 516 | Tool Properties: 517 | collapsed: false 518 | Views: 519 | collapsed: true 520 | Width: 2560 521 | X: 2560 522 | Y: 0 523 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/nodes/offline_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // ROS 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "kinematic_icp_ros/nodes/offline_node.hpp" 39 | #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" 40 | #include "kinematic_icp_ros/utils/RosbagUtils.hpp" 41 | #include "kinematic_icp_ros/utils/indicators.hpp" 42 | 43 | namespace { 44 | std::filesystem::path generateOutputFilename(const std::string &bag_filename) { 45 | size_t last_dot = bag_filename.find_last_of("."); 46 | std::string output_file = bag_filename.substr(0, last_dot); 47 | output_file += "_kinematic_icp_poses_tum.txt"; 48 | std::filesystem::path output_path(output_file); 49 | return output_path.filename(); 50 | } 51 | } // namespace 52 | 53 | namespace kinematic_icp_ros { 54 | 55 | OfflineNode::OfflineNode(const rclcpp::NodeOptions &options) { 56 | node_ = rclcpp::Node::make_shared("kinematic_icp_offline_node", options); 57 | lidar_topic_ = node_->declare_parameter("lidar_topic"); 58 | use_2d_lidar_ = node_->declare_parameter("use_2d_lidar"); 59 | odometry_server_ = std::make_shared(node_); 60 | if (use_2d_lidar_) { 61 | RCLCPP_INFO_STREAM(node_->get_logger(), 62 | "Started in 2D scanner mode with topic: " << lidar_topic_); 63 | } else { 64 | RCLCPP_INFO_STREAM(node_->get_logger(), 65 | "Started in 3D Lidar mode with topic: " << lidar_topic_); 66 | } 67 | 68 | auto bag_filename = node_->declare_parameter("bag_filename"); 69 | const auto poses_filename = generateOutputFilename(bag_filename); 70 | output_pose_file_ = std::filesystem::path(node_->declare_parameter("output_dir")); 71 | output_pose_file_ /= poses_filename; 72 | auto tf_bridge = std::make_shared(node_); 73 | bag_multiplexer_.AddBag(BufferableBag(bag_filename, tf_bridge, lidar_topic_)); 74 | } 75 | 76 | void OfflineNode::writePosesInTumFormat() { 77 | std::ofstream file(output_pose_file_); 78 | if (!file.is_open()) { 79 | std::cerr << "Error opening file: " << output_pose_file_ << std::endl; 80 | return; 81 | } 82 | 83 | RCLCPP_INFO_STREAM(node_->get_logger(), "Saving poses in TUM format in " << output_pose_file_); 84 | 85 | // Iterate over the poses and timestamps 86 | for (const auto &[timestamp, pose] : poses_with_timestamps_) { 87 | const Eigen::Vector3d &translation = pose.translation(); 88 | const Eigen::Quaterniond &quaternion = pose.so3().unit_quaternion(); 89 | // Write the timestamp, position, and quaternion to the file 90 | file << std::fixed << std::setprecision(6) << timestamp << " " << translation.x() << " " 91 | << translation.y() << " " << translation.z() << " " << quaternion.x() << " " 92 | << quaternion.y() << " " << quaternion.z() << " " << quaternion.w() << "\n"; 93 | } 94 | 95 | // Close the file 96 | file.close(); 97 | } 98 | 99 | void OfflineNode::Run() { 100 | indicators::ProgressBar bar{indicators::option::BarWidth{100}, 101 | indicators::option::Start{"["}, 102 | indicators::option::Fill{"="}, 103 | indicators::option::Lead{">"}, 104 | indicators::option::Remainder{" "}, 105 | indicators::option::End{"]"}, 106 | indicators::option::ForegroundColor{indicators::Color::cyan}, 107 | indicators::option::ShowPercentage{true}, 108 | indicators::option::ShowElapsedTime{true}, 109 | indicators::option::ShowRemainingTime{true}, 110 | indicators::option::Stream{std::cout}, 111 | indicators::option::MaxProgress{bag_multiplexer_.message_count()}}; 112 | 113 | auto lidar2d_to_3d = [this](const sensor_msgs::msg::LaserScan::SharedPtr &msg) { 114 | auto projected_scan = std::make_shared(); 115 | laser_projector_.projectLaser(*msg, *projected_scan, -1.0, 116 | laser_geometry::channel_option::Timestamp); 117 | return projected_scan; 118 | }; 119 | // Deserialize the next pointcloud message from the bagfiles 120 | auto GetNextMsg = [&] { 121 | if (use_2d_lidar_) { 122 | const rclcpp::Serialization serializer; 123 | const auto lidar_msg = std::make_shared(); 124 | const auto &msg = bag_multiplexer_.GetNextMessage(); 125 | rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); 126 | serializer.deserialize_message(&serialized_msg, lidar_msg.get()); 127 | return lidar2d_to_3d(lidar_msg); 128 | } else { 129 | const rclcpp::Serialization serializer; 130 | const auto lidar_msg = std::make_shared(); 131 | const auto &msg = bag_multiplexer_.GetNextMessage(); 132 | rclcpp::SerializedMessage serialized_msg(*msg.serialized_data); 133 | serializer.deserialize_message(&serialized_msg, lidar_msg.get()); 134 | return lidar_msg; 135 | } 136 | }; 137 | 138 | // This is the main blocking loop that this simulates the subscription form the online node, 139 | // but instead of fetching the data from a topic, it does it from the provided bagfiles 140 | while (rclcpp::ok() && bag_multiplexer_.IsMessageAvailable()) { 141 | const auto msg = GetNextMsg(); 142 | odometry_server_->RegisterFrame(msg); 143 | const auto &stamp = odometry_server_->timestamps_handler_.last_processed_stamp_; 144 | poses_with_timestamps_.emplace_back(stamp.sec + stamp.nanosec * 1e-9, 145 | odometry_server_->kinematic_icp_->pose()); 146 | bar.tick(); 147 | } 148 | bar.mark_as_completed(); 149 | } 150 | 151 | } // namespace kinematic_icp_ros 152 | 153 | int main(int argc, char **argv) { 154 | rclcpp::init(argc, argv); 155 | auto offline_node = kinematic_icp_ros::OfflineNode(rclcpp::NodeOptions()); 156 | offline_node.Run(); 157 | offline_node.writePosesInTumFormat(); 158 | rclcpp::shutdown(); 159 | return 0; 160 | } 161 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/nodes/online_node.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include 24 | #include 25 | #include 26 | 27 | // ROS 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "kinematic_icp_ros/nodes/online_node.hpp" 36 | #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" 37 | 38 | namespace kinematic_icp_ros { 39 | 40 | OnlineNode ::OnlineNode(const rclcpp::NodeOptions &options) { 41 | node_ = rclcpp::Node::make_shared("kinematic_icp_online_node", options); 42 | lidar_topic_ = node_->declare_parameter("lidar_topic"); 43 | odometry_server_ = std::make_shared(node_); 44 | const bool use_2d_lidar = node_->declare_parameter("use_2d_lidar"); 45 | if (use_2d_lidar) { 46 | RCLCPP_INFO_STREAM(node_->get_logger(), 47 | "Started in 2D scanner mode with topic: " << lidar_topic_); 48 | laser_scan_sub_ = node_->create_subscription( 49 | lidar_topic_, rclcpp::SystemDefaultsQoS(), 50 | [&](const sensor_msgs::msg::LaserScan::ConstSharedPtr &msg) { 51 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr lidar_msg = [&]() { 52 | auto projected_scan = std::make_shared(); 53 | laser_projector_.projectLaser(*msg, *projected_scan, -1.0, 54 | laser_geometry::channel_option::Timestamp); 55 | return projected_scan; 56 | }(); 57 | odometry_server_->RegisterFrame(lidar_msg); 58 | }); 59 | } else { 60 | RCLCPP_INFO_STREAM(node_->get_logger(), 61 | "Started in 3D Lidar mode with topic: " << lidar_topic_); 62 | pointcloud_sub_ = node_->create_subscription( 63 | lidar_topic_, rclcpp::SystemDefaultsQoS(), 64 | [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { 65 | odometry_server_->RegisterFrame(msg); 66 | }); 67 | } 68 | } 69 | 70 | rclcpp::node_interfaces::NodeBaseInterface::SharedPtr OnlineNode::get_node_base_interface() { 71 | return node_->get_node_base_interface(); 72 | } 73 | 74 | } // namespace kinematic_icp_ros 75 | 76 | #include "rclcpp_components/register_node_macro.hpp" 77 | RCLCPP_COMPONENTS_REGISTER_NODE(kinematic_icp_ros::OnlineNode) 78 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "kinematic_icp_ros/server/LidarOdometryServer.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "kinematic_icp/pipeline/KinematicICP.hpp" 34 | #include "kinematic_icp_ros/utils/RosUtils.hpp" 35 | 36 | // ROS 2 headers 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | namespace { 53 | // Time Type aliases to simplify code 54 | using milliseconds = std::chrono::milliseconds; 55 | using seconds = std::chrono::duration; 56 | using std::chrono::duration_cast; 57 | } // namespace 58 | 59 | namespace kinematic_icp_ros { 60 | 61 | using namespace utils; 62 | 63 | LidarOdometryServer::LidarOdometryServer(rclcpp::Node::SharedPtr node) : node_(node) { 64 | lidar_odom_frame_ = node->declare_parameter("lidar_odom_frame", lidar_odom_frame_); 65 | wheel_odom_frame_ = node->declare_parameter("wheel_odom_frame", wheel_odom_frame_); 66 | base_frame_ = node->declare_parameter("base_frame", base_frame_); 67 | publish_odom_tf_ = node->declare_parameter("publish_odom_tf", publish_odom_tf_); 68 | invert_odom_tf_ = node->declare_parameter("invert_odom_tf", invert_odom_tf_); 69 | tf_timeout_ = 70 | duration_cast(seconds(node->declare_parameter("tf_timeout", 0.0))); 71 | 72 | kinematic_icp::pipeline::Config config; 73 | // Preprocessing 74 | config.max_range = node->declare_parameter("max_range", config.max_range); 75 | config.min_range = node->declare_parameter("min_range", config.min_range); 76 | // Mapping parameters 77 | config.voxel_size = node->declare_parameter("voxel_size", config.voxel_size); 78 | config.max_points_per_voxel = 79 | node->declare_parameter("max_points_per_voxel", config.max_points_per_voxel); 80 | // Correspondence threshold parameters 81 | config.use_adaptive_threshold = 82 | node->declare_parameter("use_adaptive_threshold", config.use_adaptive_threshold); 83 | config.fixed_threshold = 84 | node->declare_parameter("fixed_threshold", config.fixed_threshold); 85 | // Registration Parameters 86 | config.max_num_iterations = 87 | node->declare_parameter("max_num_iterations", config.max_num_iterations); 88 | config.convergence_criterion = 89 | node->declare_parameter("convergence_criterion", config.convergence_criterion); 90 | config.max_num_threads = 91 | node->declare_parameter("max_num_threads", config.max_num_threads); 92 | config.use_adaptive_odometry_regularization = node->declare_parameter( 93 | "use_adaptive_odometry_regularization", config.use_adaptive_odometry_regularization); 94 | config.fixed_regularization = 95 | node->declare_parameter("fixed_regularization", config.fixed_regularization); 96 | // Motion compensation 97 | config.deskew = node->declare_parameter("deskew", config.deskew); 98 | if (config.max_range < config.min_range) { 99 | RCLCPP_WARN(node_->get_logger(), 100 | "[WARNING] max_range is smaller than min_range, settng min_range to 0.0"); 101 | config.min_range = 0.0; 102 | } 103 | 104 | // Construct the main KISS-ICP odometry node 105 | kinematic_icp_ = std::make_unique(config); 106 | 107 | // Initialize publishers 108 | rclcpp::QoS qos((rclcpp::SystemDefaultsQoS().keep_last(1).durability_volatile())); 109 | odom_publisher_ = node_->create_publisher("lidar_odometry", qos); 110 | frame_publisher_ = node_->create_publisher("frame", qos); 111 | kpoints_publisher_ = node_->create_publisher("keypoints", qos); 112 | map_publisher_ = node_->create_publisher("local_map", qos); 113 | voxel_grid_pub_ = node_->create_publisher("voxel_grid", qos); 114 | 115 | set_pose_srv_ = node_->create_service( 116 | "set_pose", [&](const std::shared_ptr request, 117 | std::shared_ptr response) { 118 | const auto pose = LookupTransform(wheel_odom_frame_, base_frame_, tf2_buffer_); 119 | RCLCPP_WARN_STREAM(node_->get_logger(), "Resetting KISS-ICP pose:\n" 120 | << pose.matrix() << "\n"); 121 | kinematic_icp_->SetPose(pose); 122 | response->success = true; 123 | }); 124 | 125 | // Initialize the transform broadcaster 126 | tf_broadcaster_ = std::make_unique(node_); 127 | tf2_buffer_ = std::make_unique(node_->get_clock()); 128 | tf2_listener_ = std::make_unique(*tf2_buffer_); 129 | 130 | // Initialize the odometry and tf msg. Since tf by design does not allow for having two frames 131 | // in the tree with a different parent, to publish the odom_lidar frame, we need to add a child 132 | // frame (we can't provide odom_lidar -> base_footprint, which is what we estimate internally). 133 | // Therefore we publish the inverted transformation (base_footprint -> odom_lidar), which turns 134 | // to be an implemantation detail as tf lookups are transparent and one can query for the 135 | // odom_lidar -> base_footprint transform, the tf will invert it again on the fly. 136 | if (invert_odom_tf_) { 137 | tf_msg_.header.frame_id = base_frame_; 138 | tf_msg_.child_frame_id = lidar_odom_frame_; 139 | } else { 140 | tf_msg_.header.frame_id = lidar_odom_frame_; 141 | tf_msg_.child_frame_id = base_frame_; 142 | } 143 | 144 | // fixed covariancecovariance 145 | const auto position_covariance = node->declare_parameter("position_covariance", 0.1); 146 | const auto orientation_covariance = 147 | node->declare_parameter("orientation_covariance", 0.1); 148 | odom_msg_.header.frame_id = lidar_odom_frame_; 149 | odom_msg_.child_frame_id = base_frame_; 150 | odom_msg_.pose.covariance.fill(0.0); 151 | odom_msg_.pose.covariance[0] = position_covariance; 152 | odom_msg_.pose.covariance[7] = position_covariance; 153 | odom_msg_.pose.covariance[35] = orientation_covariance; 154 | odom_msg_.twist.covariance.fill(0); 155 | odom_msg_.twist.covariance[0] = position_covariance; 156 | odom_msg_.twist.covariance[7] = position_covariance; 157 | odom_msg_.twist.covariance[35] = orientation_covariance; 158 | } 159 | 160 | void LidarOdometryServer::InitializePoseAndExtrinsic( 161 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { 162 | if (!tf2_buffer_->_frameExists(wheel_odom_frame_) || !tf2_buffer_->_frameExists(base_frame_) || 163 | !tf2_buffer_->_frameExists(msg->header.frame_id)) { 164 | return; 165 | } 166 | 167 | // Independent from the service, start the ROS node from a known state 168 | const auto pose = LookupTransform(wheel_odom_frame_, base_frame_, tf2_buffer_); 169 | RCLCPP_INFO_STREAM(node_->get_logger(), "Resetting KISS-ICP pose:\n" << pose.matrix() << "\n"); 170 | kinematic_icp_->SetPose(pose); 171 | 172 | try { 173 | sensor_to_base_footprint_ = tf2::transformToSophus( 174 | tf2_buffer_->lookupTransform(base_frame_, msg->header.frame_id, tf2::TimePointZero)); 175 | } catch (tf2::TransformException &ex) { 176 | RCLCPP_ERROR(node_->get_logger(), "%s", ex.what()); 177 | return; 178 | } 179 | 180 | // Initialization finished 181 | RCLCPP_INFO(node_->get_logger(), "KISS-ICP ROS 2 odometry node initialized"); 182 | timestamps_handler_.last_processed_stamp_ = msg->header.stamp; 183 | initialize_odom_node = true; 184 | } 185 | 186 | void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) { 187 | if (!initialize_odom_node) { 188 | InitializePoseAndExtrinsic(msg); 189 | } 190 | 191 | // Buffer the last state This will be used for computing the veloicty 192 | const auto last_pose = kinematic_icp_->pose(); 193 | const auto &[begin_odom_query, end_odom_query, timestamps] = 194 | timestamps_handler_.ProcessTimestamps(msg); 195 | 196 | // Get the initial guess from the wheel odometry 197 | const auto delta = 198 | LookupDeltaTransform(base_frame_, begin_odom_query, base_frame_, end_odom_query, 199 | wheel_odom_frame_, tf_timeout_, tf2_buffer_); 200 | 201 | // Run kinematic ICP 202 | if (delta.log().norm() > 1e-3) { 203 | const auto &extrinsic = sensor_to_base_footprint_; 204 | const auto points = PointCloud2ToEigen(msg, {}); 205 | const auto &[frame, kpoints] = 206 | kinematic_icp_->RegisterFrame(points, timestamps, extrinsic, delta); 207 | PublishClouds(frame, kpoints); 208 | } 209 | 210 | // Compute velocities, use the elapsed time between the current msg and the last received 211 | const double elapsed_time = 212 | timestamps_handler_.toTime(end_odom_query) - timestamps_handler_.toTime(begin_odom_query); 213 | const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log(); 214 | const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time; 215 | 216 | // Spit the current estimated pose to ROS pc_out_msgs handling the desired target frame 217 | PublishOdometryMsg(kinematic_icp_->pose(), velocity); 218 | } 219 | 220 | void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose, 221 | const Sophus::SE3d::Tangent &velocity) { 222 | // Broadcast over the tf tree 223 | if (publish_odom_tf_) { 224 | tf_msg_.transform = [&]() { 225 | if (invert_odom_tf_) return tf2::sophusToTransform(pose.inverse()); 226 | return tf2::sophusToTransform(pose); 227 | }(); 228 | tf_msg_.header.stamp = timestamps_handler_.last_processed_stamp_; 229 | tf_broadcaster_->sendTransform(tf_msg_); 230 | } 231 | 232 | // publish odometry msg 233 | odom_msg_.pose.pose = tf2::sophusToPose(pose); 234 | odom_msg_.twist.twist.linear.x = velocity[0]; 235 | odom_msg_.twist.twist.angular.z = velocity[5]; 236 | odom_msg_.header.stamp = timestamps_handler_.last_processed_stamp_; 237 | odom_publisher_->publish(odom_msg_); 238 | } 239 | 240 | void LidarOdometryServer::PublishClouds(const std::vector frame, 241 | const std::vector keypoints) { 242 | // For re-publishing the input frame and keypoints, we do it in the LiDAR coordinate frames 243 | std_msgs::msg::Header lidar_header; 244 | lidar_header.frame_id = base_frame_; 245 | lidar_header.stamp = timestamps_handler_.last_processed_stamp_; 246 | 247 | // The internal map representation is in the lidar_odom_frame_ 248 | std_msgs::msg::Header map_header; 249 | map_header.frame_id = lidar_odom_frame_; 250 | map_header.stamp = timestamps_handler_.last_processed_stamp_; 251 | 252 | // Check for subscriptions before publishing to avoid unnecesary CPU usage 253 | if (frame_publisher_->get_subscription_count() > 0) { 254 | frame_publisher_->publish(std::move(EigenToPointCloud2(frame, lidar_header))); 255 | } 256 | if (kpoints_publisher_->get_subscription_count() > 0) { 257 | kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, lidar_header))); 258 | } 259 | if (map_publisher_->get_subscription_count() > 0) { 260 | map_publisher_->publish( 261 | std::move(EigenToPointCloud2(kinematic_icp_->LocalMap(), map_header))); 262 | } 263 | } 264 | 265 | } // namespace kinematic_icp_ros 266 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/utils/RosUtils.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "kinematic_icp_ros/utils/RosUtils.hpp" 24 | 25 | #include 26 | #include 27 | 28 | namespace kinematic_icp_ros::utils { 29 | 30 | std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, 31 | const Sophus::SE3d &T) { 32 | std::vector points; 33 | points.reserve(msg->height * msg->width); 34 | sensor_msgs::PointCloud2ConstIterator iter_xyz(*msg, "x"); 35 | for (size_t i = 0; i < msg->height * msg->width; ++i, ++iter_xyz) { 36 | points.emplace_back(T * Eigen::Vector3d{iter_xyz[0], iter_xyz[1], iter_xyz[2]}); 37 | } 38 | return points; 39 | } 40 | 41 | std::unique_ptr EigenToPointCloud2(const std::vector &points, 42 | const Header &header) { 43 | auto msg = std::make_unique(); 44 | msg->header = header; 45 | 46 | auto &offset = msg->point_step; 47 | offset = addPointField(*msg, "x", 1, PointField::FLOAT32, offset); 48 | offset = addPointField(*msg, "y", 1, PointField::FLOAT32, offset); 49 | offset = addPointField(*msg, "z", 1, PointField::FLOAT32, offset); 50 | 51 | // Resize the 52 | sensor_msgs::PointCloud2Modifier modifier(*msg); 53 | modifier.resize(points.size()); 54 | 55 | sensor_msgs::PointCloud2Iterator iter_xyz(*msg, "x"); 56 | for (size_t i = 0; i < points.size(); i++, ++iter_xyz) { 57 | const Eigen::Vector3d &point = points[i]; 58 | iter_xyz[0] = point.x(); 59 | iter_xyz[1] = point.y(); 60 | iter_xyz[2] = point.z(); 61 | } 62 | return msg; 63 | } 64 | 65 | visualization_msgs::msg::Marker VoxelsToMarker(const std::vector &voxels, 66 | const double voxel_size, 67 | const Header &header) { 68 | visualization_msgs::msg::Marker voxel_grid; 69 | voxel_grid.header = header; 70 | voxel_grid.ns = "voxel_grid"; 71 | voxel_grid.id = 0; // Always same voxel grid, this should modify the whole structure 72 | voxel_grid.type = visualization_msgs::msg::Marker::LINE_LIST; 73 | voxel_grid.action = visualization_msgs::msg::Marker::MODIFY; 74 | 75 | // TODO(God): Convert this marker into a custom vizualization type so we can modify these 5 76 | // parameters from rviz instead of the ROS node! 77 | voxel_grid.scale.x = 0.01; // Line width 78 | voxel_grid.color.a = 0.1; // alpha 79 | voxel_grid.color.r = 1.0; // red 80 | voxel_grid.color.g = 1.0; // green 81 | voxel_grid.color.b = 1.0; // blue 82 | 83 | // Compute the 8 vertices of each voxel in grid coordinates 84 | const auto corners = [&voxels]() { 85 | std::vector> voxel_corners; 86 | voxel_corners.reserve(voxels.size()); 87 | for (const auto &voxel : voxels) { 88 | // clang-format off 89 | voxel_corners.emplace_back(std::array{ 90 | voxel + Eigen::Vector3i{0, 0, 0}, 91 | voxel + Eigen::Vector3i{1, 0, 0}, 92 | voxel + Eigen::Vector3i{1, 1, 0}, 93 | voxel + Eigen::Vector3i{0, 1, 0}, 94 | voxel + Eigen::Vector3i{0, 0, 1}, 95 | voxel + Eigen::Vector3i{1, 0, 1}, 96 | voxel + Eigen::Vector3i{1, 1, 1}, 97 | voxel + Eigen::Vector3i{0, 1, 1} 98 | }); 99 | // clang-format on 100 | } 101 | return voxel_corners; 102 | }(); 103 | 104 | // Lambda function to add an edge between two voxel vertices 105 | auto AddEdge = [&](const Eigen::Vector3i &vertex1, const Eigen::Vector3i &vertex2) { 106 | geometry_msgs::msg::Point point1, point2; 107 | point1.x = vertex1.x() * voxel_size; 108 | point1.y = vertex1.y() * voxel_size; 109 | point1.z = vertex1.z() * voxel_size; 110 | voxel_grid.points.push_back(point1); 111 | 112 | point2.x = vertex2.x() * voxel_size; 113 | point2.y = vertex2.y() * voxel_size; 114 | point2.z = vertex2.z() * voxel_size; 115 | voxel_grid.points.push_back(point2); 116 | }; 117 | 118 | // Add edges for each voxel to create the line list 119 | for (const auto &corner : corners) { 120 | AddEdge(corner[0], corner[1]); 121 | AddEdge(corner[1], corner[2]); 122 | AddEdge(corner[2], corner[3]); 123 | AddEdge(corner[3], corner[0]); 124 | AddEdge(corner[4], corner[5]); 125 | AddEdge(corner[5], corner[6]); 126 | AddEdge(corner[6], corner[7]); 127 | AddEdge(corner[7], corner[4]); 128 | AddEdge(corner[0], corner[4]); 129 | AddEdge(corner[1], corner[5]); 130 | AddEdge(corner[2], corner[6]); 131 | AddEdge(corner[3], corner[7]); 132 | } 133 | 134 | return voxel_grid; 135 | } 136 | } // namespace kinematic_icp_ros::utils 137 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/utils/RosbagUtils.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "kinematic_icp_ros/utils/RosbagUtils.hpp" 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // ROS 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace { 41 | auto GetTimestampsFromRosbagSerializedMsg(const rosbag2_storage::SerializedBagMessage &msg) { 42 | #if RCLCPP_VERSION_GTE(22, 0, 0) 43 | return std::chrono::nanoseconds(msg.send_timestamp); 44 | #else 45 | return std::chrono::nanoseconds(msg.time_stamp); 46 | #endif 47 | } 48 | } // namespace 49 | 50 | /// TFBridge---------------------------------------------------------------------------------------- 51 | BufferableBag::TFBridge::TFBridge(rclcpp::Node::SharedPtr node) { 52 | tf_broadcaster = std::make_unique(node); 53 | tf_static_broadcaster = std::make_unique(node); 54 | serializer = rclcpp::Serialization(); 55 | } 56 | 57 | void BufferableBag::TFBridge::ProcessTFMessage( 58 | std::shared_ptr msg) const { 59 | tf2_msgs::msg::TFMessage tf_message; 60 | rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); 61 | serializer.deserialize_message(&serialized_msg, &tf_message); 62 | // Broadcast tranforms to /tf and /tf_static topics 63 | for (auto &transform : tf_message.transforms) { 64 | if (msg->topic_name == "/tf_static") { 65 | tf_static_broadcaster->sendTransform(transform); 66 | } else { 67 | tf_broadcaster->sendTransform(transform); 68 | } 69 | } 70 | } 71 | 72 | /// BufferableBag----------------------------------------------------------------------------------- 73 | BufferableBag::BufferableBag(const std::string &bag_filename, 74 | const std::shared_ptr tf_bridge, 75 | const std::string &topic, 76 | const std::chrono::seconds buffer_size) 77 | : tf_bridge_(tf_bridge), 78 | bag_reader_(std::make_unique()), 79 | topic_(topic), 80 | buffer_size_(buffer_size) { 81 | bag_reader_->open(bag_filename); 82 | message_count_ = [&]() { 83 | std::size_t message_count = 0; 84 | const auto &metadata = bag_reader_->get_metadata(); 85 | const auto topic_info = metadata.topics_with_message_count; 86 | const auto it = std::find_if(topic_info.begin(), topic_info.end(), [&](const auto &info) { 87 | return info.topic_metadata.name == topic_; 88 | }); 89 | if (it != topic_info.end()) { 90 | message_count += it->message_count; 91 | } 92 | return message_count; 93 | }(); 94 | 95 | BufferMessages(); 96 | } 97 | 98 | bool BufferableBag::finished() const { return !bag_reader_->has_next(); }; 99 | 100 | std::size_t BufferableBag::message_count() const { return message_count_; } 101 | 102 | void BufferableBag::BufferMessages() { 103 | auto buffer_is_filled = [&]() -> bool { 104 | if (buffer_.empty()) return false; 105 | const auto first_stamp = GetTimestampsFromRosbagSerializedMsg(buffer_.front()); 106 | const auto last_stamp = GetTimestampsFromRosbagSerializedMsg(buffer_.back()); 107 | return (last_stamp - first_stamp) > buffer_size_; 108 | }; 109 | 110 | // Advance reading one message until the buffer is filled or we finish the bagfile 111 | while (!finished() && !buffer_is_filled()) { 112 | // Fetch next message from bagfile, could be anything 113 | const auto msg = bag_reader_->read_next(); 114 | // TODO(Nacho): The following logic should be customizable from the outside world 115 | // If the msg is TFMessage, fill the tf_buffer and broadcast the transformation and don't 116 | // populate the buffered_messages_ as we already processed it 117 | if (msg->topic_name == "/tf" || msg->topic_name == "/tf_static") { 118 | tf_bridge_->ProcessTFMessage(msg); 119 | } else if (msg->topic_name == topic_) { 120 | // If the msg is not TFMessage then push it to the interal buffer of all messages 121 | buffer_.push(*msg); 122 | } 123 | } 124 | } 125 | 126 | rosbag2_storage::SerializedBagMessage BufferableBag::PopNextMessage() { 127 | const rosbag2_storage::SerializedBagMessage msg = buffer_.front(); 128 | buffer_.pop(); 129 | BufferMessages(); 130 | return msg; 131 | } 132 | 133 | /// Multiplexer ------------------------------------------------------------------------------------ 134 | void BagMultiplexer::AddBag(BufferableBag &&bag) { 135 | message_count_ += bag.message_count(); 136 | bags_.push_back(std::move(bag)); 137 | } 138 | 139 | std::size_t BagMultiplexer::message_count() const { return message_count_; }; 140 | 141 | bool BagMultiplexer::IsMessageAvailable() const { return !bags_[current_index_].finished(); } 142 | 143 | rosbag2_storage::SerializedBagMessage BagMultiplexer::GetNextMessage() { 144 | if (bags_[current_index_].finished() && current_index_ + 1 < bags_.size()) { 145 | current_index_++; 146 | } 147 | return bags_[current_index_].PopNextMessage(); 148 | } 149 | -------------------------------------------------------------------------------- /ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp: -------------------------------------------------------------------------------- 1 | // MIT License 2 | 3 | // Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill 4 | // Stachniss. 5 | 6 | // Permission is hereby granted, free of charge, to any person obtaining a copy 7 | // of this software and associated documentation files (the "Software"), to deal 8 | // in the Software without restriction, including without limitation the rights 9 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | // copies of the Software, and to permit persons to whom the Software is 11 | // furnished to do so, subject to the following conditions: 12 | 13 | // The above copyright notice and this permission notice shall be included in all 14 | // copies or substantial portions of the Software. 15 | 16 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | // SOFTWARE. 23 | #include "kinematic_icp_ros/utils/TimeStampHandler.hpp" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace { 37 | 38 | using PointCloud2 = sensor_msgs::msg::PointCloud2; 39 | using PointField = sensor_msgs::msg::PointField; 40 | using Header = std_msgs::msg::Header; 41 | 42 | std::optional GetTimestampField(const PointCloud2::ConstSharedPtr msg) { 43 | PointField timestamp_field; 44 | for (const auto &field : msg->fields) { 45 | if ((field.name == "t" || field.name == "timestamp" || field.name == "time" || 46 | field.name == "stamps")) { 47 | timestamp_field = field; 48 | } 49 | } 50 | if (timestamp_field.count) return timestamp_field; 51 | RCLCPP_WARN_ONCE(rclcpp::get_logger("kinematic_icp_ros"), 52 | "Field 't', 'timestamp', 'time', or 'stamps' does not exist. " 53 | "Disabling scan deskewing"); 54 | return {}; 55 | } 56 | 57 | auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, 58 | const PointField ×tamp_field) { 59 | auto number_of_digits_decimal_part = [](const auto &stamp) { 60 | const uint64_t number_of_seconds = static_cast(std::round(stamp)); 61 | return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1; 62 | }; 63 | auto extract_timestamps = 64 | [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { 65 | const size_t n_points = msg->height * msg->width; 66 | std::vector timestamps; 67 | timestamps.reserve(n_points); 68 | for (size_t i = 0; i < n_points; ++i, ++it) { 69 | double stampd = static_cast(*it); 70 | // If the number of digits is greater than 10 (which is the maximum number of digits 71 | // that can be represented with a 32 bits integer), the stamp is in nanoseconds instead 72 | // of seconds, perform conversion 73 | if (number_of_digits_decimal_part(stampd) > 10) { 74 | stampd *= 1e-9; 75 | } 76 | timestamps.emplace_back(stampd); 77 | } 78 | return timestamps; 79 | }; 80 | 81 | // According to the type of the timestamp == type, return a PointCloud2ConstIterator 82 | using sensor_msgs::PointCloud2ConstIterator; 83 | if (timestamp_field.datatype == PointField::UINT32) { 84 | return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); 85 | } else if (timestamp_field.datatype == PointField::FLOAT32) { 86 | return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); 87 | } else if (timestamp_field.datatype == PointField::FLOAT64) { 88 | return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); 89 | } 90 | 91 | // timestamp type not supported, please open an issue :) 92 | throw std::runtime_error("timestamp field type not supported"); 93 | } 94 | 95 | std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { 96 | auto timestamp_field = GetTimestampField(msg); 97 | if (!timestamp_field.has_value()) return {}; 98 | 99 | // Extract timestamps from cloud_msg 100 | std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); 101 | 102 | return timestamps; 103 | } 104 | } // namespace 105 | 106 | namespace kinematic_icp_ros::utils { 107 | 108 | std::tuple> TimeStampHandler::ProcessTimestamps( 109 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { 110 | std::vector timestamps = GetTimestamps(msg); 111 | const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); 112 | const StampType msg_stamp = msg->header.stamp; 113 | const StampType begin_stamp = last_processed_stamp_; 114 | StampType end_stamp = msg_stamp; 115 | if (max_it != timestamps.cend()) { 116 | const double &max_stamp_in_seconds = *max_it; 117 | const double &min_stamp_in_seconds = *min_it; 118 | const double msg_stamp_in_seconds = this->toTime(msg_stamp); 119 | 120 | // Check if stamping happens and the beginning or the end of scan 121 | const bool is_stamped_at_the_beginning = 122 | std::abs(msg_stamp_in_seconds - max_stamp_in_seconds) > 1e-8; 123 | if (is_stamped_at_the_beginning) { 124 | // begin-stamping -> add scan duration to the stamp 125 | const auto scan_duration = 126 | tf2::durationFromSec(max_stamp_in_seconds - min_stamp_in_seconds); 127 | end_stamp = StampType(rclcpp::Time(end_stamp) + scan_duration); 128 | } 129 | 130 | // Normalize timestamps 131 | std::transform(timestamps.cbegin(), timestamps.cend(), timestamps.begin(), 132 | [&](const auto ×tamp) { 133 | return (timestamp - min_stamp_in_seconds) / 134 | (max_stamp_in_seconds - min_stamp_in_seconds); 135 | }); 136 | } 137 | last_processed_stamp_ = end_stamp; 138 | return std::make_tuple(begin_stamp, end_stamp, timestamps); 139 | } 140 | 141 | } // namespace kinematic_icp_ros::utils 142 | --------------------------------------------------------------------------------