Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras

Related tags

Miscellaneous mlcc
Overview

Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras

The pre-print version of our paper is available here. The pre-release code has been uploaded. Our experiment video is availalbe on YouTube and Bilibili.

Introduction

In this paper, we propose a fast, accurate, and targetless extrinsic calibration method for multiple LiDARs and cameras based on adaptive voxelization. On the theory level, we incorporate the LiDAR extrinsic calibration with the bundle adjustment method. We derive the second-order derivatives of the cost function w.r.t. the extrinsic parameter to accelerate the optimization. On the implementation level, we apply the adaptive voxelization to dynamically segment the LiDAR point cloud into voxels with non-identical sizes, and reduce the computation time in the process of feature correspondence matching.

Fig. 1 Dense colorized point cloud reconstructed with LiDAR poses and extrinsic calibrated with our proposed method.

Adaptive Voxelization

In both LiDAR-LiDAR and LiDAR-camera extrinsic calibration, we implement adaptive voxelization to accelerate the feature correspondence matching process. The point cloud map is dynamically segmented into voxels with non-identical sizes, such that only one plane feature is contained in each voxel. This process sufficiently saves the execution time of k-d tree searching from our previous work1 (see Fig. 2) and work2 (see Fig. 3).

Fig. 2 Adaptive voxelization in LiDAR-LiDAR extrinsic calibration.

Fig. 3 Adaptive voxelization in LiDAR-camera extrinsic calibration. A) real world image. B) raw point cloud of this scene. C) voxelization of previous work where the yellow circles indicate the false edge estimation. D) edges extracted with our proposed method.

1. Prerequisites

1.1 ROS and Ubuntu

Our code has been tested on Ubuntu 16.04 with ROS Kinetic and Ubuntu 18.04 with ROS Melodic.

1.2 Ceres Solver

Our code has been tested on ceres solver 1.14.x.

1.3 OpenCV

Our code has been tested on OpenCV 3.4.14.

1.4 Eigen

Our code has been tested on Eigen 3.3.7.

1.5 PCL

Our code has been tested on PCL 1.8.

2. Build and Run

Clone the repository and catkin_make:

cd ~/catkin_ws/src
git clone [email protected]:hku-mars/mlcc.git
cd .. && catkin_make
source ~/catkin_ws/devel/setup.bash

3. Run Our Example

The parameters base LiDAR (AVIA or MID), test scene (scene-1 or scene-2), adaptive_voxel_size, etc., could be modified in the corresponding launch file.

3.1 LiDAR-LiDAR Extrinsic Calibration

Step 1: base LiDAR pose optimization (the initial pose is stored in scene-x/original_pose)

roslaunch mlcc pose_refine.launch

Step 2: LiDAR extrinsic optimization (the initial extrinsic is stored in config/init_extrinsic)

roslaunch mlcc extrinsic_refine.launch

Step 3: pose and extrinsic joint optimization

roslaunch mlcc global_refine.launch

3.2 LiADR-Camera Extrinsic Calibration

roslaunch mlcc calib_camera.launch

4. Run Your Own Data

To test on your own data, you need to transform the point cloud into .dat format (see source/tobinary.cpp for more details). Please only collect the point cloud and images when the LiDAR (sensor platform) is not moving for the optimal precision (or segment them from a complete rosbag). The base LiDAR poses and initial extrinsic values shall also be provided. These initial values could be obtained by general SLAM and Hand-eye calibration algorithms.

You may need to modify the parameters adaptive_voxel_size, feat_eigen_limit and downsampling_size for LiDAR-LiDAR extrinsic calibration to adjust the precision and speed. You need to change the corresponding path and topic name in the yaml files in the config folder.

5. Known Issues

Currently, we seperate the LiDAR extrinsic calibration process into three steps for debug reasons. In future release, we wish to combine them together to make it more convenient to use.

6. License

The source code is released under GPLv2 license.

We are still working on improving the performance and reliability of our codes. For any technical issues, please contact us via email [email protected] and [email protected].

For commercial use, please contact Dr. Fu Zhang [email protected].

Comments
  • run extrinsic_refine.launch error

    run extrinsic_refine.launch error

    When I follow the guide, run roslaunch mlcc pose_refine.launch, it`s fine。After that, I run roslaunch mlcc extrinsic_refine.launch, some errors occur, below are some msg, btw, my env is ubuntu18.04, ros is melodic.thanks for your reply.

    setting /run_id to 6b384e2a-355c-11ec-850e-8cec4b814673 process[rosout-1]: started with pid [7627] started core service [/rosout] process[extrinsic_refine-2]: started with pid [7634] process[rviz-3]: started with pid [7635]

    iteration 0 extrinsic_refine: /home/admini//ws3/src/mlcc/include/extrinsic_refine.hpp:308: void EXTRIN_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' failed.

    opened by levenJun 5
  • Error for calib_camera.launch

    Error for calib_camera.launch

    hello! Thanks for your excellent works !!

    When I follow the guide, I run roslaunch mlcc calib_camera.launch, some errors occur, below are some msg. my env is ubuntu18.04, ros is melodic.

    [ INFO] [1645761068.171630692]: Camera 0 Configuration Camera Matrix: [863.590518437255, 0.116814496662654, 621.666074631063; 0, 863.100180533059, 533.971978652819; 0, 0, 1] Distortion Coeffs: [-0.0944205499243979; 0.09467276777765039; -0.00807970960613932; 8.07461209775283e-05; 6.527166468758789e-05] Extrinsic Params: [0, -1, 0, 0; 0, 0, -1, 0; 1, 0, 0, 0; 0, 0, 0, 1] Rotation error:0 [ INFO] [1645761068.172541533]: Camera 1 Configuration Camera Matrix: [863.081687640302, 0.176140650303666, 628.941349825503; 0, 862.563371991533, 533.00290953509; 0, 0, 1] Distortion Coeffs: [-0.0943795554942897; 0.09829982415249131; -0.0125418048527694; 0.000456235380677041; -8.73113795357082e-05] Extrinsic Params: [0, -1, 0, 0; 0, 0, -1, 0; 1, 0, 0, 0; 0, 0, 0, 1] Rotation error:0 total ext_number:3 [calib_camera-2] process has died [pid 7304, exit code -9, cmd /home/A/ros_ws/catkin_ws_mlcc/devel/lib/mlcc/calib_camera /home/A/ros_ws/catkin_ws_mlcc/src/mlcc/config/left.yaml /home/A/ros_ws/catkin_ws_mlcc/src/mlcc/config/right.yaml /home/A/ros_ws/catkin_ws_mlcc/src/mlcc/config/avia_stereo.yaml /home/A/ros_ws/catkin_ws_mlcc/src/mlcc/result __name:=calib_camera __log:=/home/A/.ros/log/29b15922-95ee-11ec-ad7f-a8a15920a9d6/calib_camera-2.log]. log file: /home/A/.ros/log/29b15922-95ee-11ec-ad7f-a8a15920a9d6/calib_camera-2*.log

    Is there any config or setting that I need to change? thanks for your reply. Thanks

    opened by JangChangWon 4
  • Question about vpnp cost function

    Question about vpnp cost function

    Dear author, thanks for your excellent work, but I am puzzled by the vpnp cost function, I did not understand why we need the line direction to compute the matrix V, and the physical meaning of this formula VRV.transpose a9dbc88dd7d6bdc163c148b62cf3a50 Look forward to your reply

    opened by RoseWillow 4
  • !std::isnan(residual2)

    !std::isnan(residual2)

    hello, I met a problem with this 20220615-012739 it's in the first step pose_refine.launch.I have read the details about this same phenomenon but It doesn't work can you give me some advice

    opened by debuleilei 3
  • calibration of one camera and one lidar

    calibration of one camera and one lidar

    Thanks for your excellent work. I want to use the code to calibrate one camera and one lidar. Can you give me some advice about how to change the code?

    opened by zzkslam 3
  • calibration of multi-lidar and camera

    calibration of multi-lidar and camera

    Thanks for your excellent work. I want to use the code to calibrate multi lidar and two- camera for my custom robot Can you give me some advice about how to make the .dat file such as src/mlcc/scene0/00/patch0.dat?

    opened by JangChangWon 2
  • quastion about your voxel: does it only for livox lidar:

    quastion about your voxel: does it only for livox lidar:

    Dear authors: In your project,I found that loc_xyz sames like must more than 0. And,i found the livox lidar's frame is like this,x>0,y>0,z>0. but ,I want using it for robosens lidar . If I need modify this param? thanks for your help.


      for (int j = 0; j < 3; j++)
    	{
    		loc_xyz[j] = pt_trans[j] / voxel_size;
    		if (loc_xyz[j] < 0)
    			loc_xyz[j] -= 1.0;
    	}
    

    opened by mingkunwxc 2
  • problem:   extrinsic_refine.hpp:309:** void EXTRIN_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' failed.

    problem: extrinsic_refine.hpp:309:** void EXTRIN_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' failed.

    i used the scen* from your project to test this program,i got the problem:

    ———————————————————————————————————————————— iteration 0 residual0: 5.352935 nan u: 0.010000 v: 2.000000 q: nan -nan nan extrinsic_refine: ******/rrrlive_/mlcc/src/mlcc/include/extrinsic_refine.hpp:309: void EXTRIN_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' failed. —————————————————————————————————————————————————— how can i fix it?

    opened by mingkunwxc 2
  • Pose_refine reported the following error: Assertion `!std::isnan(residual2)'

    Pose_refine reported the following error: Assertion `!std::isnan(residual2)'

    Thanks for your excellent work. When I converted the KITTI360 data into the required format for pose optimization, the problem of "Assertion !std::isnan(residual2)" was reported After I conducted a series of tests, I found that when the number of frames I used was less than or equal to 35, there was no problem with Pose_refine (0.pcd~34.pcd). But as long as the number of frames is greater than 35, the error will be reported.

    process[pose_refine-1]: started with pid [26669]
    process[rviz-2]: started with pid [26676]
    ---------------------
    iteration 0
    pose_refine: /home/ly/catkin_ws/src/mlcc/include/pose_refine.hpp:272: void LM_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' faile
    d.
    [pose_refine-1] process has died [pid 26669, exit code -6, cmd /home/ly/catkin_ws/devel/lib/mlcc/pose_refine __name:=pose_refine __log:=/home
    /ly/.ros/log/f4dda018-eeec-11ec-a53c-04d4c45d3faf/pose_refine-1.log].
    log file: /home/ly/.ros/log/f4dda018-eeec-11ec-a53c-04d4c45d3faf/pose_refine-1*.log
    
    opened by Yurri-hub 1
  • Extrinsic refine - NaN residual

    Extrinsic refine - NaN residual

    Hi Team. I'm having hard time to replicate your experiment on your data (scene1 and secene2). I'm working inside ros:melodic based docker. I've installed your tested dependencies (Eigen, Ceres, OpenCV). I have no errors during compilation, just few warnings.

    First stage (pose_refine) works fine. But I am not able to successfully run second stage (extrinsic_refine) program stops on first iteration on NaN assert on line extrinsic_refine.hpp:308:

    iteration 0
    residual0: 2.757675 nan u: 0.010000 v: 2.000000 q: nan -nan nan
    extrinsic_refine: /home/robo/catkin_ws/src/mlcc/include/extrinsic_refine.hpp:308: void EXTRIN_OPTIMIZER::optimize(): Assertion `!std::isnan(residual2)' failed.
    

    Final stage then works fine. Final camera - lidar alignment eats all of my 32GB Ram a crashed on std::bad_alloc. Same as mentioned before: here

    I am thankful for any advice how to make this calibration working.

    opened by Trvnik 1
  • Livox Mid-70 and IMU Calibration

    Livox Mid-70 and IMU Calibration

    Hi Team,

    Great work. I am struggling with the calibration of the Livox Mid-70 and IMU. Which package or process would you guys suggest to tackle this issue ? Thanks!

    opened by h-rover 1
  • Question about lidar-camera extrinsic calibration problem and how to build residuals.

    Question about lidar-camera extrinsic calibration problem and how to build residuals.

    Excellent work!

    I have two questions that I can't figure out.

    1. I guess E_C is as close to 0 as better, so why there is not a square or absolute operation in the below formula? 2022-06-23 15-00-42 的屏幕截图

    2. After watching Question about vpnp cost function #2, i still can't figure out the residuals' building process below, can you show me more detail or give me more reference? 2022-06-23 15-08-18 的屏幕截图

    (3. What's vPnP?) Looking forward to your reply, thanks!!!

    opened by TheCuriousJoe 0
Owner
HKU-Mars-Lab
Mechatronics and Robotic Systems (MaRS) Laboratory
HKU-Mars-Lab
image_projection is a ROS package to create various projections from multiple calibrated cameras.

image_projection Overview image_projection is a ROS package to create various projections from multiple calibrated cameras. Use cases involve: Rectify

Technische Universität Darmstadt ROS Packages 115 Sep 19, 2022
ROS package to calibrate the extrinsic parameters between LiDAR and Camera.

lidar_camera_calibrator lidar_camera_calibrator is a semi-automatic, high-precision, feature-based camera and LIDAR extrinsic calibration tool. In gen

Networked Robotics and Sytems Lab 63 Sep 1, 2022
A real-time, direct and tightly-coupled LiDAR-Inertial SLAM for high velocities with spinning LiDARs

LIMO-Velo [Alpha] ?? [16 February 2022] ?? The project is on alpha stage, so be sure to open Issues and Discussions and give all the feedback you can!

Andreu Huguet 128 Sep 22, 2022
Fix some extrinsic parameter importing problems. 6-axis IMU works now. Lidar without ring works now.

LVI-SAM-MODIFIED This repository is a modified version of LVI-SAM. Modification Add function to get extrinsic parameters.The original code assumes the

null 77 Sep 28, 2022
Lidar-with-velocity - Lidar with Velocity: Motion Distortion Correction of Point Clouds from Oscillating Scanning Lidars

Lidar with Velocity A robust camera and Lidar fusion based velocity estimator to undistort the pointcloud. This repository is a barebones implementati

ISEE Research Group 139 Sep 21, 2022
Solve 20 simple math questions and see how accurate/fast you are!

Math-Quiz Solve 20 simple math questions and see how accurate/fast you are! Want to try? Clone this repository $ git clone https://github.com/Mini-War

Mini Ware 8 Sep 11, 2022
BLEND: A Fast, Memory-Efficient, and Accurate Mechanism to Find Fuzzy Seed Matches

BLEND is a mechanism that can efficiently find fuzzy seed matches between sequences to significantly improve the performance and accuracy while reducing the memory space usage of two important applications: 1) finding overlapping reads and 2) read mapping.

SAFARI Research Group at ETH Zurich and Carnegie Mellon University 13 Sep 27, 2022
This repository is used for automatic calibration between high resolution LiDAR and camera in targetless scenes.

livox_camera_calib livox_camera_calib is a robust, high accuracy extrinsic calibration tool between high resolution LiDAR (e.g. Livox) and camera in t

HKU-Mars-Lab 428 Sep 30, 2022
A generic and robust calibration toolbox for multi-camera systems

MC-Calib Toolbox described in the paper "MultiCamCalib: A Generic Calibration Toolbox for Multi-Camera Systems". Installation Requirements: Ceres, Boo

null 168 Oct 3, 2022
the implementations of 'A Flexible New Technique for Camera Calibration' and Bouguet's method

StereoCameraCalibration MonocularCameraCalibration/StereoCameraCalibration/StereoCameraRectification 1、Class "MonocularCameraCalibration" provides the

gtc1072 7 Sep 23, 2022
Project to create a teensy based gamecube controller with hall effect sensors, snapback filtering, and notch calibration

PhobGCC Gamecube controller motherboard using a teensy as the microcontroller. Aim is to make an accessible and consistent controller. Has the option

null 87 Sep 29, 2022
This package estimates the calibration parameters that transforms the camera frame (parent) into the lidar frame (child)

Camera-LiDAR Calibration This package estimates the calibration parameters that transforms the camera frame (parent) into the lidar frame (child). We

Australian Centre for Field Robotics 185 Sep 29, 2022
Dwm_lut - Apply 3D LUTs to the Windows desktop for system-wide color correction/calibration

About This tool applies 3D LUTs to the Windows desktop by hooking into DWM. It works in both SDR and HDR modes, and uses tetrahedral interpolation on

null 177 Oct 3, 2022
Fuses IMU readings with a complementary filter to achieve accurate pitch and roll readings.

SimpleFusion A library that fuses accelerometer and gyroscope readings quickly and easily with a complementary filter. Overview This library combines

Sean Boerhout 5 Aug 22, 2022
RRxIO - Robust Radar Visual/Thermal Inertial Odometry: Robust and accurate state estimation even in challenging visual conditions.

RRxIO - Robust Radar Visual/Thermal Inertial Odometry RRxIO offers robust and accurate state estimation even in challenging visual conditions. RRxIO c

Christopher Doer 60 Sep 19, 2022
VID-Fusion: Robust Visual-Inertial-Dynamics Odometry for Accurate External Force Estimation

VID-Fusion VID-Fusion: Robust Visual-Inertial-Dynamics Odometry for Accurate External Force Estimation Authors: Ziming Ding , Tiankai Yang, Kunyi Zhan

ZJU FAST Lab 79 Sep 6, 2022
Accurate C port of AHX's Amiga replayer

ahx2play Aims to be an accurate C port of AHX 2.3d-sp3's internal replayer. The project contains example code in the ahx2play folder on how to interfa

Olav Sørensen 10 Sep 27, 2022
Aims to be an accurate C port of Impulse Tracker 2.15's IT replayer (with selectable IT2 sound drivers)

it2play Aims to be an accurate C port of Impulse Tracker 2.15's IT replayer (with selectable IT2 sound drivers). This is a direct port of the original

Olav Sørensen 18 Sep 27, 2022
Allows for multiple SwitchBot buttons and curtains to be controlled via MQTT sent to ESP32. ESP32 will send BLE commands to switchbots and return MQTT responses to the broker. Also supports Meter/Temp Sensor

SwitchBot-MQTT-BLE-ESP32 Switchbot local control using ESP32. no switchbot hub used/required. works with any smarthub that supports MQTT https://githu

null 305 Oct 2, 2022