Open source release

.clang-format

@@ -0,0 +1,10 @@
+BasedOnStyle: Google
+UseTab: Never
+IndentWidth: 4
+AccessModifierOffset: -4
+ColumnLimit: 100
+BinPackParameters: false
+SortIncludes: true
+Standard: c++17
+DerivePointerAlignment: false
+PointerAlignment: Right

.cmake-format.yaml

@@ -0,0 +1,4 @@
+enable_markup: false
+line_width: 120
+format:
+  max_subgroups_hwrap: 5

.github/CODEOWNERS

@@ -0,0 +1 @@
+* @nachovizzo @benemer @tizianoGuadagnino

.github/workflows/cpp.yml

@@ -0,0 +1,56 @@
+name: C++ API
+
+on:
+  push:
+    branches: ["main"]
+  pull_request:
+    branches: ["main"]
+
+env:
+  BUILD_TYPE: Release
+
+jobs:
+  cpp_api:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+    steps:
+      - uses: actions/checkout@v3
+      - name: Setup cmake
+        uses: jwlawson/[email protected]
+        with:
+          cmake-version: "3.25.x"
+      - name: Configure CMake
+        run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp
+      - name: Build
+        run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+  # As the previous job will always install the dependencies from cmake, and this is guaranteed to
+  # work, we also want to support dev sandboxes where the main dependencies are already
+  # pre-installed in the system. For now, we only support dev machines under a GNU/Linux
+  # environmnets. If you are reading this and need the same functionallity in Windows/macOS please
+  # open a ticket.
+  cpp_api_dev:
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+
+    steps:
+      - uses: actions/checkout@v3
+      - name: Cache dependencies
+        uses: actions/cache@v2
+        with:
+          path: ~/.apt/cache
+          key: ${{ runner.os }}-apt-${{ hashFiles('**/ubuntu_dependencies.yml') }}
+          restore-keys: |
+            ${{ runner.os }}-apt-
+      - name: Install dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y build-essential cmake git libeigen3-dev libtbb-dev
+      - name: Configure CMake
+        run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{github.workspace}}/cpp/kinematic_icp
+      - name: Build
+        run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}

.github/workflows/pre-commit.yml

@@ -0,0 +1,15 @@
+name: Style Check
+
+on:
+  push:
+    branches: ["main"]
+  pull_request:
+    branches: ["main"]
+
+jobs:
+  pre-commit:
+    name: Pre-commit checks
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - uses: pre-commit/[email protected]

.github/workflows/ros.yml

@@ -0,0 +1,24 @@
+name: ROS nodes
+
+on:
+  push:
+    branches: ["main"]
+  pull_request:
+    branches: ["main"]
+
+jobs:
+  ros2_node:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        release: [humble, iron, jazzy, rolling]
+    container: osrf/ros:${{ matrix.release }}-desktop
+    steps:
+      - name: Setup cmake
+        uses: jwlawson/[email protected]
+        with:
+          cmake-version: "3.25.x"
+      - uses: actions/checkout@v3
+      - name: Run colcon
+        run: source /opt/ros/${{ matrix.release }}/setup.bash && colcon build --event-handlers console_direct+
+        shell: bash

.gitignore

@@ -0,0 +1,117 @@
+# Created by https://www.toptal.com/developers/gitignore/api/c++
+# Edit at https://www.toptal.com/developers/gitignore?templates=c++
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# End of https://www.toptal.com/developers/gitignore/api/c++
+n# Created by https://www.toptal.com/developers/gitignore/api/cmake
+# Edit at https://www.toptal.com/developers/gitignore?templates=cmake
+
+### CMake ###
+CMakeLists.txt.user
+CMakeCache.txt
+CMakeFiles
+CMakeScripts
+Testing
+Makefile
+cmake_install.cmake
+install_manifest.txt
+compile_commands.json
+CTestTestfile.cmake
+_deps
+
+### CMake Patch ###
+# External projects
+*-prefix/
+
+# End of https://www.toptal.com/developers/gitignore/api/cmake
+n# Created by https://www.toptal.com/developers/gitignore/api/ros
+# Edit at https://www.toptal.com/developers/gitignore?templates=ros
+
+### ROS ###
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
+
+# End of https://www.toptal.com/developers/gitignore/api/ros
+n

.pre-commit-config.yaml

@@ -0,0 +1,18 @@
+repos:
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+    -   id: trailing-whitespace
+        exclude: \.patch$
+    -   id: end-of-file-fixer
+        exclude: \.patch$
+    -   id: check-yaml
+    -   id: check-added-large-files
+-   repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v14.0.0
+    hooks:
+    -   id: clang-format
+-   repo: https://github.com/ahans/cmake-format-precommit
+    rev: 8e52fb6506f169dddfaa87f88600d765fca48386
+    hooks:
+    - id: cmake-format

LICENSE

@@ -0,0 +1,22 @@
+MIT License
+
+Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+Stachniss.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,106 @@
+<div align="center">
+    <h1>Kinematic-ICP</h1>
+    <a href="https://github.com/PRBonn/kinematic-icp/releases"><img src="https://img.shields.io/github/v/release/PRBonn/kinematic-icp?label=version" /></a>
+    <a href="https://github.com/PRBonn/kinematic-icp/blob/main/LICENSE"><img src="https://img.shields.io/github/license/PRBonn/kinematic-icp" /></a>
+    <a href="https://github.com/PRBonn/kinematic-icp/blob/main/"><img src="https://img.shields.io/badge/Linux-FCC624?logo=linux&logoColor=black" /></a>
+    <br />
+    <br />
+    <a href=https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf>Paper</a>
+    <span>&nbsp;&nbsp;•&nbsp;&nbsp;</span>
+    <a href=https://github.com/PRBonn/kinematic-icp/issues>Contact Us</a>
+  <br />
+  <br />
+
+[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.
+
+<img src="https://github.com/user-attachments/assets/93826e4b-7319-459b-9d84-83929606ef4d" alt="Kinematic-ICP" width="500"/>
+
+</div>
+
+# How to Build
+
+Our system operates on ROS2, supporting **ROS Humble**, **Iron**, and **Jazzy**. To build and run Kinematic-ICP, follow these steps:
+
+1. **Clone the Repository**:
+   ```sh
+   cd <your_ros_workspace>/src
+   git clone https://github.com/PRBonn/kinematic-icp
+   cd ..
+   ```
+
+2. **Ensure all Dependencies are Installed**:
+   ```sh
+   rosdep install --from-paths src --ignore-src -r -y
+   ```
+
+3. **Build the Workspace**:
+   ```sh
+   colcon build
+   ```
+
+4. **Source the Setup Script**:
+   ```sh
+   source ./install/setup.bash
+   ```
+
+
+# TF Requirements
+
+Kinematic ICP can enhance existing odometry using a 3D LiDAR. However, there are specific requirements regarding motion and transformations due to the assumption that the robot operates on a unicycle kinematic model. Below are the key requirements:
+
+1. **Planar Movement**: The robot is expected to move primarily on a planar surface.
+
+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.)
+
+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.
+
+Finally, Kinematic ICP will publish a new `tf` transformation between `base_link` and `odom_lidar`.
+
+# Running the System
+
+This system offers two entry points for deployment, depending on your use case: one for real-time operation and one for offline processing.
+
+## 1. Real-Time Deployment: `online_node`
+
+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:
+
+```sh
+ros2 launch kinematic_icp online_node.launch.py lidar_topic:=<TOPIC>
+```
+
+To enable simultaneous visualization through RViz, use the `visualize` flag set to `true`:
+
+```sh
+ros2 launch kinematic_icp online_node.launch.py lidar_topic:=<TOPIC> visualize:=true
+```
+
+## 2. Offline Processing: `offline_node`
+
+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:
+
+```sh
+ros2 launch kinematic_icp offline_node.launch.py lidar_topic:=<TOPIC> bag_filename:=<ROSBAG>
+```
+
+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 **<ROSBAG>_kinematic_poses_tum.txt**. This file is saved in the same directory as the ROS bag file by default.
+
+To specify a custom directory for the output file, use the `output_dir` parameter:
+
+```sh
+ros2 launch kinematic_icp offline_node.launch.py lidar_topic:=<TOPIC> bag_filename:=<ROSBAG> output_dir:=<OUTPUT_DIRECTORY>
+```
+
+
+## Citation
+
+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).
+
+```bibtex
+@article{kissteam2024arxiv,
+  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}
+  title     = {{Kinematic-ICP: Enhancing LiDAR Odometry with Kinematic Constraints for Wheeled Mobile Robots Moving on Planar Surfaces
+  journal   = arXiv Preprint,
+  year      = {2024},
+  codeurl   = {https://github.com/PRBonn/kinematic-icp},
+}
+```