IMU-based Online Multi-lidar Calibration

TL;DR

This paper introduces a reliable, markerless, IMU-based method for online multi-lidar extrinsic calibration on vehicles, improving accuracy without odometry or fiducial markers, validated on real test vehicle data.

Contribution

The paper presents a novel IMU-based approach for online multi-lidar calibration that does not require odometry or fiducial markers, using feature matching and observability criteria.

Findings

Successful validation on Scania test vehicles

Improved calibration accuracy without odometry

Efficient subset selection using mutual information

Abstract

Modern autonomous systems typically use several sensors for perception. For best performance, accurate and reliable extrinsic calibration is necessary. In this research, we propose a reliable technique for the extrinsic calibration of several lidars on a vehicle without the need for odometry estimation or fiducial markers. First, our method generates an initial guess of the extrinsics by matching the raw signals of IMUs co-located with each lidar. This initial guess is then used in ICP and point cloud feature matching which refines and verifies this estimate. Furthermore, we can use observability criteria to choose a subset of the IMU measurements that have the highest mutual information -- rather than comparing all the readings. We have successfully validated our methodology using data gathered from Scania test vehicles.

Tables

Table 1. TABLE I :

Performance comparison at different configurations: IMU test board dataset
Configuration	$\Delta x\left[m\right]$	$\Delta y\left[m\right]$	$\Delta z\left[m\right]$	$\Delta roll{[}^{\circ }]$	$\Delta pitch{[}^{\circ }]$	$\Delta yaw{[}^{\circ }]$
${30}^{\circ }$	-0.0500	-0.0461	0.0015	-0.3268	-2.3129	-0.8808
${45}^{\circ }$	-0.0950	0.1018	0.0018	-0.4305	-2.2219	-1.2211
${90}^{\circ }$	-0.0003	0.0146	-0.0065	-0.3174	-2.3144	-0.0562

Table 2. TABLE II :

Scania dataset collection details for the experiments
Data	Scenario	Length ($\mathrm{Km}$)	Duration ($\mathrm{secs}$)	Feature richness	APE
Seq-1	8-pattern in parking	0.325	72.5	Low	2.390
Seq-2	Slow turning	0.333	128.7	Medium	2.152
Seq-3	Urban driving	0.439	78.1	High	1.886
Seq-4	Urban driving	2.209	281.4	High	5.793

Table 3. TABLE III :

Performance comparison of calibration routines: Scania dataset
Sequence	Method	$\Delta x\left[m\right]$	$\Delta y\left[m\right]$	$\Delta z\left[m\right]$	$\Delta roll{[}^{\circ }]$	$\Delta pitch{[}^{\circ }]$	$\Delta yaw{[}^{\circ }]$
Seq-1	Kabsch	0.367	0.099	-0.309	-1.533	-2.647	-5.744
	CROON†	-0.844	-0.959	-0.340	-1.707	-2.783	1.918
	Ours⋆	-0.164	-0.184	-0.153	-1.415	-2.759	-0.592
Seq-2	Kabsch	0.734	-0.822	-0.342	-1.654	-2.860	-1.019
	CROON†	-0.134	-0.099	-0.178	-1.761	-2.722	0.265
	Ours⋆	0.187	0.043	-0.183	-2.105	-2.562	-1.889
Seq-3	Kabsch	0.675	-0.392	0.440	-2.474	-1.940	0.083
	CROON†	0.310	-0.030	0.081	-3.387	-1.762	-1.710
	Ours⋆	0.261	0.095	-0.072	0.057	-1.561	-0.159
Seq-4	Kabsch	1.260	-0.339	0.870	1.784	-4.947	0.304
	CROON†	0.494	0.403	0.054	-1.036	-1.447	-4.654
	Ours⋆	0.118	-0.112	-0.059	0.438	-1.267	0.138
⋆ Only translation initialization needed, † Both translation and rotation initialization needed.

Equations

$$\left[\begin{array}[]{c}\boldsymbol{\omega}_{\mathtt{I}_{t}}\\ \mathbf{a}_{\mathtt{I}_{t}}\end{array}\right]=\left[\begin{array}[]{c}\boldsymbol{\hat{\boldsymbol{\omega}}_{\mathtt{I}_{t}}}\\ \hat{\mathbf{a}}_{\mathtt{I}_{t}}-\mathbf{R}_{\mathtt{W}\mathtt{I}_{t}}\mathbf{g}_{\mathtt{W}}\end{array}\right]+\left[\begin{array}[]{l}\mathbf{b}_{\mathtt{I}_{t}}^{\omega}\\ \mathbf{b}_{\mathtt{I}_{t}}^{a}\end{array}\right]+\left[\begin{array}[]{c}\boldsymbol{\eta}_{\mathtt{I}_{t}}^{\omega}\\ \boldsymbol{\eta}_{\mathtt{I}_{t}}^{a}\end{array}\right].$$

$${\mathbf{q}}_{\mathtt{B}{\mathtt{I}}_t}=\left[\cos \left(\frac{\Delta t}{2}\Vert {\mathbf{\omega }}_{{\mathtt{I}}_t}\Vert \right){\mathbf{I}}_4+\frac{\sin \left(\frac{\Delta t}{2}\Vert {\mathbf{\omega }}_{{\mathtt{I}}_t}\Vert \right)}{\Vert {\mathbf{\omega }}_{{\mathtt{I}}_t}\Vert }{\mathbf{\Omega }}_{{\mathtt{I}}_t}\right]{\mathbf{q}}_{\mathtt{B}{\mathtt{I}}_{t-1}}$$

$$\boldsymbol{\Omega}_{\mathtt{I}_{t}}=\left[\begin{array}[]{cccc}0&{\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{z}}&-\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{y}&{\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{x}}\\ -\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{z}&0&{\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{x}}&{\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{y}}\\ {\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{y}}&-\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{x}&0&{\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{z}}\\ -\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{x}&-\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{y}&-\left[\boldsymbol{\omega}_{\mathtt{I}_{t}}\right]_{z}&0\end{array}\right].$$

$$\hat{\mathbf{q}}_{\mathtt{{B}}\mathtt{I}_{t}}=\operatorname*{arg\,min}_{\mathbf{q}_{\mathtt{{B}}\mathtt{I}_{t}}\in\mathbb{R}^{4\times 1}}\left(\begin{array}[]{l}\tilde{\mathbf{q}}_{\mathtt{{B}}\mathtt{I}_{t}}\otimes{\mathbf{R}_{\mathtt{{B}}\mathtt{{W}}}^{-1}}\mathbf{g}_{\mathtt{W}}\otimes\mathbf{q}_{\mathtt{{B}}\mathtt{I}_{t}}-\hat{\mathbf{a}}_{\mathtt{I}_{t}}\\ \end{array}\right),$$

$$\mathtt{Rest\ detected}=\|\begin{array}[]{l}\tilde{\hat{\mathbf{q}}}_{\mathtt{{B}}\mathtt{I}_{t}}\otimes\mathbf{g}_{\mathtt{{B}}}\otimes\hat{\mathbf{q}}_{\mathtt{{B}}\mathtt{I}_{t}}-\hat{\mathbf{a}}_{\mathtt{I}_{t}}\end{array}\|_{2}\leq\tau$$

$${\mathbf{b}}^a$$

$${\mathbf{\Sigma }}_a$$

$$⟹{\hat{\mathbf{a}}}_{{\mathtt{I}}_t}$$

$$\text{System State},{\hat{\mathbf{x}}}_0$$

$$\text{Measurement Noise},{\mathbf{W}}_0$$

$$\text{Process Noise},\mathbf{Q}$$

$$\text{Initial Covariance},{\mathbf{P}}_0$$

$${\mathbf{x}}_t$$

$${\mathbf{y}}_t$$

$${\mathbf{P}}_t$$

$${\mathbf{K}}_t$$

$${\hat{\mathbf{x}}}_t$$

$${\mathbf{P}}_t$$

$${\mathbf{R}}_{\mathtt{B}\mathtt{I}}^{\star }$$

$$\text{s.t.}{\mathbf{R}}_{\mathtt{B}\mathtt{I}}{\mathbf{R}}_{\mathtt{B}\mathtt{I}}^T={\mathbf{I}}_3,$$

$${({\mathbf{R}}_{\mathtt{B}\mathtt{I}}^{\star })}^{-1}{\hat{\mathbf{a}}}_{\mathtt{I}}$$

$$={\hat{\mathbf{a}}}_{\mathtt{B}}+[{\hat{\mathbf{\omega }}}_{\mathtt{B}}{]}_{\times }^2{\mathbf{t}}_{\mathtt{B}\mathtt{I}}+[{\dot{\hat{\mathbf{\omega }}}}_{\mathtt{B}}{]}_{\times }{\mathbf{t}}_{\mathtt{B}\mathtt{I}},$$

$$\displaystyle\mathbf{t}_{\mathtt{{B}}\mathtt{I}}^{\star}=\operatorname*{arg\,min}_{\mathbf{t}_{\mathtt{{B}}\mathtt{{I}}}}\sum_{i=1}^{N}\leavevmode\resizebox{294.85841pt}{}{$\|\underbrace{(\left[\boldsymbol{\hat{\omega}_{\mathtt{{B}}_{i}}}\right]^{2}_{\times}+[\dot{\hat{\boldsymbol{\omega}}}_{\mathtt{{B}}_{i}}]_{\times})}_{\mathbf{A}_{i}}\mathbf{t}_{\mathtt{{B}}\mathtt{I}}-\underbrace{(\mathbf{R}^{-1\star}_{\mathtt{{B}}\mathtt{{I}}}\hat{\boldsymbol{a}}_{\mathtt{{I}}_{i}}-\hat{\boldsymbol{a}}_{\mathtt{{B}}_{i}})}_{\mathbf{B}_{i}}\|^{2},$}$$

$${\mathbf{x}}^{\star }=\underset{\mathbf{x}}{\mathrm{argmin}}\Vert \mathbf{Ax}-\mathbf{B}{\Vert }_{\mathbf{\Sigma }}^2,$$

$${\mathbf{\Delta x}}^{\star }=\underset{\mathbf{\Delta x}}{\mathrm{argmin}}\frac{1}{2}\Vert (\mathbf{A}\hat{\mathbf{x}}-\mathbf{B})+\mathbf{A}\mathbf{\Delta x}{\Vert }_{\mathbf{\Sigma }}^2.$$

$$\underset{\text{Fisher information matrix}}{\underbrace{\left({\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}\mathbf{J}\right)}}\mathbf{\Delta x}=-{\mathbf{J}}^{\top }{\mathbf{\Sigma }}^{-1}(\mathbf{A}\hat{\mathbf{x}}-\mathbf{B}).$$

$${\mathbf{x}}^{\star }=\underset{\mathbf{L}\le \mathbf{x}\le \mathbf{U}}{\mathrm{argmin}}\Vert \mathbf{Ax}-\mathbf{B}{\Vert }_{\mathbf{\Sigma }}^2,$$

$${\mathbf{\Delta x}}^{\star }=\underset{\mathbf{\Delta x}}{\mathrm{argmin}}\frac{1}{2}\Vert (\mathbf{A}\hat{\mathbf{x}}-\mathbf{B})+\mathbf{A}\mathbf{\Delta x}{\Vert }_{\mathbf{\Sigma }}^2,$$

$$\text{s.t.}\mathbf{L}\le \hat{\mathbf{x}}+\mathbf{\Delta x}\le \mathbf{U}.$$

$$\underset{\text{Fisher information matrix}}{\underbrace{\left(\sum _i{\mathbf{J}}_i^{\top }{\Sigma }_i^{-1}{\mathbf{J}}_i\right)}}\mathbf{\Delta }{\mathbf{R}}_{\mathtt{B}\mathtt{I}}\stackrel{Eq.\text{ref}eq:bvl{s}_{i}terativ{e}_{s}olution}{=}-\sum _i{\mathbf{J}}_i^{\top }{\Sigma }_i^{-1}({\hat{\mathbf{R}}}_{\mathtt{B}\mathtt{I}}{\mathbf{\omega }}_{{\mathtt{B}}_i}-{\mathbf{\omega }}_{{\mathtt{I}}_i})$$

$${\mathcal{I}}_{N\times N}={\text{USU}}^T,$$

$${\mathcal{F}}_{{\mathtt{L}}^{(k)}}^L$$

$${\mathcal{F}}_{{\mathtt{L}}^{(k)}}^P$$

$$\alpha$$

$$\delta$$

$${\hat{\mathbf{R}}}_{\mathtt{Refined}}$$

$$=\underset{{\mathbf{R}}_{\mathtt{B}\mathtt{L}}}{\mathrm{argmin}}\sum _{i=1}^{N}2\left[1-\mathrm{Tr}\left({\mathbf{R}}_{\mathtt{B}\mathtt{L}}{\hat{\mathbf{n}}}_{{\mathtt{B}}_i}{\hat{\mathbf{n}}}_{{\mathtt{L}}_i}^T\right)\right].$$

$$\displaystyle\leavevmode\resizebox{377.24727pt}{}{$J(\mathbf{R}_{\mathtt{{B}}\mathtt{L}})\stackrel{{\scriptstyle Eq.~{}\ref{eq:optimal_rotation}}}{{=}}-\operatorname{Tr}\left[\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\sum_{i=1}^{N}\left(\hat{\mathbf{n}}_{\mathtt{{B}}_{i}}\hat{\mathbf{n}}_{\mathtt{L}_{i}}^{T}\right)\right]=-\operatorname{Tr}\left[\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{\Delta}_{3\times 3}\right]$}.$$

Full text

IMU-based Online Multi-lidar Calibration

Sandipan Das1,2, Bengt Boberg2, Maurice Fallon3, Saikat Chatterjee1 1 KTH EECS, Sweden. {sandipan,sach}@kth.se

2 Scania, Sweden. {sandipan.das, bengt.boberg}@scania.com

3 ORI, University of Oxford, UK. mfallon@robots.ox.ac.uk

4\urlhttps://github.com/mrsandipandas/imu-calibration

Modern autonomous systems typically use several sensors for perception. For best performance, accurate and reliable extrinsic calibration is necessary. In this research, we propose a reliable technique for the extrinsic calibration of several lidars on a vehicle without the need for odometry estimation or fiducial markers. First, our method generates an initial guess of the extrinsics by matching the raw signals of IMUs co-located with each lidar. This initial guess is then used in ICP and point cloud feature matching which refines and verifies this estimate. Furthermore, we can use observability criteria to choose a subset of the IMU measurements that have the highest mutual information — rather than comparing all the readings. We have successfully validated our methodology using data gathered from Scania test vehicles.

I Introduction

For safe navigation and sensor redundancy, autonomous vehicles need numerous sensors to ensure $360^{\circ}$ sensing coverage. For precise fusion, it is crucial to establish the accurate mounting location for each of the vehicle’s sensors — a process known as extrinsic calibration. Without the proper extrinsic calibration parameters, it is impossible to fuse the sensing data into a single reference frame. In this work, we concentrated on multi-lidar system extrinsic calibration, though the same principle might also be applied to camera or radar extrinsic calibration.

There are two main types of multi-lidar extrinsic calibration algorithms: target-based and target-less. Target-based calibration refers to putting fiducial markers in the shared field-of-view (FoV) of the lidars and mutually matching the markers. Target-less calibration is carried out by comparing the estimated states of the sensors for initialization and refined using feature-matching. In another set of target-less methods, both state and the calibration parameters are estimated by solving a joint optimization step where the reprojection error between the tracked features is minimized. Since the sensors may not have an overlapping FoV, target-less calibration is preferable as it is more general than target-based methods. It is widely recognized that not all motion segments produce enough observable information for matching states used for the initialization of extrinsic calibration. Because of this identifying any degenerate motion segments is also important to discard information that is not helpful for extrinsic calibration initialization.

I-A Motivation

As seen in Fig. 1 and Fig. 2, the mobile platforms we employed for our studies have multiple lidars. Therefore, it is essential to confirm that the sensors are correctly calibrated prior to any autonomous run. To properly excite the different degrees of freedom, motion-based calibration requires aggressive driving maneuvers which can cause state estimation drift. Moreover, state estimation itself adds additional complexity to the whole tool chain and it does not provide independent verification for the estimated extrinsics. Finally, we also need a process to verify the quality of the estimated extrinsics.

I-B Contribution

We present the following contributions:

• •

A novel observability-aware, target-less extrinsic calibration algorithm to calibrate multiple lidars using co-located IMUs without the need for any lidar odometry estimation and extrinsics rotation initialization.

• •

Online extrinsics refinement based on line feature matching and online bias compensation of the IMU signals to improve the quality of the extrinsic calibration algorithm.

• •

Verification of our method in real-time based on plane feature association with datasets collected from Scania test vehicles in urban driving scenarios.

• •

Our implementation for the IMU-IMU calibration with observability analysis on a custom-made test board and sample datasets is open-sourced on Github4.

II Related Work

Extrinsic sensor calibration is a well studied area. In our discussion, we briefly review the relevant literature and motivate our choice of methods.

II-A Target-based calibration

Target-based extrinsic calibration is performed by matching known markers or fiducials placed in the common FoV of multiple sensors. Researchers have explored target-based between camera and lidar using known geometric shapes like checkerboards, triangles, and diamonds [1] or geometric feature correspondences [2]. To calibrate a multi-lidar system, geometric elements including points, lines, and planes were retrieved and matched in [3, 4]. The most important aspect of a target-based system is there being overlapping FoV.

II-B Target-less calibration

Target-less extrinsic calibration based on the Hand-Eye method [5, 6, 7] is an extensively studied topic. The term “Hand-Eye” refers to early motion-based calibration research that estimated the motion of the gripper (hand) and the camera (eye) while constricting their poses with a fixed rigid body transformation. Multi-camera extrinsic calibration with Hand-Eye method has been explored in works like Camodocal [8] and recently extended to multi-lidar extrinsic calibration [9].

Another technique for target-less calibration is pose alignment with Kabsch method [10], where the poses are obtained from lidar odometry [11, 12] in the corresponding sensor frame. Recent works like CROON [13] and adaptive voxelization [14] improve the pose alignment with additional feature matching techniques. In Kalibr [15] the authors automatically detected sets of measurements from which they could identify an observable parameter space and then performed a maximum likelihood estimate (MLE) by minimizing the errors between landmark observations and their known correspondences. They also discarded parameter updates for numerically unobserved directions and degenerate scenarios. In [16], a global bundle adjustment was performed to minimize the reprojection error between the tracked features to estimate the camera-lidar extrinsics. A similar principle can be applied for multiple lidar extrinsic calibration if the sensors have an overlapping FoV. However, this method would still require lidar odometry estimates, which is a computationally expensive process.

Because we have integrated IMUs within our lidars (with factory calibrations), we aim to use them to create a multi-IMU (MIMU) configuration to estimate initial extrinsics estimates without the need for any pose alignment. Kalibr was extended towards MIMU calibration [17] by matching poses fitted to a B-spline and further constraining motion with known image landmarks. In MIMC-VINS [18], an efficient multi-state constraint Kalman filter was used to jointly propagate all IMU states while enforcing rigid body constraints between the IMUs during the filter update stage, which produced the MIMU calibration. Unlike prior works, we used the fundamental property that a rigid body’s angular velocity is constant around every point of the body to formulate our MIMU calibration. We used MLE to match the raw angular velocity between the IMU(s) embedded within the lidar(s) in accordance with the signal-to-signal match principle [19].

We then selected measurements that have contain sufficient signal excitation which is known as observability-aware criteria [20, 21, 22, 23]. We then maximized mutual information between the angular velocity signals to identify relevant motion segments. This method can be extended to other modalities and does not require overlapping FoV between the sensors.

III Problem Statement

III-A Sensor platform and reference frames

We evaluate our proposed method using data collected from a Scania test vehicle. The sensor platform of the vehicle with its illustrative sensor FoV and corresponding reference frames is shown in Fig. 1 and Fig. 2 respectively. Each sensor housing contains a lidar with an embedded IMU and two cameras. Note that the cameras were not used in this work.

Now we describe the necessary notation and reference frames used in our system. The vehicle base frame is denoted as $\mathtt{{B}}$ and the world frame is denoted as $\mathtt{W}$. Sensor readings from the lidars and IMUs are represented in their respective sensor frames as $\mathtt{L}^{(k)}$, and $\mathtt{I}^{(k)}$ respectively, where, $k\in[\mathtt{F}_{\mathtt{L}}-\mathtt{Top},\mathtt{F}_{\mathtt{R}}-\mathtt{Top}]$ denotes the location of the sensor in the vehicle corresponding to front-left-top and front-right-top respectively. In our discussions, the transformation matrix is denoted as, \mathbf{T}=\left[\begin{array}[]{ll}\mathbf{R}_{3\times 3}&\mathbf{t}_{3\times 1}\\ \mathbf{0}^{\top}&1\end{array}\right]\in\mathrm{SE}(3) and $\mathbf{T}_{\mathtt{A}\mathtt{B}}$ denotes the transformation matrix of frame $\mathtt{B}$ wrt frame $\mathtt{A}$.

III-B Problem formulation

The primary goal is to estimate the extrinsic calibration between multiple lidar sensors, $\mathbf{T}_{\mathtt{L}^{(k_{i})}\mathtt{L}^{(k_{j})}}$, in real-time without the need for any calibration targets or odometry estimation. The system is initialized ($\mathbf{T}_{\mathtt{init}}$) only with rough translation component parameters from the vehicle and sensor CAD models. Finally, the extrinsics are also verified in real-time to ensure robustness.

IV Methodology

Our method starts by matching the noise compensated IMU signals between the base frame, $\mathtt{{B}}$, and the IMU frame, $\mathtt{I}^{(k)}$, which estimates $\hat{\mathbf{T}}_{\mathtt{IMU}}$. Then a GICP-based [24] point cloud alignment is performed with the initial estimate to compute $\hat{\mathbf{T}}_{\mathtt{GICP}}$, which is further refined by feature matching to estimate $\hat{\mathbf{T}}_{\mathtt{Refined}}$. An additional online verification is performed to ensure the correctness of the calibration process. Thus, $\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}\simeq\hat{\mathbf{T}}_{\mathtt{Refined}}\times\hat{\mathbf{T}}_{\mathtt{GICP}}\times\hat{\mathbf{T}}_{\mathtt{IMU}}\times\mathbf{T}_{\mathtt{init}}$. For notional brevity we consider one of the lidars to be base and discuss a general calibration routine between the base frame, $\mathtt{{B}}$, and the lidar frame, $\mathtt{L}^{(k)}$.

IV-A IMU-based initial estimate

IV-A1 IMU initialization

The IMU measurements are in their corresponding sensor frame. For gravity alignment, we use equations eq. 25, 26 from [25] to estimate roll and pitch and obtain $\mathtt{R}_{\mathtt{W}\mathtt{{B}}}$ after collecting IMU data when the vehicle is static for a few seconds.

IV-A2 IMU sensor model

We have considered a 6-DoF (degree of freedom) IMU such that it has a 3-axis accelerometer and 3-axis gyroscope. The IMU sensor data in its corresponding sensor frame can be represented as,

[TABLE]

Here, $[\boldsymbol{\omega}_{\mathtt{I}_{t}}\in\mathbb{R}^{3\times 1},\mathbf{a}_{\mathtt{I}_{t}}\in\mathbb{R}^{3\times 1}]$ represents the measured angular velocity and linear acceleration at timestamp $t$ and $[\hat{\boldsymbol{\omega}}_{\mathtt{I}_{t}},\hat{\mathbf{a}}_{\mathtt{I}_{t}}]$ represent the latent ideal angular velocity and linear acceleration respectively. $\mathbf{b}_{\mathtt{I}_{t}}^{\omega}\in\mathbb{R}^{3\times 1}$ and $\mathbf{b}_{\mathtt{I}_{t}}^{a}\in\mathbb{R}^{3\times 1}$ represent the gyro and accelerometer biases which change with time and other factors like temperature. $\boldsymbol{\eta}_{\mathtt{I}_{t}}^{\omega}\sim\mathcal{N}(0,\boldsymbol{\Sigma}_{\omega})$ and $\boldsymbol{\eta}_{\mathtt{I}_{t}}^{a}\sim\mathcal{N}(0,\boldsymbol{\Sigma}_{a})$ are the additive zero-mean white Gaussian noises for gyroscope and accelerometer with covariance $\boldsymbol{\Sigma}_{\omega}\in\mathbb{R}^{3\times 3}$ and $\boldsymbol{\Sigma}_{a}\in\mathbb{R}^{3\times 3}$ respectively. $\mathbf{g}_{\mathtt{W}}\in\mathbb{R}^{3\times 1}$, represents the gravity vector in the world frame, $\mathtt{W}$, and $\mathbf{R}_{\mathtt{{B}}\mathtt{W}}$ represent the gravity alignment rotation matrix.

IV-A3 IMU bias characterization

IMU biases are estimated by collecting data sequences at rest. The accelerometer bias is unchanged till the next rest state detection whereas the gyro bias is tracked online with a Kalman filter [26]. The biases are recomputed whenever a rest state is detected which limits the bias covariance growth.

IMU state propagation

The IMU dynamical model based on angular velocity in quaternion form is shown as,

[TABLE]

and,

[TABLE]

$\Delta t$ denotes the sampling period of the IMU data. We used the Madgwick filter [27] to estimate the refined rotation which minimizes the difference between the measured acceleration and the aligned gravity vector using a gradient descent algorithm as,

[TABLE]

where, $\tilde{\mathbf{q}}$ represents the quaternion conjugate and $\otimes$ is the quaternion product operator. Note that we use the compensated accelerometer signals after bias compensation which is discussed in IV-A3.

Rest state

The rest state is detected if the difference between the norm of the aligned gravity vector and the acceleration vector is less than a predefined threshold, $\tau$ for at least more than 2 seconds. Thus,

[TABLE]

Accelerometer bias estimation

The accelerometer bias is estimated by computing the mean of the signal at rest state and the standard deviation gives us the co-variance of the white Gaussian noise. We recompute these parameters whenever the rest state is detected and keep them unchanged until the next rest state detection. If the rest period duration is $N$ seconds then,

[TABLE]

Gyro bias estimation

The initialization of the gyro bias is carried out in a similar fashion as we did for the accelerometer and recomputed whenever the rest state is detected. Thus,

[TABLE]

After that we use a Kalman filter to track the gyroscope bias as a state. The system is modeled by,

[TABLE]

The standard Kalman filter update equations become,

[TABLE]

Thus after estimating the biases we can rearrange eq. 1 to compute $\hat{\boldsymbol{\omega}}_{\mathtt{I}_{t}}$.

IV-A4 IMU signal matching

Rotation estimate

The extrinsics of $\mathtt{L}^{(k)}$ wrt $\mathtt{{B}}$ frame is closely related to with the extrinsics of $\mathtt{I}^{(k)}$ wrt $\mathtt{{B}}$ frame as they are co-located with known extrinsics from sensor supplier. A rigid body has uniform angular velocity throughout the body. The angular velocities of all the IMU sensors in our sensor arrangement must be equal if their orientations are the same because they are all securely affixed to the vehicle. Let, [$\hat{\boldsymbol{\omega}}_{\mathtt{I}_{i}^{(k)}}$, $\hat{\mathbf{a}}_{\mathtt{I}_{i}^{(k)}}$] and [$\hat{\boldsymbol{\omega}}_{\mathtt{{B}}_{i}}$, $\hat{\mathbf{a}}_{\mathtt{{B}}_{i}}$] be the estimated angular velocity and linear acceleration of the $k^{th}$ IMU and the base IMU at timestamp $t_{i}$ after bias compensation. By removing the $k^{th}$ superscript for brevity, our optimization problem becomes,

[TABLE]

which, can be simply solved with Kabsch alignment [10].

Translation estimate

For the acceleration components we account for the Coriolis forces as illustrated in Fig. 3 and equate the translation components as,

[TABLE]

where, $[\boldsymbol{.}]_{\times}$ is a skew-symmetric matrix and $[\boldsymbol{a}]_{\times}\boldsymbol{b}=\boldsymbol{a}\times\boldsymbol{b}$.

Since, we have already computed $\mathbf{R}^{\star}_{\mathtt{{B}}\mathtt{I}}$, our optimization problem for the translation component becomes,

[TABLE]

where, $\mathbf{A}=[\mathbf{A}_{0}\ \mathbf{A}_{1}\ \dots\ \mathbf{A}_{n}]^{T}$, $\mathbf{B}=[\mathbf{B}_{0}\ \mathbf{B}_{1}\ \dots\ \mathbf{B}_{n}]^{T}$ and $\mathbf{x}=\mathbf{t}_{\mathtt{{B}}\mathtt{I}}$ $\forall i\in[1,N]$. This is a system of linear equations of the form, $\mathbf{A}{\mathbf{x}}=\mathbf{B}$, which can be solved using a least-square approach as,

[TABLE]

where, ${\boldsymbol{\Sigma}}$ denotes the co-variance of the residual. The main approach used to tackle these problems [28] involves solving a series of approximations to the original problem repeatedly by linearizing as, $F(\mathbf{x}+\Delta\mathbf{x})\approx F(\mathbf{x})+\mathbf{J}(\mathbf{x})\Delta\mathbf{x}$, where, $J$ being the jacobian of $F(\mathbf{x})$. In this way $\hat{\mathbf{x}}$ is updated in the current iteration as, $\hat{\mathbf{x}}\leftarrow\hat{\mathbf{x}}\boxplus\mathbf{\Delta x}$, where $\boxplus$ is an addition operator in the manifold and the problem becomes,

[TABLE]

The optimal solution is given by,

[TABLE]

In practice, since we can take a reliable initial estimate for the translation component from the CAD parameters (within $\approx$10 cm), we search for $\mathbf{x}^{\star}$ only in a local neighborhood within reasonable bounds. Thus, we solve a bounded variable least squares problem as,

[TABLE]

where, $\mathbf{L}$ and $\mathbf{U}$ are the lower and upper bounds of $\mathbf{x}$ respectively. Thus the solution space in Eq. 14 is modified with an additional constraint as,

[TABLE]

Observability analysis

Not all vehicle motions are suitable for exciting the degrees of freedom to allow calibration. As a result, it’s important to identify suitable motion segments for information-aware calibration updates. We captured this information by comparing the angular velocity between $\mathtt{I}^{(k)}$ and $\mathtt{{B}}$ frame based on Eq.10 which can also be solved iteratively with the update step as,

[TABLE]

where, ${\boldsymbol{\Sigma}_{i}}=\operatorname{cov}(\hat{\mathbf{\mathbf{R}}}_{\mathtt{{B}}\mathtt{{I}}}\boldsymbol{\omega}_{\mathtt{{B}}_{i}}-\boldsymbol{\omega}_{\mathtt{I}_{i}})$ and $\mathbf{J}_{i}=\boldsymbol{\omega}_{\mathtt{{B}}_{i}}^{T}$. The Fisher information matrix, $\mathcal{I}_{N\times N}$ captures all the information contained in the measurements. We do the processing after arranging the data in a batch size of $N$. We perform a Singular Value Decomposition of $\mathcal{I}_{N\times N}$ for each batch as:

[TABLE]

where, $\textbf{U}=[\textbf{u}_{1},\textbf{u}_{2}\dots,\textbf{u}_{N}]$ and $\textbf{S}=\text{diag}(\sigma_{1},\sigma_{2}\dots,\sigma_{N})$ is a diagonal matrix of singular values in decreasing order. Information about the data in the batch is indicated by the value of the minimal singular value. If the minimal singular value exceeds a certain threshold (design decision), we may say that there is sufficient excitation in the batch of data to allow for extrinsics computation and hence chosen for calibration.

IV-B Point cloud based alignment

Initial match

Based on our sensor setup we first filtered the point clouds with a box filter to retain the region comprising of maximum overlap between the two point clouds for faster processing. For our setup, we filtered the point clouds between $[0,-10,-10]m$ and $[50,10,10]m$. Then the initial guess from the IMU-based matching was used as a prior for the GICP-based [24] matching on the filtered point clouds.

Line and plane feature extraction and matching

After obtaining improved extrinsic parameters from GICP we further refined the extrinsics using line feature extraction and matching. First, segmentation [11] and local curvature [29] of each point in the filtered clouds are evaluated. The points with the highest and lowest curvature are allocated to the sets of line and plane clouds, respectively. Then, we fit the line and plane model to the corresponding line and plane cloud sets using PROSAC [30] which exploits the spatial coherence of the line and plane points. Thus, for each lidar point cloud, we obtained the line and plane feature set as ${\cal F}^{L}$ and ${\cal F}^{P}$, where a line and plane are modeled as,

[TABLE]

where, $\hat{\mathbf{n}}$ and $\hat{\mathbf{c}}$ denotes the estimated direction and center of the line and plane respectively. Finally, additional filtering is performed where only the closest line and planes between the set of different point clouds are kept for further refinement and verification. Two lines or planes are considered a close match if,

[TABLE]

Extrinsic refinement

For refinement, we chose $N$ set of corresponding line features from the point clouds for which $\tau_{\alpha}^{(l)}=5^{\circ}$ and $\tau_{\delta}^{(l)}=0.5m$. By removing the $k$ superscript for brevity, the optimal rotation can be computed by solving the following optimization,

[TABLE]

We formulate our problem as a Q-method and convert the cost function based on rotation matrices to quaternion form as discussed in our earlier work [23] as,

[TABLE]

The attitude quaternion which minimizes the cost function is the unit eigenvector corresponding to the largest eigenvalue of $\mathbf{K}$ (eigenvalues of a symmetric matrix are real and hence can be sorted). Thus,

[TABLE]

where, \mathbf{K}=\left[\begin{array}[]{cc}\mathbf{\Gamma}-\mu\mathbf{I}&\mathbf{\Lambda}\\ \mathbf{\Lambda}^{T}&\mu\end{array}\right], \mathbf{\Lambda}=\left[\begin{array}[]{ccc}\mathbf{\Delta}(2,3)-\mathbf{\Delta}(3,2)\\ \mathbf{\Delta}(3,1)-\mathbf{\Delta}(1,3)\\ \mathbf{\Delta}(1,2)-\mathbf{\Delta}(2,1)\end{array}\right], $\mathbf{\Gamma}=\mathbf{\Delta}^{T}+\mathbf{\Delta}$ and $\mu=\operatorname{Tr}(\mathbf{\Delta})$.

The optimal translation is obtained by matching the line centers as,

[TABLE]

Extrinsics verification

After estimating the refined extrinsics we obtained the final extrinsic parameters as $\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}\simeq\hat{\mathbf{T}}_{\mathtt{Refined}}\times\hat{\mathbf{T}}_{\mathtt{GICP}}\times\hat{\mathbf{T}}_{\mathtt{IMU}}\times\mathbf{T}_{\mathtt{init}}$. At this stage, we applied the transformation to the point clouds to orient them to a common reference frame and extract plane features. If for the corresponding plane features, $\tau_{\alpha}^{(p)}=1^{\circ}$ and $\tau_{\delta}^{(p)}=0.3m$, we accept the final extrinsic calibration, otherwise, we recompute the whole point-based alignment.

V Experimental Results

We carried out a series of experiments to test different aspects of algorithm — starting from a basic sensor rig and building up to a full demonstration on a test vehicle.

We first carried out MIMU calibration by mounting 2 IMUs on a custom made planar board. After this we collected sensor data from the test vehicle and computed the baseline extrinsics, where the (lidar) odometry was estimated using Fast-LIO2 [12] and matched using Kabsch alignment [10].

We then benchmark our results against CROON [13] where ground plane extraction and ICP (with normal) was performed between corresponding lidar frames and averaged our results across all the frames in the scene. Finally, we performed our calibration experiments with IMU-based initialization. The results are presented as the difference between ground truth (GT) and estimated extrinsics.

V-A MIMU calibration on a test rig

We created the planar test rig seen in Fig. 4 to verify the MIMU calibration technique. The board contained 2 Xsens MTi-300 IMUs along with GNSS receivers to ensure time synchronization. $\mathtt{IMU-B}$ was fixed to the board while we could adjust the pose of $\mathtt{IMU-A}$ clockwise to three different orientations — $30^{\circ},45^{\circ}$ and $90^{\circ}$. We collected IMU data at each orientation and computed the extrinsics by matching the IMU signals as discussed in IV-A4. We measured the GT translation components between the IMU centers using a measuring tape. The results are shown in Table I and an illustration of the signals before and after calibration of the $45^{\circ}$ configuration are shown in Fig. 5 and Fig. 6 respectively. We see that after the calibration steps the IMU signals show good qualitative alignment when plotted in the same reference frame.

V-B Lidar extrinsic calibration using IMU-based initialization on Scania datasets

After verifying the basic MIMU calibration approach on the test rig, we used the same method to pre-initialize our lidar extrinsic calibration algorithm. The GT for the extrinsic calibration parameters was pre-computed offline. We analyzed our lidar calibration results on 4 different test sequences which are described in Table II, covering different motion scenarios. We consider the feature richness level to be low if there are very few ($\leq$ 3) geometric features detected per frame on average. In Seq-1, we performed aggressive maneuvers to excite all possible degrees of freedom in a parking lot. Seq-2 consists of very slow driving (\approx$$10\text{\,}\mathrm{km}\text{/}\mathrm{h}) in a feature rich environment.

Seq-3 and Seq-4 contain data from an urban driving scenario (\approx$$40\text{\,}\mathrm{km}\text{/}\mathrm{h}) in normal traffic conditions. We also computed the lidar odometry using FAST-LIO2 [12], for all the sequences and computed the RMSE of the absolute pose error (APE) between the lidars using GT extrinsics. This helped us to create the baseline for the extrinsics computation using Kabsch alignment. The high APE supports our claim that because the pose alignment technique has inherent drift and computationally heavy, it does not bring any additional value to the target-less extrinsics estimation process other than just a rough initialization.

V-C Bias estimation results

To improve our calibration performance, which relies on the signal-to-signal matching policy, we compensate for the biases in both the accelerometer and angular velocity signals. When a period of rest is identified, the accelerometer biases are recomputed and held constant for the following period. The angular velocity biases are updated based on the estimated orientation using the Madgwick filter that uses the bias-compensated accelerometer signals for gravity alignment. Whenever a period of rest is detected the covariances of the angular velocity biases converge.

In Seq-1, we didn’t have any periods of rest. Because of this, the angular velocity bias covariances did not converge as seen in Fig. 7. Whereas, in Fig. 8, we see that the angular velocity bias covariances converge after the detection of the rest period for a few seconds at the beginning of Seq-4.

V-D Observability analysis

To study observability analysis, we extracted the information matrix by comparing the angular velocity signals between the IMU, $\mathtt{I}^{(k)}$, and the $\mathtt{{B}}$ frame by dividing the data into equal segments of 10 sec each. We analyze the SVD of the Fisher information matrix from Eq.18 and select the IMU data in the segment for calibration if the minimum singular value is greater than a threshold. For our experimental dataset, we set the threshold as $5^{-10}$.

As seen in Fig. 9(b), we selected all segments (highlighted in green) as there was enough excitation in the angular velocity during all the time in Seq-1 as the vehicle was driving in a figure-of-8 pattern. The corresponding selected trajectory segments are shown in Fig. 9(a). Similarly, for Seq-4, we can see that in Fig. 10(a), relevant trajectories for calibration are selected only when there are strong turns maneuvers are observed in Fig. 10(b) as this is when there is enough excitation of the different degrees of freedom. This analysis helped us to understand that the best possible maneuvers for IMU-based initialization must contain aggressive turns like 8-patterns.

V-E Online calibration experiments

GT calibration of the lidars is obtained by refining the vehicle CAD parameters. The refinement process analyzes known static feature positions around the vehicle in the world frame, $\mathtt{W}$, and matches the corresponding detected features from the lidar perspective, using $\mathbf{T}_{\mathtt{W}\mathtt{B}}$ from GNSS. After that, we obtain the extrinsics of the lidars to the base frame, $\mathtt{{B}}$ and compute the transformation matrix, $\mathbf{T}_{\mathtt{G}\mathtt{T}}=\mathbf{T}^{-1}_{\mathtt{{B}}\mathtt{L}^{(\mathtt{F}_{\mathtt{L}}-\mathtt{Top})}}\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(\mathtt{F}_{\mathtt{R}}-{\mathtt{Top}})}}$, between the 2 lidars. Ideally, GT calibration should be recomputed regularly. However, for our sequences, even though they were collected on different days we used the same GT parameters.

We compare the results after evaluating $\mathbf{T}_{\mathtt{G}\mathtt{T}}-\hat{\mathbf{T}}_{\mathtt{Estimated}}$ for all the sequences using Kabsch alignment, CROON and our method. In the refinement step, we match a set of $N=100$ lines before applying the optimization discussed in Sec. IV-B. Kabsch alignment requires lidar odometry and CROON requires ICP matching with normal estimation. Hence for both methods, the computation time is significantly higher. Also, our method does not need any rotation initialization and we can also verify the quality of the extrinsic parameters online which is not considered in these works. As seen in Table III, our method performs better in feature rich environments present in Seq-3 and Seq-4 and under aggressive maneuvers in Seq-1, where rich motion sequences are available. In Fig. 11 once can see the closest features from the two point clouds after refinement. The features extracted from $\mathtt{F_{L}-Top}$ and $\mathtt{F_{R}-Top}$ lidar are colored red and green respectively.

VI Conclusion

In this work, we show that it is possible to initialize the extrinsic calibration of multiple lidars by matching the signals from IMUs co-located with the lidars. Unlike other methods which use odometry estimates for matching poses, this is a lightweight method that relies on raw signal matching. An observability-aware module informs us of the maneuvers needed to produce the excitations necessary for successful extrinsic calibration. Our method provides comparable extrinsics to the vehicle’s CAD parameters when sufficient signal excitations are present in normal urban driving scenarios.

VII Acknowledgements

This research has been jointly funded by the Swedish Foundation for Strategic Research (SSF) and Scania. The research is also affiliated with Wallenberg AI, Autonomous Systems and Software Program (WASP).