├── .devcontainer └── devcontainer.json ├── .gitignore ├── .gitmodules ├── .pages ├── Dockerfile ├── LICENSE.md ├── README.md ├── config ├── demo.yaml └── params.yaml ├── data └── demo │ ├── lidar_1.pcd │ ├── lidar_2.pcd │ └── lidar_3.pcd ├── doc └── img │ ├── Multi-LiCa_pipeline.png │ └── calibration_results.png ├── docker ├── build_docker.sh └── run_docker.sh ├── evaluation ├── config.yaml ├── evaluation.py └── evaluation_rel.py ├── launch └── calibration.launch.py ├── multi_lidar_calibrator ├── __init__.py ├── calibration │ ├── Calibration.py │ ├── Geometry.py │ ├── Lidar.py │ ├── __init__.py │ └── points_horizontal.pcd └── multi_lidar_calibrator.py ├── output └── .gitkeep ├── package.xml ├── requirements.txt ├── resource └── multi_lidar_calibrator ├── setup.cfg └── setup.py /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | // For format details, see https://aka.ms/vscode-remote/devcontainer.json or this file's README at: 2 | // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.1/containers/docker-existing-dockerfile 3 | { 4 | "name": "ros2_dev_container", 5 | // Sets the run context to one level up instead of the .devcontainer folder. 6 | "context": "..", 7 | // Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename. 8 | "dockerFile": "../Dockerfile", 9 | // Set *default* container specific settings.json values on container create. 10 | "settings": { 11 | "python.pythonPath": "/bin/python3", 12 | }, 13 | // Add the IDs of extensions you want installed when the container is created. 14 | "extensions": [ 15 | "ms-python.python", 16 | ], 17 | // Uncomment the next line to run commands after the container is created - for example installing curl. 18 | // Workspace mount 19 | "workspaceMount": "source=${localWorkspaceFolder},target=/ros_ws/src/,type=bind,consistency=delegated", 20 | // Define workspace 21 | "workspaceFolder": "/ros_ws", 22 | "runArgs": [ 23 | "-e", "DISPLAY=${env:DISPLAY}", 24 | "-v", "/tmp/.X11-unix:/tmp/.X11-unix", 25 | "-v", "/etc/localtime:/etc/localtime:ro", 26 | "-v", "/dev/input:/dev/input", 27 | "-e", "ROS_DOMAIN_ID=${env:ROS_DOMAIN_ID}", 28 | "-v", "/home/dominik/Documents/datasets", 29 | "--network=host", 30 | "--cap-add=SYS_PTRACE", 31 | "--security-opt=seccomp:unconfined", 32 | "--security-opt=apparmor:unconfined", 33 | "--privileged" 34 | ], 35 | "containerEnv": { 36 | "LIBGL_ALWAYS_SOFTWARE": "1" 37 | } 38 | } 39 | 40 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | output/** 2 | **/__pycache__ 3 | 4 | !output/.gitkeep -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "TEASER-plusplus"] 2 | path = TEASER-plusplus 3 | url = https://github.com/MIT-SPARK/TEASER-plusplus.git 4 | -------------------------------------------------------------------------------- /.pages: -------------------------------------------------------------------------------- 1 | nav: 2 | - ... | regex=^(?!.*(README.md|CONTRIBUTING.md|CODE_OF_CONDUCT.md|DISCLAIMER.md)).*$ 3 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | ARG ROS_DISTRO=humble 4 | ENV ROS_DISTRO=${ROS_DISTRO} 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | 7 | RUN apt-get update && \ 8 | apt-get install -y software-properties-common locales && \ 9 | add-apt-repository universe 10 | RUN locale-gen en_US en_US.UTF-8 && \ 11 | update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 12 | ENV LANG=en_US.UTF-8 13 | 14 | # Create a non-root user 15 | ARG USERNAME=local 16 | ARG USER_UID=1000 17 | ARG USER_GID=$USER_UID 18 | RUN groupadd --gid $USER_GID $USERNAME \ 19 | && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ 20 | # [Optional] Add sudo support for the non-root user 21 | && apt-get update \ 22 | && apt-get install -y sudo \ 23 | && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ 24 | && chmod 0440 /etc/sudoers.d/$USERNAME \ 25 | # Cleanup 26 | && rm -rf /var/lib/apt/lists/* \ 27 | && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc 28 | 29 | RUN apt-get update && apt-get install -y -q --no-install-recommends \ 30 | curl \ 31 | nano \ 32 | vim \ 33 | build-essential \ 34 | ca-certificates \ 35 | curl \ 36 | gnupg2 \ 37 | lsb-release \ 38 | python3-pip \ 39 | git \ 40 | cmake \ 41 | make \ 42 | apt-utils 43 | 44 | # ROS 2 45 | RUN sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' 46 | RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - 47 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 48 | RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 49 | RUN apt-get update && apt-get install -y -q --no-install-recommends \ 50 | python3-colcon-common-extensions \ 51 | ros-${ROS_DISTRO}-desktop \ 52 | ros-${ROS_DISTRO}-rmw-cyclonedds-cpp 53 | 54 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 55 | 56 | RUN apt-get remove python3-blinker -y 57 | 58 | WORKDIR /ros_ws 59 | 60 | COPY . /ros_ws/src/multi_lidar_calibration/ 61 | 62 | RUN cd /ros_ws/src/multi_lidar_calibration/TEASER-plusplus && \ 63 | mkdir build && \ 64 | cd build && \ 65 | cmake -DTEASERPP_PYTHON_VERSION=3.10 .. && \ 66 | make teaserpp_python && \ 67 | cd python && pip install . 68 | 69 | RUN pip install --no-cache-dir --upgrade pip && \ 70 | pip install --no-cache-dir -r src/multi_lidar_calibration/requirements.txt 71 | 72 | RUN /bin/bash -c '. /opt/ros/$ROS_DISTRO/setup.bash && \ 73 | colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to multi_lidar_calibrator' 74 | 75 | ENV QT_DEBUG_PLUGINS=1 76 | 77 | CMD [ "bash" ] -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | # GNU Lesser General Public License 2 | 3 | _Version 3, 29 June 2007_ 4 | _Copyright © 2007 Free Software Foundation, Inc. <>_ 5 | 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | ### 0. Additional Definitions 14 | 15 | As used herein, “this License” refers to version 3 of the GNU Lesser 16 | General Public License, and the “GNU GPL” refers to version 3 of the GNU 17 | General Public License. 18 | 19 | “The Library” refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An “Application” is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A “Combined Work” is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the “Linked 30 | Version”. 31 | 32 | The “Minimal Corresponding Source” for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The “Corresponding Application Code” for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | ### 1. Exception to Section 3 of the GNU GPL 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | ### 2. Conveying Modified Versions 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | - **a)** under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | - **b)** under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | ### 3. Object Code Incorporating Material from Library Header Files 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | - **a)** Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | - **b)** Accompany the object code with a copy of the GNU GPL and this license 76 | document. 77 | 78 | ### 4. Combined Works 79 | 80 | You may convey a Combined Work under terms of your choice that, 81 | taken together, effectively do not restrict modification of the 82 | portions of the Library contained in the Combined Work and reverse 83 | engineering for debugging such modifications, if you also do each of 84 | the following: 85 | 86 | - **a)** Give prominent notice with each copy of the Combined Work that 87 | the Library is used in it and that the Library and its use are 88 | covered by this License. 89 | 90 | - **b)** Accompany the Combined Work with a copy of the GNU GPL and this license 91 | document. 92 | 93 | - **c)** For a Combined Work that displays copyright notices during 94 | execution, include the copyright notice for the Library among 95 | these notices, as well as a reference directing the user to the 96 | copies of the GNU GPL and this license document. 97 | 98 | - **d)** Do one of the following: 99 | 100 | - **0)** Convey the Minimal Corresponding Source under the terms of this 101 | License, and the Corresponding Application Code in a form 102 | suitable for, and under terms that permit, the user to 103 | recombine or relink the Application with a modified version of 104 | the Linked Version to produce a modified Combined Work, in the 105 | manner specified by section 6 of the GNU GPL for conveying 106 | Corresponding Source. 107 | - **1)** Use a suitable shared library mechanism for linking with the 108 | Library. A suitable mechanism is one that **(a)** uses at run time 109 | a copy of the Library already present on the user's computer 110 | system, and **(b)** will operate properly with a modified version 111 | of the Library that is interface-compatible with the Linked 112 | Version. 113 | 114 | - **e)** Provide Installation Information, but only if you would otherwise 115 | be required to provide such information under section 6 of the 116 | GNU GPL, and only to the extent that such information is 117 | necessary to install and execute a modified version of the 118 | Combined Work produced by recombining or relinking the 119 | Application with a modified version of the Linked Version. (If 120 | you use option **4d0**, the Installation Information must accompany 121 | the Minimal Corresponding Source and Corresponding Application 122 | Code. If you use option **4d1**, you must provide the Installation 123 | Information in the manner specified by section 6 of the GNU GPL 124 | for conveying Corresponding Source.) 125 | 126 | ### 5. Combined Libraries 127 | 128 | You may place library facilities that are a work based on the 129 | Library side by side in a single library together with other library 130 | facilities that are not Applications and are not covered by this 131 | License, and convey such a combined library under terms of your 132 | choice, if you do both of the following: 133 | 134 | - **a)** Accompany the combined library with a copy of the same work based 135 | on the Library, uncombined with any other library facilities, 136 | conveyed under the terms of this License. 137 | - **b)** Give prominent notice with the combined library that part of it 138 | is a work based on the Library, and explaining where to find the 139 | accompanying uncombined form of the same work. 140 | 141 | ### 6. Revised Versions of the GNU Lesser General Public License 142 | 143 | The Free Software Foundation may publish revised and/or new versions 144 | of the GNU Lesser General Public License from time to time. Such new 145 | versions will be similar in spirit to the present version, but may 146 | differ in detail to address new problems or concerns. 147 | 148 | Each version is given a distinguishing version number. If the 149 | Library as you received it specifies that a certain numbered version 150 | of the GNU Lesser General Public License “or any later version” 151 | applies to it, you have the option of following the terms and 152 | conditions either of that published version or of any later version 153 | published by the Free Software Foundation. If the Library as you 154 | received it does not specify a version number of the GNU Lesser 155 | General Public License, you may choose any version of the GNU Lesser 156 | General Public License ever published by the Free Software Foundation. 157 | 158 | If the Library as you received it specifies that a proxy can decide 159 | whether future versions of the GNU Lesser General Public License shall 160 | apply, that proxy's public statement of acceptance of any version is 161 | permanent authorization for you to choose that version for the 162 | Library. 163 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 | 3 |

Multi - LiCa

4 | 5 | Multi - LiDAR-to-LiDAR calibration framework for ROS 2 and non-ROS applications 6 | 7 | [![Linux](https://img.shields.io/badge/os-linux-blue.svg)](https://www.linux.org/) 8 | [![Docker](https://badgen.net/badge/icon/docker?icon=docker&label)](https://www.docker.com/) 9 | [![ROS2humble](https://img.shields.io/badge/ros2-humble-blue.svg)](https://docs.ros.org/en/humble/index.html) 10 | [![arXiv](https://img.shields.io/badge/arXiv-1234.56789-b31b1b.svg)](https://arxiv.org/abs/2501.11088) 11 | [![DOI:10.1109/MFI62651.2024.10705773](http://img.shields.io/badge/DOI-10.1109/MFI62651.2024.10705773-00629B.svg)](https://doi.org/10.1109/MFI62651.2024.10705773) 12 | 13 | 14 | 15 |
16 | 17 |

Introduction

18 | This project provides an extrinsic calibration framework for quickly calibrating multiple LiDAR sensors. It employs the Generalized Iterative Closest Point (GICP) algorithm for LiDAR-to-LiDAR extrinsic calibration and uses the RANdom SAmple Consensus (RANSAC) method to calibrate the pitch and z-distance to the ground of a single LiDAR, assuming other coordinates are known. 19 | 20 | It has proven to be robust for different sensor setups and environments without the need of an initial guess. 21 | We use a FPFH-based feature vector creation with an TEASER++ feature matching for the coarse alignment, which is used as initial guess for the GICP algorithm. 22 | 23 |

Overview

24 |
25 | 26 | 27 | _Motion- and targetless multi - LiDAR-to-LiDAR Calibration Pipeline, 28 | developed at the Institute of Automotive Technology, TUM_ 29 | 30 |
31 |

Limitations

32 | 33 | - Our tool was specifically developed for motionless calibration. 34 | - We assume that each LiDAR to be calibrated has either a directly overlapping FOV with the target LiDAR FOV or has overlap with other LiDAR(s) with overlap to the target. This can be cascading dependency to the target. 35 | - We assume that the ground is flat and the environment is static. 36 | - Input point clouds for the calibration are in sensor_msgs/PointCloud2 or in .pcd format. 37 | 38 |

Prerequisites

39 | The bare minimum requirement for our tool is a Linux-based OS and Docker, as we provide a Docker image with our framework. 40 | You do not need to build anything locally, but you are free to do so as described in the following section. 41 | For the local build, you will need ROS 2 - humble, Python 3.10 with opend3d, scipy, ros2_numpy and pandas (optional). 42 | 43 |

Installation and Usage

44 | 45 | We use submodules, therefore, use `git clone --recurse-submodules git@github.com:TUMFTM/Multi_LiCa.git` 46 | 47 |

🐋 Docker Environment

48 | 49 | 1. Build the Docker image: 50 | 51 | ``` 52 | ./docker/build_docker.sh 53 | ``` 54 | 55 | 2. Run the container: 56 | 57 | ``` 58 | ./docker/run_docker.sh 59 | ``` 60 | 61 |

🖥 Local Build

62 | 63 | 1. Install ROS2 humble (might work with other ROS2 distributions but wasn't tested): 64 | 65 | 66 | 2. Create a ROS 2 workspace: 67 | 68 | ``` 69 | mkdir -p ~/ros2_ws 70 | cd ~/ros2_ws 71 | ``` 72 | 73 | 3. Clone the repository: 74 | 75 | ``` 76 | git clone --recurse-submodules git@github.com:TUMFTM/Multi_LiCa.git 77 | ``` 78 | 79 | 4. Install dependencies: 80 | 81 | ``` 82 | cd Multi_LiCa 83 | pip install --no-cache-dir --upgrade pip 84 | pip install --no-cache-dir -r requirements.txt 85 | ``` 86 | 87 | 5. Source the ROS 2 environment and build the project using `colcon`: 88 | 89 | ``` 90 | source /opt/ros/$ROS_DISTRO/setup.bash 91 | colcon build --symlink-install --packages-up-to multi_lidar_calibrator --cmake-args -DCMAKE_BUILD_TYPE=Release 92 | ``` 93 | 94 |

⏯️ Usage

95 | 96 | 1. Configure the parameters to fit your data: 97 | 98 | ``` 99 | vim config/.yaml 100 | ``` 101 | 102 | 2. Launch the multi_lidar_calibrator node: 103 | 104 | ``` 105 | ros2 launch multi_lidar_calibrator calibration.launch.py parameter_file:=/path/to/parameter/file 106 | ``` 107 | 108 |

⚙️ Configuration

109 | 110 | - We provided a detailed parameter file with explanation with `config/params.yaml` 111 | 112 | - Configure `config/params.yaml` to fit your data. Depending on the application, you may need to specify the LiDARs, paths to .pcd files, or LiDAR topic names. You may also change GICP and RANSAC parameters. 113 | 114 | - In addition to LiDAR-to-LiDAR calibration, you can perform target LiDAR-to-ground/base calibration if your x,y translation and roll, yaw rotation are precisely known. 115 | If you are using to-base calibration, you may choose a URDF file to save the calibration so that it can be directly used in your ROS robot-state-publisher. 116 | 117 | - When running in a container, ensure that your local and container environments have the same ROS_DOMAIN_ID. If not, set it to be the same with `export ROS_DOMAIN_ID=`. 118 | 119 | - When using ROS 2, verify that the transformation guess is published on the `/tf_static` topic and that the data is published for all specified LiDARs. 120 | 121 |

🎞️ Demo

122 | 123 | On default, the tool will launch a demo with data from [OpenCalib](https://github.com/PJLab-ADG/SensorsCalibration/tree/master). 124 | It will open a window and will display three pointclouds and their initial transforms. You can inspect the files in the interactive window. After closing the window (press Q), the tool will calculate the transformations ans will print the results to the terminal, write them to the output directory and will display a windows with the transformed pointclouds. 125 | 126 |

Citation

127 | 128 | ```bibtex 129 | @article{kulmermfi2024, 130 | author = {Kulmer, Dominik and Tahiraj, Ilir and Chumak, Andrii and Lienkamp, Markus}, 131 | title = {{Multi-LiCa: A Motion- and Targetless Multi - LiDAR-to-LiDAR Calibration Framework}}, 132 | journal = {2024 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI)}, 133 | pages = {1-7}, 134 | doi = {10.1109/MFI62651.2024.10705773}, 135 | year = {2024}, 136 | codeurl = {https://github.com/TUMFTM/Multi_LiCa}, 137 | } 138 | ``` 139 | 140 |

Other OSS Calibration Frameworks

141 | 142 | - [GMMCalib](https://github.com/TUMFTM/GMMCalib) 143 | - [OpenCalib](https://github.com/PJLab-ADG/SensorsCalibration/tree/master) 144 | - [LL-Calib](https://github.com/autocore-ai/calibration_tools/tree/main/lidar-lidar-calib) 145 | - [Multi LiDAR Calibrator ROS 1](https://github.com/Ridecell/Autoware/tree/master/ros/src/sensing/fusion/packages/multi_lidar_calibrator) 146 | - [Multi LiDAR Calibrator ROS 2](https://github.com/pixmoving-moveit/multi_lidar_calibration_ros2) 147 | - [mlcc](https://github.com/hku-mars/mlcc) 148 | - [T-FAC](https://github.com/AlienCat-K/LiDAR-Automatic-Calibration) 149 | - [AutoL2LCalib](https://github.com/JunhaAgu/AutoL2LCalib) 150 | - [Multi LiDAR Calibration](https://github.com/yinwu33/multi_lidar_calibration) 151 | - [Multi LiDARs CALIBRATE](https://github.com/GDUT-Kyle/MULTI_LIDARs_CALIBRATE) 152 | - [multi lidar calibration](https://github.com/liuzm-slam/multi_lidar_calibration) 153 | - [Appearance Calibration](https://github.com/ram-lab/lidar_appearance_calibration) 154 | -------------------------------------------------------------------------------- /config/demo.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | visualize: true 4 | 5 | # Topics for all lidars 6 | lidar_topics: [lidar_1, lidar_2, lidar_3] 7 | 8 | # path for point clouds in pcd format. Structure: /pcd_directory/.pcd 9 | pcd_directory: /../data/demo/ 10 | 11 | # logging output 12 | results_file: demo_results.txt 13 | 14 | # calibration results are stored here (path relative to the dir with multi_lidar_calibrator.py) 15 | output_dir: /../output/ 16 | -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | # number of point clouds for each lidar for the calibration. Larger number might improve robustness 4 | # when > 1, the environment and LiDARs should be static. In general, calibration is (much) worse if dynamic. 5 | frame_count: 5 6 | # number of complete calibration runs (for testing purposes) 7 | runs_count: 1 8 | 9 | # when enabled,finds point clouds with most overlap automatically 10 | # this method is more general and might be more precise but is slower 11 | use_fitness_based_calibration: false 12 | 13 | # Topics for all lidars 14 | lidar_topics: [/sensing/lidar/top/rectified/pointcloud, /rslidar_points] 15 | # topic for TFMessage 16 | tf_topic: /tf_static 17 | visualize: false 18 | # read transfromations to base from a given table instead of TF message 19 | read_tf_from_table: true 20 | table_degrees: true 21 | # table of lidar name (frame_id in ROS), xyz translation [m], rpy rotation [deg/rad] 22 | velodyne_top: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 23 | rslidar: [0.0, 0.000, 0.0, 0.0, 0.0, 0.0] 24 | 25 | read_pcds_from_file: false 26 | # path for point clouds in pcd format. Structure: //... 27 | # (path relative to the dir with multi_lidar_calibrator.py) 28 | # LiDAR transformations must be defined in this file when reading pcds from files 29 | pcd_directory: /../data/pcds/ 30 | 31 | base_to_ground_z: 0.338 # assumes base is directly above the origin 32 | 33 | # when not empty, write resultig transformations into an urdf file 34 | # assumes that joints in the file have nane in for of _joint 35 | # urdf_path: "/home/andrii/TUM/thesis/SensorCalibration/ros2_ws/src/edgar/edgar_state_publisher/urdf/edgar.description.states.urdf" 36 | 37 | # Frame id of the master lidar. Firstly, other LiDARs are calibrated w.r.t. to this 38 | target_frame_id: velodyne_top 39 | # target_frame_id: "lidar_ouster_right" 40 | # Frame id for the base frame relative to which the LiDARs are calibrated in the end 41 | base_frame_id: rear_axle_center 42 | # when true, calibrate target LiDAR (z, and pitch) to the base frame. 43 | calibrate_target: false 44 | # when true, calibrate all LiDARs to the base frame isntead of to other LiDARs 45 | calibrate_to_base: false 46 | 47 | # calibration results are stored here (path relative to the dir with multi_lidar_calibrator.py) 48 | default_output_dir: /../output/ 49 | 50 | # parameters for the GICP algorithm 51 | # Maximum correspondence points-pair distance. 52 | max_corresp_dist: 1.0 53 | # ICP algorithm stops if the relative change of fitness and rmse hit relative_fitness 54 | # and relative_rmse individually, or the iteration number exceeds max_iteration 55 | rel_fitness: 0.0000001 56 | rel_rmse: 0.0000001 57 | max_iterations: 100 58 | # just a constant. Changing it too much will probably lead to poor results. 59 | epsilon: 0.005 60 | # makes GICP faster by reducing number of points but might reduce accuracy 61 | # voxel size [m] in which the cloud is downampled. Highly recommended when using RANSAC 62 | # activated if > 0.0 63 | voxel_size: 0.05 64 | # makes GICP faster by removing ground from a cloud but might reduce accuracy 65 | remove_ground_flag: false 66 | # calibrations with fitness score <= threshold are considered unsuccessful (not enough overlap) 67 | fitness_score_threshold: 0.2 68 | 69 | # parameters for RANSAC. RANSAC is used to calibrate target LiDAR to the ground plane 70 | # or to speed up GICP by removing ground plane from the point cloud. 71 | # max distance for the point inlier to plane 72 | distance_threshold: 0.1 73 | # number of points used to define a plane 74 | ransac_n: 10 75 | # number of iterations of RANSAC. Larger number => larger probability to find a better plane 76 | num_iterations: 2000 77 | # voxel size for RANSAC for pitch calibration 78 | r_voxel_size: 0.1 79 | # number of reruns of RANSAC for better robustness 80 | r_runs: 10 81 | -------------------------------------------------------------------------------- /data/demo/lidar_1.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/data/demo/lidar_1.pcd -------------------------------------------------------------------------------- /data/demo/lidar_2.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/data/demo/lidar_2.pcd -------------------------------------------------------------------------------- /data/demo/lidar_3.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/data/demo/lidar_3.pcd -------------------------------------------------------------------------------- /doc/img/Multi-LiCa_pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/doc/img/Multi-LiCa_pipeline.png -------------------------------------------------------------------------------- /doc/img/calibration_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/doc/img/calibration_results.png -------------------------------------------------------------------------------- /docker/build_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" &>/dev/null && pwd)" 4 | 5 | docker build -f $SCRIPT_DIR/../Dockerfile -t tum.ftm.multi_lidar_calibration:latest . 6 | -------------------------------------------------------------------------------- /docker/run_docker.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" &>/dev/null && pwd)" 4 | 5 | xhost +local: 6 | 7 | docker run -it --rm --privileged \ 8 | --network host \ 9 | -e DISPLAY=${DISPLAY} \ 10 | -v /tmp/.X11-unix:/tmp/.X11-unix \ 11 | -v $SCRIPT_DIR/../config:/ros_ws/src/multi_lidar_calibration/config \ 12 | -v $SCRIPT_DIR/../data:/ros_ws/src/multi_lidar_calibration/data \ 13 | -v $SCRIPT_DIR/../output:/ros_ws/src/multi_lidar_calibration/output \ 14 | tum.ftm.multi_lidar_calibration:latest \ 15 | /bin/bash 16 | 17 | xhost -local: 18 | -------------------------------------------------------------------------------- /evaluation/config.yaml: -------------------------------------------------------------------------------- 1 | # [x y z r p y] for EDGAR 2 | # [x y z qx qy qz qw] for HeLiPR 3 | ground_truth: 4 | main_lidar: [0, 0, 0, 0, 0, 0] 5 | lidar_1: [-0.05003868754137674, -0.6182657287315831, -0.2210218828997021 ,19.761386585960206 ,9.671599526442137 ,4.528509610943743] 6 | lidar_2: [0.018654221998691264, -1.2213014405123956, -0.44237105646436303 ,39.9427218516336 ,-0.48189320093417537 ,1.220711041394239] 7 | lidar_3: [-2.692777651895696, -0.6935691501242932, -0.11539450889299188 ,19.95269089910927 ,-11.158756732625704 ,-3.0853232386037823] 8 | 9 | # [deg] x y z r p y 10 | calibration: 11 | lidar_1: [-0.04623345928427628 ,-0.6083580787632855 ,-0.2261904420354155 ,19.76077565640768 ,9.6105862444547, 4.48426248738788] 12 | lidar_2: [-0.015582894322383505, -1.2184022475297067 ,-0.43754838940491786 ,39.96339416907645 ,-0.3281780704703811 ,1.2074037966430986] 13 | lidar_3: [-2.6503996565781494 ,-0.6947256501155331, -0.1061083588396785, 19.61996156314735, -10.813415564912178 ,-3.052343294964041] -------------------------------------------------------------------------------- /evaluation/evaluation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import yaml 3 | from scipy.spatial.transform import Rotation as R 4 | 5 | def load_config(file_path): 6 | with open(file_path, 'r') as file: 7 | config = yaml.safe_load(file) 8 | return config 9 | 10 | def pose_to_matrix(x, y, z, qx, qy, qz, qw): 11 | rotation = R.from_quat([qx, qy, qz, qw]) 12 | transformation_matrix = np.eye(4) 13 | transformation_matrix[:3, :3] = rotation.as_matrix() 14 | transformation_matrix[:3, 3] = [x, y, z] 15 | return transformation_matrix 16 | 17 | def relative_to_absolute(main_matrix, rel_pose): 18 | x, y, z, roll, pitch, yaw = rel_pose 19 | roll, pitch, yaw = np.deg2rad([roll, pitch, yaw]) 20 | rotation = R.from_euler('xyz', [roll, pitch, yaw]) 21 | rel_matrix = np.eye(4) 22 | rel_matrix[:3, :3] = rotation.as_matrix() 23 | rel_matrix[:3, 3] = [x, y, z] 24 | abs_matrix = np.dot(main_matrix, rel_matrix) 25 | return abs_matrix 26 | 27 | def matrix_to_pose(matrix): 28 | rotation = R.from_matrix(matrix[:3, :3]) 29 | quat = rotation.as_quat() 30 | translation = matrix[:3, 3] 31 | return translation.tolist() + quat.tolist() 32 | 33 | def calculate_relative_pose(main_matrix, matrix): 34 | rel_matrix = np.dot(np.linalg.inv(main_matrix), matrix) 35 | rotation = R.from_matrix(rel_matrix[:3, :3]) 36 | translation = rel_matrix[:3, 3] 37 | euler_angles = np.rad2deg(rotation.as_euler('xyz')) 38 | return translation.tolist() + euler_angles.tolist() 39 | 40 | def calculate_rmse_error(gt_pose, calc_pose): 41 | gt_translation = np.array(gt_pose[:3]) 42 | calc_translation = np.array(calc_pose[:3]) 43 | translation_errors = gt_translation - calc_translation 44 | translation_error_rmse = np.sqrt(np.mean(translation_errors ** 2)) 45 | 46 | gt_rotation = R.from_euler('xyz', gt_pose[3:], degrees=True) 47 | calc_rotation = R.from_euler('xyz', calc_pose[3:], degrees=True) 48 | rotation_error = gt_rotation.inv() * calc_rotation 49 | rotation_error_degrees = rotation_error.magnitude() * (180.0 / np.pi) 50 | 51 | gt_euler = np.array(gt_pose[3:]) 52 | calc_euler = np.array(calc_pose[3:]) 53 | rotation_errors = gt_euler - calc_euler 54 | 55 | return translation_errors, translation_error_rmse, rotation_errors, rotation_error_degrees 56 | 57 | def evaluate_poses(ground_truth, calibration): 58 | main_lidar_pose = ground_truth['main_lidar'] 59 | main_lidar_matrix = pose_to_matrix(*main_lidar_pose) 60 | 61 | # Ground truth relative poses 62 | ground_truth_rel = {} 63 | for lidar, pose in ground_truth.items(): 64 | if lidar != 'main_lidar': 65 | matrix = pose_to_matrix(*pose) 66 | ground_truth_rel[lidar] = calculate_relative_pose(main_lidar_matrix, matrix) 67 | 68 | # Comparison 69 | comparison = {} 70 | for lidar, rel_pose in calibration.items(): 71 | abs_matrix = relative_to_absolute(main_lidar_matrix, rel_pose) 72 | gt_rel_pose = ground_truth_rel.get(lidar, None) 73 | if gt_rel_pose: 74 | calc_rel_pose = calculate_relative_pose(main_lidar_matrix, abs_matrix) 75 | translation_errors, translation_error_rmse, rotation_errors, rotation_error_degrees = calculate_rmse_error(gt_rel_pose, calc_rel_pose) 76 | comparison[lidar] = { 77 | "ground_truth": gt_rel_pose, 78 | "calculated": rel_pose, 79 | "absolute_calculated": matrix_to_pose(abs_matrix), 80 | "translation_error_rmse": translation_error_rmse, 81 | "rotation_error_degrees": rotation_error_degrees, 82 | "translation_errors": translation_errors.tolist(), 83 | "rotation_errors": rotation_errors.tolist() 84 | } 85 | 86 | return comparison 87 | 88 | def main(config_path): 89 | config = load_config(config_path) 90 | ground_truth = config['ground_truth'] 91 | calibration = config['calibration'] 92 | 93 | evaluation = evaluate_poses(ground_truth, calibration) 94 | 95 | for lidar, data in evaluation.items(): 96 | print(f"Evaluation for {lidar}:") 97 | print(f" Ground Truth Relative Pose: {data['ground_truth']}") 98 | print(f" Calculated Relative Pose: {data['calculated']}") 99 | print(f" Absolute Calculated Pose: {data['absolute_calculated']}") 100 | print(f" Translation Errors [x, y, z]: {data['translation_errors']}") 101 | print(f" Rotation Errors [r, p, y] (degrees): {data['rotation_errors']}") 102 | print(f" Translation Error (RMSE): {data['translation_error_rmse']}") 103 | print(f" Rotation Error (Degrees): {data['rotation_error_degrees']}") 104 | print() 105 | 106 | if __name__ == "__main__": 107 | config_path = "config.yaml" 108 | main(config_path) 109 | -------------------------------------------------------------------------------- /evaluation/evaluation_rel.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import yaml 3 | from scipy.spatial.transform import Rotation as R 4 | 5 | def load_config(file_path): 6 | with open(file_path, 'r') as file: 7 | config = yaml.safe_load(file) 8 | return config 9 | 10 | def calculate_rmse_error(gt_pose, calc_pose): 11 | gt_translation = np.array(gt_pose[:3]) 12 | calc_translation = np.array(calc_pose[:3]) 13 | translation_errors = gt_translation - calc_translation 14 | translation_error_rmse = np.sqrt(np.mean(translation_errors ** 2)) 15 | 16 | gt_rotation = R.from_euler('xyz', gt_pose[3:], degrees=True) 17 | calc_rotation = R.from_euler('xyz', calc_pose[3:], degrees=True) 18 | rotation_error = gt_rotation.inv() * calc_rotation 19 | rotation_error_degrees = rotation_error.magnitude() * (180.0 / np.pi) 20 | rotation_errors_individual = rotation_error.as_euler('xyz', degrees=True) 21 | 22 | return translation_errors, translation_error_rmse, rotation_error_degrees, rotation_errors_individual 23 | 24 | def main(config_path): 25 | config = load_config(config_path) 26 | ground_truth = config['ground_truth'] 27 | calibration = config['calibration'] 28 | 29 | main_lidar_pose = ground_truth['main_lidar'] 30 | 31 | translation_errors_all = [] 32 | rotation_errors_all = [] 33 | rotation_errors_individual_all = [] 34 | 35 | for lidar, gt_pose in ground_truth.items(): 36 | if lidar != 'main_lidar': 37 | gt_pose_matrix = np.array(gt_pose) 38 | calc_pose = calibration[lidar] 39 | translation_errors, translation_error_rmse, rotation_error_degrees, rotation_errors_individual = calculate_rmse_error(gt_pose_matrix, calc_pose) 40 | print(f"Errors for {lidar}:") 41 | print(f" Translation Errors [x, y, z]: {translation_errors}") 42 | print(f" Translation Error (RMSE) [m]: {translation_error_rmse}") 43 | print(f" Rotation Error (Degrees): {rotation_error_degrees}") 44 | print(f" Rotation Errors Individual [r, p, y]: {rotation_errors_individual}") 45 | print() 46 | translation_errors_all.append(translation_errors) 47 | rotation_errors_all.append(rotation_error_degrees) 48 | rotation_errors_individual_all.append(rotation_errors_individual) 49 | 50 | avg_translation_errors = np.mean(translation_errors_all, axis=0) 51 | avg_rotation_errors = np.mean(rotation_errors_all) 52 | avg_rotation_errors_individual = np.mean(rotation_errors_individual_all, axis=0) 53 | 54 | print(f"Average Translation Errors [m]: {avg_translation_errors}") 55 | print(f"Average Rotation Error (Degrees): {avg_rotation_errors}") 56 | print(f"Average Rotation Errors Individual [r, p, y]: {avg_rotation_errors_individual}") 57 | 58 | if __name__ == "__main__": 59 | config_path = "config.yaml" 60 | main(config_path) 61 | -------------------------------------------------------------------------------- /launch/calibration.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch_ros.actions import Node 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import DeclareLaunchArgument 8 | from launch.substitutions import LaunchConfiguration 9 | 10 | 11 | def generate_launch_description(): 12 | pkg_share = get_package_share_directory("multi_lidar_calibrator") 13 | parameter_file = os.path.join(pkg_share, "config", "demo.yaml") 14 | output = os.path.join(pkg_share, "output") 15 | 16 | output_dir_arg = DeclareLaunchArgument( 17 | "output_dir", default_value=output, description="Path to the output directory." 18 | ) 19 | 20 | params_declare = DeclareLaunchArgument( 21 | "parameter_file", 22 | default_value=parameter_file, 23 | description="Path to the ROS2 parameters file to use.", 24 | ) 25 | 26 | parameter_file_launch_config = LaunchConfiguration("parameter_file") 27 | output_dir_launch_config = LaunchConfiguration("output_dir") 28 | 29 | return LaunchDescription( 30 | [ 31 | params_declare, 32 | output_dir_arg, 33 | Node( 34 | package="multi_lidar_calibrator", 35 | executable="multi_lidar_calibrator", 36 | name="multi_lidar_calibration_node", 37 | parameters=[parameter_file_launch_config, 38 | output_dir_launch_config], 39 | output="screen", 40 | ), 41 | ] 42 | ) 43 | -------------------------------------------------------------------------------- /multi_lidar_calibrator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/multi_lidar_calibrator/__init__.py -------------------------------------------------------------------------------- /multi_lidar_calibrator/calibration/Calibration.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | 3 | import numpy as np 4 | import open3d as o3d 5 | 6 | import teaserpp_python 7 | from scipy.spatial import cKDTree 8 | 9 | from .Geometry import Rotation, TransformationMatrix, Translation 10 | from .Lidar import Lidar 11 | from scipy.spatial.transform import Rotation as R 12 | 13 | 14 | def visualize_calibration(lidar_list: list[Lidar], transformed=True, only_paint=False): 15 | """ 16 | Visualize the calibration of a list of LiDAR sensors. 17 | 18 | Args: 19 | lidar_list: A list of Lidar objects to visualize. 20 | transformed: If True, visualize the transformed point clouds. If False, visualize the original point clouds. 21 | only_paint: If true, point clouds are only painted but not transformed. 22 | Returns: 23 | None 24 | """ 25 | # Define colors for the point clouds in RGB format 26 | colors = [ 27 | [0, 0.4, 0.74], 28 | [1, 1, 1], 29 | [0.89, 0.45, 0.13], 30 | [0.64, 0.68, 0], 31 | [0.6, 0.78, 0.92], 32 | [0.5, 0.5, 0.5], 33 | [0.8, 0.8, 0.8], 34 | [0.2, 0.4, 0.8], 35 | [0.8, 0.4, 0.2], 36 | [0.8, 0.6, 0.6], 37 | ] 38 | for lidar, color in zip(lidar_list, colors): 39 | if transformed: 40 | lidar.pcd_transformed.paint_uniform_color(color) 41 | else: 42 | lidar.pcd.paint_uniform_color(color) 43 | if only_paint: 44 | return 45 | 46 | vis = o3d.visualization.Visualizer() 47 | vis.create_window(visible=True) 48 | # Change the background color to black 49 | vis.get_render_option().background_color = [0, 0, 0] 50 | vis.get_render_option().point_size = 2 51 | 52 | if transformed: 53 | for lidar in lidar_list: 54 | vis.add_geometry(lidar.pcd_transformed) 55 | else: 56 | for lidar in lidar_list: 57 | vis.add_geometry(lidar.pcd) 58 | 59 | vis.run() 60 | 61 | 62 | def modify_urdf_joint_origin(file_path: str, joint_name: str, tf_matrix: TransformationMatrix): 63 | """ 64 | Modify the origin of a joint in a URDF file. 65 | 66 | Args: 67 | file_path: The path to the URDF file. 68 | joint_name: The name of the joint to modify. 69 | tf_matrix: The new transformation matrix for the joint. 70 | 71 | Returns: 72 | None 73 | """ 74 | # Parse the URDF file 75 | parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True)) 76 | tree = ET.parse(file_path, parser=parser) 77 | root = tree.getroot() 78 | 79 | # Find the joint with the given name 80 | for joint in root.iter("joint"): 81 | if "name" in joint.attrib and joint.attrib["name"] == joint_name: 82 | origin = joint.find("origin") 83 | if origin is not None: 84 | origin.attrib["xyz"] = tf_matrix.translation.__str__() 85 | origin.attrib["rpy"] = tf_matrix.rotation.__str__() 86 | else: 87 | raise Exception("joint has no origin to be modified") 88 | tree.write(file_path, xml_declaration=True) 89 | 90 | class Calibration: 91 | """ 92 | Handles initial and GICP calibration between source and target lidars. 93 | """ 94 | 95 | def __init__( 96 | self, 97 | source: Lidar, 98 | target: Lidar, 99 | max_corresp_dist=1.0, 100 | epsilon=0.0001, 101 | rel_fitness=1e-6, 102 | rel_rmse=1e-6, 103 | max_iterations=30, 104 | distance_threshold=0.1, 105 | ransac_n=3, 106 | num_iterations=1000, 107 | crop_cloud=20, 108 | ): 109 | """ 110 | Initialize a Calibration object. 111 | 112 | Args: 113 | source: The source Lidar object. 114 | target: The target Lidar object. 115 | max_corresp_dist: The maximum correspondence distance for the GICP algorithm. 116 | epsilon: The epsilon parameter for the GICP algorithm. 117 | rel_fitness: The relative fitness convergence criterion for the GICP algorithm. 118 | rel_rmse: The relative RMSE convergence criterion for the GICP algorithm. 119 | max_iterations: The maximum number of iterations for the GICP algorithm. 120 | distance_threshold: The maximum distance for the point inlier to plane. 121 | ransac_n: The number of points used to define a plane. 122 | num_iterations: The number of iterations of RANSAC. 123 | """ 124 | self.source = source 125 | self.target = target 126 | self.max_corresp_dist = max_corresp_dist 127 | self.epsilon = epsilon 128 | self.rel_fitness = rel_fitness 129 | self.rel_rmse = rel_rmse 130 | self.max_iterations = max_iterations 131 | self.distance_threshold = distance_threshold 132 | self.ransac_n = ransac_n 133 | self.num_iterations = num_iterations 134 | self.crop_cloud = crop_cloud 135 | self.initial_transformation = self.compute_initial_transformation() 136 | self.calibrated_transformation = None 137 | self.reg_p2l = None 138 | 139 | def compute_initial_transformation(self) -> TransformationMatrix: 140 | """ 141 | Compute the initial transformation matrix between the source and target lidars. 142 | 143 | Returns: 144 | The initial transformation matrix. 145 | """ 146 | # Transform the source lidar to the basis coordinate system and 147 | # transform this lidar to the target coordinate system 148 | transformation_matrix = ( 149 | np.linalg.inv(self.target.tf_matrix.matrix) @ self.source.tf_matrix.matrix 150 | ) 151 | self.initial_transformation = TransformationMatrix.from_matrix(transformation_matrix) 152 | 153 | if self.source.pcd is None: 154 | raise Exception("no source point cloud") 155 | if self.target.pcd is None: 156 | raise Exception("no target point cloud") 157 | 158 | # Create copies of the source and target point clouds 159 | source_pcd = o3d.geometry.PointCloud(self.source.pcd.transform(transformation_matrix)) 160 | target_pcd = o3d.geometry.PointCloud(self.target.pcd) 161 | 162 | method = 'TEASER' 163 | if method == 'FPFH': 164 | fpfh_voxel_size = 0.5 165 | source_fpfh = self.preprocess_point_cloud(source_pcd, fpfh_voxel_size) 166 | target_fpfh = self.preprocess_point_cloud(target_pcd, fpfh_voxel_size) 167 | distance_threshold = fpfh_voxel_size * 10 168 | reg_fpfh = o3d.pipelines.registration.registration_fgr_based_on_feature_matching( 169 | source_pcd, target_pcd, source_fpfh, target_fpfh, 170 | o3d.pipelines.registration.FastGlobalRegistrationOption( 171 | use_absolute_scale=True, 172 | maximum_correspondence_distance=distance_threshold)) 173 | print("FPFH Trafo:") 174 | print(reg_fpfh.transformation) 175 | self.initial_transformation = TransformationMatrix.from_matrix(reg_fpfh.transformation) 176 | 177 | elif method == 'RANSAC': 178 | voxel_size = 0.35 179 | num_iterations = 20 180 | distance_threshold = voxel_size * 20 181 | transformations = [self.run_ransac_registration(source_pcd, target_pcd, voxel_size, distance_threshold) for _ in range(num_iterations)] 182 | median_trans = self.median_transformation(transformations) 183 | self.initial_transformation = TransformationMatrix.from_matrix(median_trans) 184 | 185 | elif method == 'TEASER': 186 | voxel_size = 0.35 187 | teaser_transformation = self.teaser_initial_registration(source_pcd, target_pcd, voxel_size) 188 | self.initial_transformation = TransformationMatrix.from_matrix(teaser_transformation) 189 | return self.initial_transformation 190 | 191 | def teaser_initial_registration(self, source_pcd, target_pcd, voxel_size): 192 | source_down, source_fpfh = self.preprocess_point_cloud(source_pcd, voxel_size) 193 | target_down, target_fpfh = self.preprocess_point_cloud(target_pcd, voxel_size) 194 | 195 | source_fpfh = np.array(source_fpfh.data).T 196 | target_fpfh = np.array(target_fpfh.data).T 197 | 198 | source_down = np.asarray(source_down.points).T 199 | target_down = np.asarray(target_down.points).T 200 | 201 | corrs_A, corrs_B = self.find_correspondences( 202 | source_fpfh, target_fpfh, mutual_filter=True) 203 | A_corr = source_down[:,corrs_A] 204 | B_corr = target_down[:,corrs_B] 205 | 206 | teaser_solver = self.get_teaser_solver(voxel_size) 207 | teaser_solver.solve(A_corr,B_corr) 208 | solution = teaser_solver.getSolution() 209 | transformation = np.eye(4) 210 | transformation[:3, :3] = solution.rotation 211 | transformation[:3, 3] = solution.translation 212 | return transformation 213 | 214 | def find_correspondences(self, feats0, feats1, mutual_filter=True): 215 | nns01 = self.find_knn_cpu(feats0, feats1, knn=1, return_distance=False) 216 | corres01_idx0 = np.arange(len(nns01)) 217 | corres01_idx1 = nns01 218 | 219 | if not mutual_filter: 220 | return corres01_idx0, corres01_idx1 221 | 222 | nns10 = self.find_knn_cpu(feats1, feats0, knn=1, return_distance=False) 223 | corres10_idx1 = np.arange(len(nns10)) 224 | corres10_idx0 = nns10 225 | 226 | mutual_filter = (corres10_idx0[corres01_idx1] == corres01_idx0) 227 | corres_idx0 = corres01_idx0[mutual_filter] 228 | corres_idx1 = corres01_idx1[mutual_filter] 229 | 230 | return corres_idx0, corres_idx1 231 | 232 | def find_knn_cpu(self, feat0, feat1, knn=1, return_distance=False): 233 | feat1tree = cKDTree(feat1) 234 | dists, nn_inds = feat1tree.query(feat0, k=knn, n_jobs=-1) 235 | if return_distance: 236 | return nn_inds, dists 237 | else: 238 | return nn_inds 239 | 240 | 241 | def get_teaser_solver(self, noise_bound): 242 | solver_params = teaserpp_python.RobustRegistrationSolver.Params() 243 | solver_params.cbar2 = 1.0 244 | solver_params.noise_bound = noise_bound 245 | solver_params.estimate_scaling = False 246 | solver_params.inlier_selection_mode = \ 247 | teaserpp_python.RobustRegistrationSolver.INLIER_SELECTION_MODE.PMC_EXACT 248 | solver_params.rotation_tim_graph = \ 249 | teaserpp_python.RobustRegistrationSolver.INLIER_GRAPH_FORMULATION.CHAIN 250 | solver_params.rotation_estimation_algorithm = \ 251 | teaserpp_python.RobustRegistrationSolver.ROTATION_ESTIMATION_ALGORITHM.GNC_TLS 252 | solver_params.rotation_gnc_factor = 1.4 253 | solver_params.rotation_max_iterations = 10000 254 | solver_params.rotation_cost_threshold = 1e-16 255 | solver = teaserpp_python.RobustRegistrationSolver(solver_params) 256 | return solver 257 | 258 | def run_ransac_registration(self, source_pcd, target_pcd, voxel_size, distance_threshold): 259 | source_down, source_fpfh = self.preprocess_point_cloud(source_pcd, voxel_size) 260 | target_down, target_fpfh = self.preprocess_point_cloud(target_pcd, voxel_size) 261 | 262 | reg_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( 263 | source_down, target_down, source_fpfh, target_fpfh, True, 264 | distance_threshold, 265 | o3d.pipelines.registration.TransformationEstimationPointToPoint(False), 266 | 3, 267 | [], 268 | o3d.pipelines.registration.RANSACConvergenceCriteria(1000, 0.925) 269 | ) 270 | return reg_ransac.transformation 271 | 272 | def transformation_to_rpy_xyz(self, transformation): 273 | rotation_matrix = transformation[:3, :3].copy() 274 | r = R.from_matrix(rotation_matrix) 275 | roll, pitch, yaw = r.as_euler('xyz', degrees=False) 276 | translation = transformation[:3, 3] 277 | return roll, pitch, yaw, translation[0], translation[1], translation[2] 278 | 279 | def median_transformation(self, transformations): 280 | rpy_xyz_list = [self.transformation_to_rpy_xyz(trans) for trans in transformations] 281 | rpy_xyz_array = np.array(rpy_xyz_list) 282 | 283 | median_rpy_xyz = np.median(rpy_xyz_array, axis=0) 284 | roll, pitch, yaw, x, y, z = median_rpy_xyz 285 | 286 | median_rotation_matrix = R.from_euler('xyz', [roll, pitch, yaw], degrees=False).as_matrix() 287 | median_transformation_matrix = np.eye(4) 288 | median_transformation_matrix[:3, :3] = median_rotation_matrix 289 | median_transformation_matrix[:3, 3] = [x, y, z] 290 | return median_transformation_matrix 291 | 292 | def compute_gicp_transformation(self, voxel_size=0.0, remove_ground_plane=False): 293 | """ 294 | Compute the GICP transformation between the source and target lidars. 295 | 296 | Args: 297 | voxel_size: The voxel size for downsampling the point clouds. If 0, no downsampling is performed. 298 | remove_ground_plane: If True, the ground plane is removed from the point clouds. 299 | 300 | Returns: 301 | The result of the GICP registration. 302 | """ 303 | if self.source.pcd is None: 304 | raise Exception("no source point cloud") 305 | if self.target.pcd is None: 306 | raise Exception("no target point cloud") 307 | 308 | # Create copies of the source and target point clouds 309 | source_pcd = o3d.geometry.PointCloud(self.source.pcd) 310 | target_pcd = o3d.geometry.PointCloud(self.target.pcd) 311 | 312 | # Downsample the point clouds if a voxel size is provided 313 | if voxel_size > 0.0: 314 | source_pcd = source_pcd.voxel_down_sample(voxel_size) 315 | target_pcd = target_pcd.voxel_down_sample(voxel_size) 316 | 317 | # Remove the ground plane from the point clouds if requested 318 | if remove_ground_plane: 319 | source_pcd = self.source.remove_ground_plane( 320 | source_pcd, self.distance_threshold, self.ransac_n, self.num_iterations 321 | ) 322 | target_pcd = self.source.remove_ground_plane( 323 | target_pcd, self.distance_threshold, self.ransac_n, self.num_iterations 324 | ) 325 | 326 | # Estimate normals for the point clouds 327 | source_pcd.estimate_normals() 328 | target_pcd.estimate_normals() 329 | 330 | # Perform GICP registration 331 | reg_p2l = o3d.pipelines.registration.registration_generalized_icp( 332 | source_pcd, 333 | target_pcd, 334 | self.max_corresp_dist, 335 | self.initial_transformation.matrix, 336 | o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(self.epsilon), 337 | o3d.pipelines.registration.ICPConvergenceCriteria( 338 | self.rel_fitness, self.rel_rmse, self.max_iterations 339 | ), 340 | ) 341 | 342 | # Store the transformation matrix and registration result 343 | self.calibrated_transformation = TransformationMatrix.from_matrix(reg_p2l.transformation) 344 | self.reg_p2l = reg_p2l 345 | return reg_p2l 346 | 347 | def transform_pointcloud(self, transformation_matrix=None): 348 | """ 349 | Apply a transformation to the source point cloud. 350 | 351 | Args: 352 | transformation_matrix: The transformation matrix to apply. If None, the calibrated transformation is used. 353 | 354 | Returns: 355 | None 356 | """ 357 | if transformation_matrix is None: 358 | if self.calibrated_transformation is None: 359 | self.compute_gicp_transformation() 360 | self.source.pcd_transformed.transform(self.calibrated_transformation.matrix) 361 | self.source.calib_tf_matrix = self.calibrated_transformation 362 | else: 363 | self.source.pcd_transformed.transform(transformation_matrix) 364 | self.source.calib_tf_matrix = TransformationMatrix.from_matrix(transformation_matrix) 365 | 366 | def info(self, degrees=False, matrix=False): 367 | """ 368 | Generate a string containing information about the calibration. 369 | 370 | Args: 371 | degrees: If True, the rotation angles are converted to degrees. If False, they are left in radians. 372 | matrix: If True, prints the transformation matrix. 373 | Returns: 374 | A string containing information about the calibration. 375 | """ 376 | if matrix: 377 | s = ( 378 | "calibrated transformation matrix:\n" 379 | + str(self.calibrated_transformation.matrix) 380 | + "\n" 381 | ) 382 | else: 383 | s = "" 384 | return ( 385 | self.source.name 386 | + " to " 387 | + self.target.name 388 | + " calibration\n" 389 | + "calibrated xyz = " 390 | + self.calibrated_transformation.translation.__str__() 391 | + "\n" 392 | + "calibrated rpy = " 393 | + self.calibrated_transformation.rotation.__str__(degrees) 394 | + "\n" 395 | + "fitness: " 396 | + str(self.reg_p2l.fitness) 397 | + ", inlier_rmse: " 398 | + str(self.reg_p2l.inlier_rmse) 399 | + "\n" 400 | + s 401 | + "_" * 100 402 | ) 403 | 404 | def preprocess_point_cloud(self, pcd, voxel_size): 405 | pcd_down = pcd 406 | bbox = o3d.geometry.AxisAlignedBoundingBox(min_bound=(-self.crop_cloud, -self.crop_cloud, -self.crop_cloud), 407 | max_bound=(self.crop_cloud, self.crop_cloud, self.crop_cloud)) 408 | pcd_down = pcd.crop(bbox) 409 | pcd_down = pcd_down.voxel_down_sample(voxel_size) 410 | # pcd_down = self.source.remove_ground_plane( 411 | # pcd, self.distance_threshold, self.ransac_n, self.num_iterations 412 | # ) 413 | radius_normal = voxel_size * 5 414 | pcd_down.estimate_normals( 415 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=100)) 416 | 417 | radius_feature = voxel_size * 5 418 | pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature( 419 | pcd_down, 420 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) 421 | return pcd_down, pcd_fpfh -------------------------------------------------------------------------------- /multi_lidar_calibrator/calibration/Geometry.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as R 3 | 4 | 5 | class Base: 6 | def __init__(self, x: float, y: float, z: float): 7 | """ 8 | Initialize with x, y, z translation or rotation. 9 | """ 10 | self.x = x 11 | self.y = y 12 | self.z = z 13 | 14 | def __add__(self, other): 15 | return self.__class__(self.x + other.x, self.y + other.y, self.z + other.z) 16 | 17 | def __sub__(self, other): 18 | return self.__class__(self.x - other.x, self.y - other.y, self.z - other.z) 19 | 20 | def __abs__(self): 21 | return self.__class__(*np.abs([self.x, self.y, self.z])) 22 | 23 | def __str__(self): 24 | return str(self.x) + " " + str(self.y) + " " + str(self.z) 25 | 26 | 27 | class Translation(Base): 28 | """Handles translation in 3D space.""" 29 | 30 | def __init__(self, x: float, y: float, z: float): 31 | """ 32 | Initialize with x, y, z coordinates. 33 | 34 | Parameters: 35 | x (float): x-coordinate 36 | y (float): y-coordinate 37 | z (float): z-coordinate 38 | """ 39 | super().__init__(x, y, z) 40 | 41 | def as_arr(self) -> np.ndarray: 42 | """ 43 | Return the translation as a numpy array. 44 | 45 | Returns: 46 | np.ndarray: Array of x, y, z coordinates 47 | """ 48 | return np.array([self.x, self.y, self.z], dtype=np.float64) 49 | 50 | 51 | class Rotation(Base): 52 | """Handles rotation around x, y, z axes in radians.""" 53 | 54 | def __init__(self, x: float, y: float, z: float, degrees=False): 55 | """ 56 | Initialize with x, y, z rotation angles. If degrees is True, the angles are converted to radians. 57 | 58 | Parameters: 59 | x (float): Rotation around x-axis 60 | y (float): Rotation around y-axis 61 | z (float): Rotation around z-axis 62 | degrees (bool): If True, the angles are in degrees. Otherwise, they are in radians. 63 | """ 64 | super().__init__(x, y, z) 65 | if degrees: 66 | self.x = np.deg2rad(x) 67 | self.y = np.deg2rad(y) 68 | self.z = np.deg2rad(z) 69 | 70 | @classmethod 71 | def from_quaternion(cls, quaternion: list) -> "Rotation": 72 | """ 73 | Create a Rotation object from a quaternion. 74 | 75 | Parameters: 76 | quaternion (list): Quaternion to convert 77 | 78 | Returns: 79 | Rotation: A new Rotation object initialized with the Euler angles derived from the quaternion 80 | """ 81 | euler = R.from_quat(quaternion).as_euler("xyz", degrees=False) 82 | return cls(*euler, False) # unpack euler angles 83 | 84 | def as_arr(self, degrees=False) -> np.ndarray: 85 | """ 86 | Return the rotation angles as a numpy array. If degrees is True, the angles are converted to degrees. 87 | 88 | Parameters: 89 | degrees (bool): If True, convert the angles to degrees 90 | 91 | Returns: 92 | np.ndarray: Array of rotation angles 93 | """ 94 | if not degrees: 95 | return np.array([self.x, self.y, self.z], dtype=np.float64) 96 | else: 97 | return np.rad2deg(self.as_arr(False)) 98 | 99 | def as_quaternion(self, degrees=False) -> np.ndarray: 100 | """ 101 | Return the rotation as a quaternion. If degrees is True, the angles are first converted to radians. 102 | 103 | Parameters: 104 | degrees (bool): If True, convert the angles to radians before converting to a quaternion 105 | 106 | Returns: 107 | np.ndarray: Quaternion representation of the rotation 108 | """ 109 | return R.from_euler("xyz", self.as_arr(degrees), degrees=degrees).as_quat() 110 | 111 | def __str__(self, degrees=False): 112 | if degrees: 113 | [x, y, z] = self.as_arr(True) 114 | return str(x) + " " + str(y) + " " + str(z) 115 | else: 116 | return str(self.x) + " " + str(self.y) + " " + str(self.z) 117 | 118 | 119 | class TransformationMatrix: 120 | """ 121 | Handles transformation matrices for 3D transformations. 122 | """ 123 | 124 | def __init__(self, translation: Translation, rotation: Rotation, degrees=False): 125 | """ 126 | Initialize a TransformationMatrix object. 127 | 128 | Args: 129 | translation: A Translation object representing the translation component of the transformation. 130 | rotation: A Rotation object representing the rotation component of the transformation. 131 | degrees: If True, the rotation angles are assumed to be in degrees and are converted to radians. 132 | If False, the rotation angles are assumed to be in radians. 133 | 134 | Returns: 135 | None 136 | """ 137 | self.translation = translation 138 | self.rotation = rotation 139 | self.matrix = np.zeros((4, 4), dtype=np.float64) 140 | self.matrix[:-1, 3] = translation.as_arr() 141 | self.matrix[3] = [0, 0, 0, 1] 142 | self.matrix[:-1, :-1] = R.from_euler( 143 | "xyz", rotation.as_arr(degrees), degrees=degrees 144 | ).as_matrix() 145 | 146 | @classmethod 147 | def from_matrix(cls, matrix: np.ndarray): 148 | """ 149 | Create a TransformationMatrix object from a 4x4 transformation matrix. 150 | 151 | Args: 152 | matrix: A 4x4 numpy array representing a transformation matrix. 153 | 154 | Returns: 155 | A TransformationMatrix object. 156 | """ 157 | translation = Translation(*matrix[:3, 3]) 158 | rotation = Rotation(*R.from_matrix(matrix[:3, :3].copy()).as_euler("xyz")) 159 | return cls(translation, rotation) 160 | -------------------------------------------------------------------------------- /multi_lidar_calibrator/calibration/Lidar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import open3d as o3d 3 | from geometry_msgs.msg import Transform 4 | 5 | from .Geometry import Rotation, TransformationMatrix, Translation 6 | 7 | 8 | class Lidar: 9 | """ 10 | Represents a LiDAR sensor with associated point cloud data and transformation matrices. 11 | """ 12 | 13 | def __init__(self, name: str, translation: Translation, rotation: Rotation): 14 | """ 15 | Initialize a Lidar object. 16 | 17 | Args: 18 | name: The name of the Lidar sensor. 19 | translation: A Translation object representing the translation component of the sensor's pose. 20 | rotation: A Rotation object representing the rotation component of the sensor's pose. 21 | 22 | Returns: 23 | None 24 | """ 25 | self.name = name 26 | self.translation = translation 27 | self.rotation = rotation 28 | self.tf_matrix = TransformationMatrix(translation, rotation) 29 | self.pcd = None 30 | self.pcd_transformed = None 31 | 32 | @classmethod 33 | def from_transform(cls, name: str, transform: Transform): 34 | """ 35 | Create a Lidar object from a ROS Transform message. 36 | 37 | Args: 38 | name: The name of the Lidar sensor. 39 | transform: A ROS Transform message representing the sensor's pose. 40 | 41 | Returns: 42 | A Lidar object. 43 | """ 44 | translation = Translation( 45 | transform.translation.x, transform.translation.y, transform.translation.z 46 | ) 47 | rotation = Rotation.from_quaternion( 48 | [transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w] 49 | ) 50 | return cls(name, translation, rotation) 51 | 52 | def read_pcd(self, path: str): 53 | """ 54 | Read a point cloud from a file and store it in the Lidar object. 55 | 56 | Args: 57 | path: The path to the point cloud file. 58 | 59 | Returns: 60 | None 61 | """ 62 | self.pcd = o3d.io.read_point_cloud(path) 63 | self.pcd_transformed = o3d.geometry.PointCloud(self.pcd) # Deep copy 64 | 65 | def load_pcd(self, pcd): 66 | """ 67 | Load a point cloud into the Lidar object. 68 | 69 | Args: 70 | pcd: An Open3D PointCloud object. 71 | 72 | Returns: 73 | None 74 | """ 75 | self.pcd = o3d.geometry.PointCloud(pcd) 76 | self.pcd_transformed = o3d.geometry.PointCloud(self.pcd) 77 | 78 | def remove_ground_plane( 79 | self, pcd=None, distance_threshold=0.1, ransac_n=3, num_iterations=1000 80 | ): 81 | """ 82 | Remove the ground plane from a point cloud using RANSAC. 83 | 84 | Args: 85 | pcd: An Open3D PointCloud object. If None, the point cloud of the Lidar object is used. 86 | distance_threshold: The distance threshold for the RANSAC algorithm. 87 | ransac_n: The number of points to sample for the RANSAC algorithm. 88 | num_iterations: The number of iterations for the RANSAC algorithm. 89 | 90 | Returns: 91 | An Open3D PointCloud object with the ground plane removed. 92 | """ 93 | if pcd is None: 94 | pcd = self.pcd 95 | plane_model, inliers = pcd.segment_plane(distance_threshold, ransac_n, num_iterations) 96 | outlier_cloud = pcd.select_by_index(inliers, invert=True) 97 | return outlier_cloud 98 | 99 | def calibrate_pitch(self, d_th=0.1, ransac_n=3, num_ransac_iter=1000, v_s=0.5, runs=5): 100 | """ 101 | Calibrate the pitch angle between the LiDAR and the ground plane. This method assumes that the yaw angle is accurate and ignores it. 102 | 103 | Args: 104 | d_th: The distance threshold for the RANSAC algorithm. 105 | ransac_n: The number of points to sample for the RANSAC algorithm. 106 | num_ransac_iter: The number of iterations for the RANSAC algorithm. 107 | v_s: The voxel size for downsampling the point cloud. If 0, no downsampling is performed. 108 | runs: The number of times to run the calibration. 109 | 110 | Returns: 111 | The calibrated roll and pitch angles. 112 | """ 113 | roll, pitch = np.zeros(runs), np.zeros(runs) 114 | for i in range(runs): 115 | pcd = o3d.geometry.PointCloud(self.pcd) 116 | if v_s > 0.0: 117 | pcd = pcd.voxel_down_sample(v_s) 118 | plane_model, inliers = pcd.segment_plane( 119 | distance_threshold=d_th, ransac_n=ransac_n, num_iterations=num_ransac_iter 120 | ) 121 | [a, b, c, d] = plane_model 122 | # Compute Euler angles (yaw is not applicable) 123 | roll[i] = np.arctan2(b, c) 124 | pitch[i] = -np.arctan2(a, np.sqrt(b**2 + c**2)) 125 | 126 | return [np.median(roll), np.median(pitch)] 127 | -------------------------------------------------------------------------------- /multi_lidar_calibrator/calibration/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/multi_lidar_calibrator/calibration/__init__.py -------------------------------------------------------------------------------- /multi_lidar_calibrator/calibration/points_horizontal.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/multi_lidar_calibrator/calibration/points_horizontal.pcd -------------------------------------------------------------------------------- /multi_lidar_calibrator/multi_lidar_calibrator.py: -------------------------------------------------------------------------------- 1 | import glob 2 | import os 3 | from time import time 4 | 5 | import numpy as np 6 | import open3d as o3d 7 | import rclpy 8 | import ros2_numpy as rnp 9 | from geometry_msgs.msg import Transform 10 | from rclpy.node import Node 11 | from sensor_msgs.msg import PointCloud2 12 | from tf2_msgs.msg import TFMessage 13 | 14 | from .calibration.Calibration import * 15 | 16 | 17 | def get_transfrom(tf_msg: TFMessage, child_frame_id: str) -> Transform: 18 | """ 19 | Extract the transform for a specific child frame from a TFMessage. 20 | 21 | Args: 22 | tf_msg: A TFMessage containing transforms. 23 | child_frame_id: The ID of the child frame for which to get the transform. 24 | 25 | Returns: 26 | The transform for the specified child frame, or None if the child frame is not in the TFMessage. 27 | """ 28 | for tf in tf_msg.transforms: 29 | if tf.child_frame_id == child_frame_id: 30 | return tf.transform 31 | return None 32 | 33 | 34 | class MultiLidarCalibrator(Node): 35 | """ 36 | A ROS node for calibrating multiple LiDAR sensors. 37 | 38 | This node subscribes to multiple LiDAR and /tf_static topics or reads the data from files 39 | and performs calibration using the Generalized Iterative Closest Point (GICP) algorithm. 40 | The calibration parameters can be adjusted using ROS parameters. 41 | """ 42 | 43 | def __init__(self): 44 | super().__init__("multi_subscriber_node") 45 | self.tf_topic = self.declare_parameter("tf_topic", "/tf_static").value 46 | self.visualize = self.declare_parameter("visualize", False).value 47 | self.use_fitness_based_calibration = self.declare_parameter( 48 | "use_fitness_based_calibration", False 49 | ).value 50 | self.read_tf_from_table = self.declare_parameter("read_tf_from_table", True).value 51 | self.table_degrees = self.declare_parameter("table_degrees", True).value 52 | self.topic_names = self.declare_parameter("lidar_topics", ["lidar_1, lidar_2"]).value 53 | self.target_lidar = self.declare_parameter("target_frame_id", "lidar_1").value 54 | self.base_frame_id = self.declare_parameter("base_frame_id", "base_link").value 55 | self.calibrate_target = self.declare_parameter("calibrate_target", False).value 56 | self.calibrate_to_base = self.declare_parameter("calibrate_to_base", False).value 57 | self.base_to_ground_z = self.declare_parameter("base_to_ground_z", 0.0).value 58 | self.frame_count = self.declare_parameter("frame_count", 1).value 59 | self.runs_count = self.declare_parameter("runs_count", 1).value 60 | self.crop_cloud = self.declare_parameter("crop_cloud", 25).value 61 | 62 | self.output_dir = ( 63 | os.path.dirname(os.path.realpath(__file__)) 64 | + self.declare_parameter("output_dir", "/../output/").value 65 | ) 66 | if not os.path.exists(self.output_dir): 67 | os.makedirs(self.output_dir) 68 | self.pcd_in_dir = ( 69 | os.path.dirname(os.path.realpath(__file__)) 70 | + self.declare_parameter("pcd_directory", "/../data/demo/").value 71 | ) 72 | 73 | self.rel_fitness = self.declare_parameter("rel_fitness", 1e-7).value 74 | self.rel_rmse = self.declare_parameter("rel_rmse", 1e-7).value 75 | self.max_iterations = self.declare_parameter("max_iterations", 100).value 76 | self.max_corresp_dist = self.declare_parameter("max_corresp_dist", 1.0).value 77 | self.epsilon = self.declare_parameter("epsilon", 0.005).value 78 | self.voxel_size = self.declare_parameter("voxel_size", 0.05).value 79 | self.r_voxel_size = self.declare_parameter("r_voxel_size", 0.15).value 80 | self.remove_ground_flag = self.declare_parameter("remove_ground_flag", False).value 81 | self.fitness_score_threshold = self.declare_parameter("fitness_score_threshold", 0.2).value 82 | self.distance_threshold = self.declare_parameter("distance_threshold", 0.1).value 83 | self.ransac_n = self.declare_parameter("ransac_n", 15).value 84 | self.num_iterations = self.declare_parameter("num_iterations", 5000).value 85 | self.r_runs = self.declare_parameter("r_runs", 5).value 86 | self.urdf_path = self.declare_parameter("urdf_path", "").value 87 | self.results_file = self.declare_parameter("results_file", "results.txt").value 88 | self.lidar_data = {} 89 | self.lidar_dict = {} 90 | self.subscribers = [] 91 | self.counter = 0 92 | self.read_pcds_from_file = self.declare_parameter("read_pcds_from_file", True).value 93 | with open(self.output_dir + self.results_file, "w") as file: # clean the file 94 | file.write("") 95 | self.tf_msg: TFMessage = None 96 | for topic in self.topic_names: 97 | self.subscribers.append( 98 | self.create_subscription(PointCloud2, topic, self.pointcloud_callback, 10) 99 | ) 100 | self.tf_subscriber = self.create_subscription( 101 | TFMessage, self.tf_topic, self.tf_callback, 10 102 | ) 103 | self.declared_lidars_flag = False 104 | 105 | # read all the data from files (without ROS) 106 | if self.read_pcds_from_file and self.read_tf_from_table: 107 | # get LiDAR names as subdirectory names 108 | lidar_list = [ 109 | os.path.splitext(os.path.basename(path))[0] 110 | for path in glob.glob(self.pcd_in_dir + "*") 111 | ] 112 | pcd_paths = dict(zip(lidar_list, [None] * len(lidar_list))) 113 | for lidar in lidar_list: 114 | self.declare_parameter(lidar, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) 115 | pcd_paths[lidar] = glob.glob(self.pcd_in_dir + lidar + "*") 116 | self.lidar_dict = dict( 117 | zip( 118 | lidar_list, 119 | [ 120 | Lidar( 121 | lidar, 122 | Translation(*self.get_parameter(lidar).value[0:3]), 123 | Rotation(*self.get_parameter(lidar).value[3:], True), 124 | ) 125 | for lidar in lidar_list 126 | ], 127 | ) 128 | ) 129 | for lidar in lidar_list: 130 | pcd = o3d.io.read_point_cloud(pcd_paths[lidar][0]) 131 | for i in range(1, self.frame_count): 132 | pcd += o3d.io.read_point_cloud(pcd_paths[lidar][i]) 133 | self.lidar_dict[lidar].load_pcd(pcd) 134 | self.process_data() 135 | exit(0) 136 | elif self.read_pcds_from_file and not self.read_tf_from_table: 137 | self.get_logger().error("read_tf_from_table is set to False") 138 | exit(1) 139 | else: 140 | self.get_logger().info("waiting for point cloud messages...") 141 | 142 | def log_calibration_info(self, calibration: Calibration): 143 | """Log calibration information in ROS and output file""" 144 | calibration_info = f"calibration info:\n{calibration.info(True, True)}" 145 | self.get_logger().info(calibration_info) 146 | with open(self.output_dir + self.results_file, "a") as file: 147 | file.write(calibration_info + "\n") 148 | 149 | def read_data(self): 150 | """Read point clouds from ROS and LiDAR initial transformation from either ROS or table.""" 151 | self.get_logger().info("Received all the needed point clouds") 152 | self.get_logger().info( 153 | "Reading initial transformations from tf and converting point clouds..." 154 | ) 155 | # create a dict of : whereas Lidar is created using data from 156 | # /tf_static or parameter table 157 | if self.read_tf_from_table: 158 | self.lidar_dict = dict( 159 | zip( 160 | self.lidar_data.keys(), 161 | [ 162 | Lidar( 163 | lidar, 164 | Translation(*self.get_parameter(lidar).value[0:3]), 165 | Rotation(*self.get_parameter(lidar).value[3:], True), 166 | ) 167 | for lidar in self.lidar_data.keys() 168 | ], 169 | ) 170 | ) 171 | else: 172 | self.lidar_dict = dict( 173 | zip( 174 | self.lidar_data.keys(), 175 | [ 176 | Lidar.from_transform(lidar, get_transfrom(self.tf_msg, lidar)) 177 | for lidar in self.lidar_data.keys() 178 | ], 179 | ) 180 | ) 181 | for key in self.lidar_data.keys(): 182 | # convert data from ros to pcd needed for open3d 183 | t = rnp.numpify(self.lidar_data[key][0]) 184 | pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(t["xyz"])) 185 | for i in range(1, self.frame_count): 186 | t = rnp.numpify(self.lidar_data[key][i]) 187 | pcd += o3d.geometry.PointCloud(o3d.utility.Vector3dVector(t["xyz"])) 188 | self.lidar_dict[key].load_pcd(pcd) 189 | self.get_logger().info("Converted all the needed ros-data") 190 | 191 | def standard_calibration(self, target_lidar: Lidar): 192 | """ 193 | Perform standard calibration using the Generalized Iterative Closest Point (GICP) algorithm. 194 | 195 | Args: 196 | target_lidar: The target LiDAR sensor to which other sensors will be calibrated. 197 | 198 | Returns: 199 | None 200 | """ 201 | calibrated_lidars, problematic_lidars = [], [] 202 | for source_lidar in list(self.lidar_dict.values()): 203 | if source_lidar.name == target_lidar.name: 204 | continue 205 | # Create a Calibration object and compute the GICP transformation 206 | calibration = Calibration( 207 | source_lidar, 208 | target_lidar, 209 | self.max_corresp_dist, 210 | self.epsilon, 211 | self.rel_fitness, 212 | self.rel_rmse, 213 | self.max_iterations, 214 | self.distance_threshold, 215 | self.ransac_n, 216 | self.num_iterations, 217 | self.crop_cloud, 218 | ) 219 | calibration.compute_gicp_transformation(self.voxel_size, self.remove_ground_flag) 220 | # Check the fitness score of the calibration 221 | if calibration.reg_p2l.fitness <= self.fitness_score_threshold: 222 | problematic_lidars.append(source_lidar) 223 | self.get_logger().info( 224 | f"Unsuccessful {source_lidar.name} calibration. " + "Trying other targets..." 225 | ) 226 | continue 227 | # Apply the transformation to the point cloud 228 | calibration.transform_pointcloud() 229 | # Log the calibration information 230 | self.log_calibration_info(calibration) 231 | # Modify the URDF file if a path is provided 232 | if self.urdf_path != "": 233 | modify_urdf_joint_origin( 234 | self.urdf_path, 235 | source_lidar.name + "_joint", 236 | calibration.calibrated_transformation, 237 | ) 238 | calibrated_lidars.append(source_lidar) 239 | 240 | # If there are problematic lidars, try to calibrate them to other lidars 241 | if len(problematic_lidars) > 0 and len(calibrated_lidars) > 0: 242 | # Create a combined point cloud from all successfully calibrated lidars 243 | combined_lidar = Lidar( 244 | target_lidar.name, target_lidar.translation, target_lidar.rotation 245 | ) 246 | combined_lidar.load_pcd( 247 | np.sum([lidar.pcd_transformed for lidar in calibrated_lidars]) + target_lidar.pcd 248 | ) 249 | for source_lidar in problematic_lidars: 250 | # Calibrate the problematic lidar to the combined lidar 251 | calibration = Calibration( 252 | source_lidar, 253 | combined_lidar, 254 | self.max_corresp_dist, 255 | self.epsilon, 256 | self.rel_fitness, 257 | self.rel_rmse, 258 | self.max_iterations, 259 | self.distance_threshold, 260 | self.ransac_n, 261 | self.num_iterations, 262 | self.crop_cloud, 263 | ) 264 | calibration.compute_gicp_transformation(self.voxel_size, self.remove_ground_flag) 265 | if calibration.reg_p2l.fitness <= self.fitness_score_threshold: 266 | self.get_logger().info(f"Problem with lidar {source_lidar.name} calibration") 267 | with open(self.output_dir + self.results_file, "a") as file: 268 | file.write("Calibration NOT SUCCESSFUL!\n") 269 | calibration.transform_pointcloud() 270 | self.log_calibration_info(calibration) 271 | if self.urdf_path != "": 272 | modify_urdf_joint_origin( 273 | self.urdf_path, 274 | source_lidar.name + "_joint", 275 | calibration.calibrated_transformation, 276 | ) 277 | 278 | elif len(problematic_lidars) > 0 and len(calibrated_lidars) == 0: 279 | self.get_logger().error( 280 | "calibration failed. Try choosing other target lidar " 281 | + ", providing more accurate initial transformation, " 282 | + "or using fitness based calibration (params)" 283 | ) 284 | 285 | def fitness_based_calibration(self, target_lidar: Lidar): 286 | """ 287 | Perform fitness-based calibration using the Generalized Iterative Closest Point (GICP) algorithm. 288 | 289 | This method first finds the source and target that result in the highest fitness score, performs the calibration, and saves the resulting cloud. Then, it finds a new calibration with the highest score (including the calibrated cloud). This process is repeated until all other LiDARs are calibrated to the target. 290 | 291 | Args: 292 | target_lidar: The target LiDAR sensor to which other sensors will be calibrated. 293 | 294 | Returns: 295 | None 296 | """ 297 | # Initialize lists for calibrated and problematic lidars 298 | calibrations_tmp = [] 299 | not_calibrated = list(self.lidar_dict.values()) 300 | 301 | # Compute GICP transformations for all pairs of lidars 302 | for source_lidar in not_calibrated: 303 | for other_lidar in not_calibrated: 304 | if source_lidar.name != other_lidar.name: 305 | calibration = Calibration( 306 | source_lidar, 307 | other_lidar, 308 | self.max_corresp_dist, 309 | self.epsilon, 310 | self.rel_fitness, 311 | self.rel_rmse, 312 | self.max_iterations, 313 | self.distance_threshold, 314 | self.ransac_n, 315 | self.num_iterations, 316 | self.crop_cloud, 317 | ) 318 | calibration.compute_gicp_transformation( 319 | self.voxel_size, self.remove_ground_flag 320 | ) 321 | calibrations_tmp.append(calibration) 322 | 323 | # Repeat until only the target lidar is left 324 | while not_calibrated != [target_lidar]: 325 | # Choose the calibration with the highest fitness score 326 | max_fitness_index = np.argmax( 327 | [calibration.reg_p2l.fitness for calibration in calibrations_tmp] 328 | ) 329 | calibration = calibrations_tmp[max_fitness_index] 330 | 331 | # If the fitness score is below the threshold, exit with an error 332 | if calibration.reg_p2l.fitness <= self.fitness_score_threshold: 333 | self.get_logger().error( 334 | f"no calibration within given threshold possible, " 335 | + "try reducing the thershold, make more overlap between the point clouds " 336 | + "or provide more accurate initial transformation" 337 | ) 338 | exit(1) 339 | 340 | # Don't transform the target lidar 341 | if calibration.source == target_lidar: 342 | calibration.source, calibration.target = calibration.target, calibration.source 343 | calibration.initial_transformation = TransformationMatrix.from_matrix( 344 | np.linalg.inv(calibration.initial_transformation.matrix) 345 | ) 346 | calibration.calibrated_transformation = TransformationMatrix.from_matrix( 347 | np.linalg.inv(calibration.calibrated_transformation.matrix) 348 | ) 349 | 350 | # Apply the transformation to the point cloud 351 | calibration.transform_pointcloud() 352 | 353 | # Combine the point clouds of the source and target lidars 354 | calibration.target.pcd += calibration.source.pcd_transformed 355 | 356 | # If the target lidar is the original target, remove the source from the list of not calibrated lidars 357 | if calibration.target == target_lidar: 358 | not_calibrated.remove(calibration.source) 359 | self.log_calibration_info(calibration) 360 | if self.urdf_path != "": 361 | modify_urdf_joint_origin( 362 | self.urdf_path, 363 | calibration.source.name + "_joint", 364 | calibration.calibrated_transformation, 365 | ) 366 | 367 | # Remove all calibrations involving the source lidar 368 | calibrations_tmp = [ 369 | c 370 | for c in calibrations_tmp 371 | if calibration.source != c.source and calibration.source != c.target 372 | ] 373 | 374 | # Recompute fitness scores for calibrations involving the target lidar 375 | for c in calibrations_tmp: 376 | if c.source == calibration.target or c.target == calibration.target: 377 | c.compute_gicp_transformation(self.voxel_size, self.remove_ground_flag) 378 | 379 | def process_data(self): 380 | """ 381 | Start the calibration pipeline. 382 | 383 | If calibrate_to_base and calibrate_target are set, the target LiDAR is first calibrated to the base frame. 384 | Then, other LiDARs are calibrated to the calibrated target LiDAR, i.e., to the base frame. 385 | 386 | If only LiDAR-to-LiDAR calibration is required, it can be performed without calibrating the target LiDAR to the base frame. 387 | 388 | Returns: 389 | None 390 | """ 391 | self.get_logger().info("Starting the calibration...") 392 | if self.visualize: 393 | visualize_calibration(list(self.lidar_dict.values()), False) 394 | target_lidar = self.lidar_dict[self.target_lidar] 395 | if self.calibrate_to_base and self.calibrate_target: 396 | # Perform target to ground (base) calibration. This computes the z-distance between the ground and the target 397 | # LiDAR and calibrates the pitch angle. It assumes that x, y, and yaw are precisely known 398 | # as well as the transformation between the base and ground. 399 | self.get_logger().info(f"Calibrating target lidar to the ground") 400 | roll, pitch = self.lidar_dict[self.target_lidar].calibrate_pitch( 401 | self.distance_threshold, 402 | self.ransac_n, 403 | self.num_iterations, 404 | self.r_voxel_size, 405 | self.r_runs, 406 | ) 407 | 408 | # Update the target lidar's rotation 409 | target_lidar.rotation.y = pitch 410 | rotation = target_lidar.rotation 411 | 412 | # Create a horizontal point cloud to use as a ground reference 413 | horizontal = Lidar(self.base_frame_id, Translation(0, 0, 0), Rotation(0, 0, 0)) 414 | dir_path = os.path.dirname(os.path.realpath(__file__)) 415 | horizontal.read_pcd(dir_path + "/calibration/points_horizontal.pcd") 416 | 417 | # Calibrate the target lidar to the ground 418 | calibration = Calibration( 419 | target_lidar, 420 | horizontal, 421 | self.max_corresp_dist, 422 | self.epsilon, 423 | self.rel_fitness, 424 | self.rel_rmse, 425 | self.max_iterations, 426 | self.crop_cloud, 427 | ) 428 | translation = target_lidar.translation 429 | calibration.compute_gicp_transformation(self.voxel_size, remove_ground_plane=False) 430 | 431 | # Update the target lidar's translation 432 | if self.read_tf_from_table: 433 | translation.z = ( 434 | calibration.calibrated_transformation.translation.z - self.base_to_ground_z 435 | ) 436 | else: 437 | translation.z = ( 438 | calibration.calibrated_transformation.translation.z 439 | - get_transfrom(self.tf_msg, self.base_frame_id).translation.z 440 | ) 441 | 442 | # Update the target lidar's transformation and point cloud 443 | calibration.calibrated_transformation = TransformationMatrix(translation, rotation) 444 | self.log_calibration_info(calibration) 445 | if self.urdf_path != "": 446 | modify_urdf_joint_origin( 447 | self.urdf_path, 448 | self.target_lidar + "_joint", 449 | calibration.calibrated_transformation, 450 | ) 451 | calibration.transform_pointcloud() 452 | 453 | # Reset the target lidar's transformation and update its point cloud 454 | self.lidar_dict.pop(self.target_lidar) 455 | target_lidar = Lidar(self.base_frame_id, Translation(0, 0, 0), Rotation(0, 0, 0)) 456 | target_lidar.load_pcd(calibration.source.pcd_transformed) 457 | self.lidar_dict[target_lidar.name] = target_lidar 458 | 459 | elif self.calibrate_to_base: 460 | # If the target to base transformation is already known, just transform the point cloud 461 | target_lidar.pcd.transform(target_lidar.tf_matrix.matrix) 462 | target_lidar.translation = Translation(0, 0, 0) 463 | target_lidar.rotation = Rotation(0, 0, 0) 464 | 465 | # Perform the main LiDAR-to-LiDAR calibration 466 | if self.use_fitness_based_calibration: 467 | self.fitness_based_calibration(target_lidar) 468 | else: 469 | self.standard_calibration(target_lidar) 470 | 471 | visualize_calibration(list(self.lidar_dict.values()), True, not self.visualize) 472 | # Stitch point clouds 473 | stitched_pcd = o3d.geometry.PointCloud() 474 | for lidar in self.lidar_dict.values(): 475 | stitched_pcd += lidar.pcd 476 | # Save stitched point clouds to a .pcd file 477 | o3d.io.write_point_cloud(self.output_dir + "stitched_initial.pcd", stitched_pcd) 478 | # Create a point cloud for the transformed data 479 | stitched_pcd_transformed = o3d.geometry.PointCloud() 480 | for lidar in self.lidar_dict.values(): 481 | stitched_pcd_transformed += lidar.pcd_transformed 482 | # Save the transformed point clouds to a .pcd file 483 | o3d.io.write_point_cloud( 484 | self.output_dir + "stitched_transformed.pcd", stitched_pcd_transformed 485 | ) 486 | self.get_logger().info(f"Saved fused point cloud: {self.output_dir}") 487 | self.get_logger().info(f"Calibrations results are stored in: {self.output_dir}") 488 | 489 | def tf_callback(self, msg): 490 | self.get_logger().info("Received TFMessage data") 491 | self.tf_msg = msg 492 | 493 | def pointcloud_callback(self, msg: PointCloud2): 494 | # Wait for the TFMessage before processing the point cloud if table is not used 495 | if self.tf_msg is None and not self.read_tf_from_table: 496 | self.get_logger().info("Waiting for tf...") 497 | return 498 | 499 | # Add the point cloud to the lidar_data dictionary 500 | if msg.header.frame_id not in self.lidar_data.keys(): 501 | self.lidar_data[msg.header.frame_id] = [msg] 502 | else: 503 | if len(self.lidar_data[msg.header.frame_id]) >= self.frame_count: 504 | return # Don't save more point clouds than needed 505 | self.lidar_data[msg.header.frame_id].append(msg) 506 | 507 | # Process the data after receiving the required number of point clouds for each LiDAR 508 | if [len(self.lidar_data[i]) == self.frame_count for i in self.lidar_data.keys()] == [ 509 | True 510 | ] * len(self.topic_names): 511 | if self.read_tf_from_table and not self.declared_lidars_flag: 512 | for lidar in self.lidar_data.keys(): 513 | self.declare_parameter(lidar) 514 | self.declared_lidars_flag = True # Don't repeatedly declare the same parameters 515 | begin = time() 516 | self.read_data() 517 | self.process_data() 518 | end = time() 519 | with open(self.output_dir + self.results_file, "a") as file: 520 | file.write(f"Complete calibration time: {end - begin}\n") 521 | self.lidar_data = {} # Clean the data after each calibration (for multiple runs) 522 | self.counter += 1 523 | if self.counter >= self.runs_count: 524 | exit(0) 525 | 526 | 527 | def main(args=None): 528 | rclpy.init(args=args) 529 | node = MultiLidarCalibrator() 530 | rclpy.spin(node) 531 | node.destroy_node() 532 | rclpy.shutdown() 533 | 534 | 535 | if __name__ == "__main__": 536 | main() 537 | -------------------------------------------------------------------------------- /output/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/output/.gitkeep -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | multi_lidar_calibrator 5 | 0.0.1 6 | Multi - LiDAR-to-LiDAR calibration framework for ROS 2 and non-ROS applications 7 | Andrii Chumak 8 | Dominik Kulmer 9 | LGPL-3.0 license 10 | 11 | rclpy 12 | std_msgs 13 | 14 | ros2launch 15 | 16 | 17 | ament_python 18 | 19 | 20 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | open3d 2 | scipy 3 | ros2_numpy 4 | pandas 5 | setuptools==58.2 6 | -------------------------------------------------------------------------------- /resource/multi_lidar_calibrator: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/Multi_LiCa/5e6de27de53f71c9c36dbe118f12d94148fa17ce/resource/multi_lidar_calibrator -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/multi_lidar_calibrator 3 | [install] 4 | install_scripts=$base/lib/multi_lidar_calibrator 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | 4 | from setuptools import setup 5 | 6 | package_name = "multi_lidar_calibrator" 7 | submodules = "multi_lidar_calibrator/calibration" 8 | 9 | setup( 10 | name=package_name, 11 | version="0.0.1", 12 | packages=[package_name, submodules], 13 | data_files=[ 14 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 15 | ("share/" + package_name, ["package.xml"]), 16 | (os.path.join("share", package_name, "launch"), glob("launch/*.py")), 17 | (os.path.join("share", package_name, "config"), glob("config/*.yaml")), 18 | ], 19 | install_requires=["setuptools"], 20 | zip_safe=True, 21 | maintainer="Andrii Chumak, Dominik Kulmer", 22 | maintainer_email="ge65wex@mytum.de, dominik.kulmer@tum.de", 23 | description="Multi - LiDAR-to-LiDAR calibration framework for ROS 2 and non-ROS applications", 24 | license="LGPL-3.0 license", 25 | tests_require=["pytest"], 26 | entry_points={ 27 | "console_scripts": [ 28 | "multi_lidar_calibrator = multi_lidar_calibrator.multi_lidar_calibrator:main", 29 | ], 30 | }, 31 | ) 32 | --------------------------------------------------------------------------------