# Observability-aware online multi-lidar extrinsic calibration

Sandipan Das1,2, Ludvig af Klinteberg2, Maurice Fallon3, Saikat Chatterjee1 1
KTH EECS, Sweden<EMAIL_ADDRESS>
2 Autonomous Transport Solutions Research, Scania CV AB, Sweden.
{sandipan.das<EMAIL_ADDRESS>
3 Oxford Robotics Institute, UK<EMAIL_ADDRESS>
###### Abstract

Accurate and robust extrinsic calibration is necessary for deploying
autonomous systems which need multiple sensors for perception. In this paper,
we present a robust system for real-time extrinsic calibration of multiple
lidars in vehicle base frame without the need for any fiducial markers or
features. We base our approach on matching absolute GNSS and estimated lidar
poses in real-time. Comparing rotation components allows us to improve the
robustness of the solution than traditional least-square approach comparing
translation components only. Additionally, instead of comparing all
corresponding poses, we select poses comprising maximum mutual information
based on our novel observability criteria. This allows us to identify a subset
of the poses helpful for real-time calibration. We also provide stopping
criteria for ensuring calibration completion. To validate our approach
extensive tests were carried out on data collected using Scania test vehicles
(7 sequences for a total of $\approx$ 6.5 Km). The results presented in this
paper show that our approach is able to accurately determine the extrinsic
calibration for various combinations of sensor setups.

##  I Introduction

Autonomous vehicles require multiple sensors to create $360^{\circ}$ sensing
coverage for navigating safely and redundancy. Thus it is important to
determine the accurate mounting position of the sensors in the vehicle which
is referred to as extrinsic calibration. It is not possible to fuse the
sensing data in a common reference frame without correct extrinsic calibration
parameters. In our work we focused on extrinsic calibration of multi-lidar
systems.

Extrinsic calibration can be performed in 2 ways: Offline and Online. Offline
procedure refers to matching features between the lidars by placing fiducial
markers in their common field-of-view (FoV) of both the sensors [1, 2]. Online
calibration is performed by matching the estimated states of the sensor in its
corresponding sensor frame [3, 4, 5, 6, 7, 8]. Its desirable to perform online
calibration since the sensors may not have common FoV and at the same time it
reduces engineering effort compared to offline process.

Online calibration methods rely on motion of the mobile platform. At the same
time it is well known [9, 10, 11], all motion segments do not provide enough
information necessary for extrinsic calibration. Hence, identifying the
degenerate motion segments help to discard poses which are not necessary for
extrinsic calibration computation.

We provide an observability-aware online calibration framework which can be
run in the vehicle for calibrating multiple lidars in real-time. We also
provide an analytical study on the effect noisy state estimates in the online
calibration process.

Figure 1: Illustration of the four lidars with their embedded IMUs positioned
around one data collection vehicle, when the sensors are calibrated. The
vehicle base frame $\mathtt{{B}}$, is located at the center of the rear axle.
The sensor frames of the lidars are: $\mathtt{L}^{(\mathtt{F}_{\mathtt{L}})}$,
$\mathtt{L}^{(\mathtt{F}_{\mathtt{R}})}$,
$\mathtt{L}^{(\mathtt{R}_{\mathtt{R}})}$ and
$\mathtt{L}^{(\mathtt{F}_{\mathtt{L}})}$, represented in solid lines; whereas,
the sensor frames of the IMUs are: $\mathtt{I}^{(\mathtt{F}_{\mathtt{L}})}$,
$\mathtt{I}^{(\mathtt{F}_{\mathtt{R}})}$,
$\mathtt{I}^{(\mathtt{R}_{\mathtt{R}})}$ and
$\mathtt{I}^{(\mathtt{R}_{\mathtt{L}})}$, represented in dashed lines.
${(\mathtt{F}_{\mathtt{L}})}$: front-left, ${(\mathtt{F}_{\mathtt{R}})}$:
front-right, ${(\mathtt{R}_{\mathtt{R}})}$: rear-right and
${(\mathtt{R}_{\mathtt{L}})}$: rear-left.

###  I-A Motivation

The mobile platforms used in our experiments have multitude of sensors as
shown in Fig. 1. Moreover, before any autonomous run it is necessary to ensure
that the sensors are properly calibrated. So we need to perform real-time
calibration of all the sensors wrt to a common reference frame, usually
located in center of the rear-axle. We also need to identify motion segments
which provide enough excitation for different degrees of freedom required for
calibration. Finally we need a mechanism to understand when the online
calibration performance is satisfactory.

To achieve this, we calibrate all the lidars wrt a GNSS system which reports
pose of the common reference frame in global frame. The GNSS system
additionally reports the inertial measurements of the vehicle. All the lidar
sensors used in our experiments also have an embedded IMU. Hence, for
observability analysis we compare only the raw angular velocity signals
between the GNSS system and the embedded IMU within the lidar(s) unlike
comparing estimated states which inherently has process noise. Finally, we
introduce a cost function to stop the calibration process when the results are
satisfactory.

###  I-B Contribution

Our work is motivated by the broad literature in motion based calibration. Our
proposed contributions are:

  * •

An extrinsic calibration algorithm to calibrate multiple lidars in real-time
using GNSS reference poses with a calibration completion criteria.

  * •

A novel approach for observability analysis based on comparing raw angular
velocity signals.

  * •

We also provide our analytical study on sensitivity of the estimated states
used in our online calibration algorithm.

  * •

Verification of our method based on data collected from Scania autonomous
vehicles with the sensor setup shown in Fig. 2, with FoV schematics similar to
Fig. 1.

##  II Related Work

Online extrinsic calibration based on different strategies is an widely
studied area. In our discussion we briefly review the relevant literature and
motivate our choice of method adapted in our work.

###  II-A Motion-based methods for online extrinsic calibration

Online extrinsic calibration based on the Hand-Eye method [12, 13] is an
extensively studied topic. Initial research on motion-based calibration
aligned a gripper (hand) and a camera (eye) by estimating the motion of the
gripper along with the camera and constraining their poses with a fixed rigid
body transformation, this approach is called Hand-Eye calibration and has been
reviewed by Shah et al. [14]. Multi-camera extrinsic calibration with the
Hand-Eye method have been explored in works like Camodocal [4] and recently
extended to multi-lidar extrinsic calibration [15, 16].

Instead of performing Hand-Eye calibration, in Kalibr [9] 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
unobservant directions and degenerate scenarios. Furgale et al. [5] further
generalized Kalibr for spatio-temporal calibration. We build upon the
philosophy of Kalibr and minimize the error between the poses estimated by the
lidar(s) and the GNSS in a batch optimization process to recover the extrinsic
calibration parameters.

####  II-A1 Types of motion models

Huang et al. [6] proposed a framework to evaluate different motion based
strategies with their work on noise sensitivity analysis. They categorized the
motion-based methods as:

  * •

Model A: Comparing absolute poses in a common frame.

  * •

Model B: Comparing absolute poses in separate frames.

  * •

Model C: Comparing relative poses – Hand-Eye method.

They show that Model A performs well for all the relative angles between the
pose pairs and has minimum number of degeneration zones. Hence, we base our
cost function to recover transformation based on Model A.

####  II-A2 Types of cost functions

We can formulate the cost function in 2 different approaches:

  * •

Minimize the absolute translation in a least-square approach [17, 18].

  * •

Assume additive Gaussian noise in the estimated poses and minimize the
weighted noise [19, 7] component.

In our work we formulate our cost function as a least-square problem. Unlike
comparing only the translation component for comparing the poses we also
compare the rotation components using the Q-method by Davenport [20].
Additionally, we analytically show that optimization problem remains invariant
under certain noisy conditions. We also base our novel observability criteria
based on comparing the raw angular velocity signals and stop the calibration
process when satisfactory performance is reached.

###  II-B Lidar odometry

There have been vast array of works on lidar odometry after the LOAM paper by
Zhang et al. [21]. Often geometric features are extracted from the point
clouds and tracked between frames for computational efficiency of various
state estimation algorithms [22, 23, 24]. However, this principle doesn’t work
for featureless environments. Hence, instead of feature extraction, the whole
point cloud is often processed, which is known as direct estimation, shown in
works like Fast-LIO2 [25] and Faster-LIO [26]. We adapted Fast-LIO2 for
estimating lidar odometry in our online calibration algorithm.

##  III Problem Statement

###  III-A Sensor platform and reference frames

Figure 2: Reference frames conventions for our bus and truck platforms. In
both cases, the world frame $\mathtt{W}$ is a fixed frame, while the base
frame $\mathtt{{B}}$, as shown in Fig. 1, is located at the rear axle center
of the vehicle. Each sensor unit contains the two optical frames $\mathtt{C}$,
an IMU frame, $\mathtt{I}$, and lidar frame $\mathtt{L}$.

The sensor configuration used is as shown in Fig. 2 for a bus and truck. We
also show illustrations of the fields-of-view of the sensors for the bus in
Fig. 1. Each of the sensor housings contains a lidar with an integrated IMU
and two cameras. Please note the cameras are not used for this work.

First we will describe the necessary notation and the reference frames used in
our system. The vehicle base frame, $\mathtt{{B}}$ is located on the center of
the rear-axle of the vehicle. Sensor readings from GNSS, lidars and IMUs are
represented in their respective sensor frames as $\mathtt{G}$,
$\mathtt{L}^{(k)}$ and $\mathtt{I}^{(k)}$ respectively. Here,
$k\in[\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}},\mathtt{R}_{\mathtt{L}},\mathtt{R}_{\mathtt{R}}]$
denotes the location of the sensor in the vehicle corresponding to front-left,
front-right, rear-left and rear-right respectively. The GNSS measurements are
reported in world fixed frame, $\mathtt{W}$ and transformed to $\mathtt{{B}}$
frame by performing a calibration routine outside the scope of this work. 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{R}\mathbf{R}^{T}=\mathbf{I}_{3\times 3}$, since the rotation matrix
is orthogonal.

###  III-B Problem formulation

Our primary goal is to estimate the extrinsic calibration of multiple lidar
sensors wrt the base frame and to recover
$\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}$ in real-time without the need for
any fiducial markers. Since, our method is based on a motion-based approach,
we first estimate the pose of the lidar sensor,
$\mathbf{T}_{\mathtt{L}_{i}^{(k)}}$, at time $t_{i}$ in its corresponding
sensor frame, $\mathtt{L}^{(k)}$, prior to the online calibration. We also
study the effect of process noise in our algorithm and identify the states
which provide maximum excitation for different degrees of freedom, based on
which online extrinsic calibration is performed.

##  IV Methodology

###  IV-A Initialization

All the GNSS poses in base frame, $\mathtt{{B}}$ are normalized wrt to the
initial pose,
$\tensor[_{\mathtt{W}}]{\mathbf{T}}{{}_{\mathtt{{\mathtt{W}\mathtt{G}}}}}$.
The GNSS unit also provides IMU measurements in the $\mathtt{G}$ frame using
its internally embedded IMU. These measurement are then transformed to base
frame, $\mathtt{{B}}$. For gravity alignment we use the equations 25 and 26
from [27] to estimate roll and pitch after collecting IMU data when the
vehicle is static for few seconds according to the GNSS estimate.

Note that the noise processes and starting bias estimates for the embedded IMU
sensor within the lidars and the GNSS unit were characterized in advance by
estimating the Allan Variance [28] parameters using logs collected while the
vehicle was stationary.

###  IV-B Lidar-IMU calibration

The integrated IMU within the lidar is utilized to rectify motion-induced
lidar distortion as well as to pre-compute the initial guess for scan matching
within the lidar odometry. Hence, it is important to calibrate the lidar and
its corresponding integrated IMU. If the sensor supplier provided extrinsics
are not available, the calibration can be formulated as a Hand-Eye problem to
estimate the relative IMU and lidar poses separately. We integrate the IMU
measurements between estimated lidar poses using IMU preintegration [29]. The
state propagation equations are based on the IMU data
$[\boldsymbol{\omega}:\text{angular velocity},\mathbf{f}:\text{linear
acceleration}]$ between two timestamps [$t_{i}$, $t_{j}$] for all the $\Delta
t$ (inverse of sensor frequency) intervals in the $\mathrm{SE}(3)$ manifold is
derived in [29] as:

$\displaystyle\Delta\mathtt{R}_{ij}$
$\displaystyle=\prod_{k=i}^{j-1}\operatorname{Exp}\left(\boldsymbol{\omega}_{\mathtt{I}_{k}}\Delta
t\right),$ (1) $\displaystyle\Delta\mathbf{v}_{ij}$
$\displaystyle=\sum_{k=i}^{j-1}\Delta\mathrm{R}_{ik}\mathbf{f}_{\mathtt{I}_{k}}\Delta
t,$ $\displaystyle\Delta\mathbf{p}_{ij}$
$\displaystyle=\sum_{k=i}^{j-1}\left[\Delta\mathbf{v}_{ik}\Delta
t+\frac{1}{2}\Delta\mathrm{R}_{ik}\mathbf{f}_{\mathtt{I}_{k}}\Delta
t^{2}\right],$

where, $\Delta\mathtt{R}_{ij},\Delta\mathbf{v}_{ij},\Delta\mathbf{p}_{ij}$
represent the relative rotation, velocity and position respectively. Based on
the computed lidar II-B and IMU relative poses (Eq. 1) we formulate a Hand-Eye
problem as,

${\mathbf{T}}{{}_{\mathtt{L}_{i}\mathtt{L}_{j}}}{\mathbf{T}}{{}_{\mathtt{L}\mathtt{I}}}={\mathbf{T}}{{}_{\mathtt{L}\mathtt{I}}}{\mathbf{T}}{{}_{{\mathtt{I}_{i}\mathtt{I}_{j}}}}.$
(2)

Since, we use the relative transforms from the IMU propagation,
$\mathbf{R}{{}_{\mathtt{L}\mathtt{I}}^{\star}}$ is determined by equating the
rotation parts in Eq. 2 in quaternion form, similar to [8] as,

$\displaystyle{\mathbf{q}}{{}_{\mathtt{L}_{i}\mathtt{L}_{j}}}\otimes{\mathbf{q}}{{}_{\mathtt{L}\mathtt{I}}}={\mathbf{q}}{{}_{\mathtt{L}\mathtt{I}}}\otimes{\mathbf{q}}_{\mathtt{I}_{i}\mathtt{I}_{j}}$
(3) $\displaystyle\implies$
$\displaystyle\underbrace{\left[\mathcal{L}(\mathbf{q}{{}_{\mathtt{L}_{i}\mathtt{L}_{j}}})-\mathcal{R}(\mathbf{q}_{\mathtt{I}_{i}\mathtt{I}_{j}})\right]}_{\mathbf{A}_{k}}{\mathbf{q}}{{}_{\mathtt{L}\mathtt{I}}}=0,$

where, the matrices, $\mathcal{L}(\mathbf{q})$ and $\mathcal{R}(\mathbf{q})$
are defined as,

$\displaystyle\mathcal{L}(\mathbf{q})$
$\displaystyle=\left[\begin{array}[]{cc}q_{s}\mathbf{I}_{3}-\left[\mathbf{q}_{v}\right]_{\times}&\mathbf{q}_{v}\\\
-\mathbf{q}_{v}^{T}&q_{s}\end{array}\right]$
$\displaystyle\mathcal{R}(\mathbf{q})$
$\displaystyle=\left[\begin{array}[]{cc}q_{s}\mathbf{I}_{3}+\left[\mathbf{q}_{v}\right]_{\times}&\mathbf{q}_{v}\\\
-\mathbf{q}_{v}^{T}&q_{s}\end{array}\right],$

and, $\mathbf{q}$ is a quaternion represented as,

$\displaystyle\mathbf{q}=\underbrace{q_{s}}_{\text{Scalar
component}}+\underbrace{q_{x}\hat{i}+q_{y}\hat{j}+q_{z}\hat{k}}_{\text{Vector
component}}=\left[q_{s},\mathbf{q}_{v}\right].$ (4)

For a set of relative poses we can form a system of linear equations of the
form, $\mathbf{Ax}=\mathbf{0}$, where $\mathbf{A}=[\mathbf{A}_{0}^{T}\
\mathbf{A}_{1}^{T}\ \dots\ \mathbf{A}_{n}^{T}]^{T}$.
${\mathbf{q}}^{\star}_{\mathtt{L}\mathtt{I}}$ lies in the nullspace of
$\mathbf{A}$, which corresponds to the right unit singular vector with the
smallest singular value of $\mathbf{A}$.

After finding $\mathbf{q}{{}_{\mathtt{L}\mathtt{I}}^{\star}}$, for the
translation component we get,

$\underbrace{\left(\mathbf{R}_{{\mathtt{L}_{i}}{\mathtt{L}_{j}}}-\mathbf{I}\right)}_{\mathbf{A}_{k}}\mathbf{t}{{}_{\mathtt{L}\mathtt{I}}}\stackrel{{\scriptstyle
Eq.~{}\ref{eq:hand-eye-
li}}}{{=}}\underbrace{\left(\mathbf{R}{{}_{\mathtt{L}\mathtt{I}}^{\star}}\mathbf{t}_{{\mathtt{I}_{i}}{\mathtt{I}_{j}}}-\mathbf{t}_{{\mathtt{L}_{i}}{\mathtt{L}_{j}}}\right)}_{\mathbf{B}_{k}},$
(5)

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

$\mathbf{x}^{\star}=\operatorname*{arg\,min}_{\mathbf{x}}\|\mathbf{A}\mathbf{x}-\mathbf{B}\|^{2}_{\boldsymbol{\Sigma}},$
(6)

where, ${\boldsymbol{\Sigma}}$ denotes the covariance of the residual. For
these problems, the general strategy [30] is to iteratively approximate the
original problem 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})$. Thus, $\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 additional operator on the manifold and the problem then
becomes,

$\displaystyle\mathbf{\Delta
x}^{\star}=\operatorname*{arg\,min}_{\mathbf{\Delta
x}}\frac{1}{2}\|(\mathbf{A}\hat{\mathbf{x}}-\mathbf{B})+\mathbf{A}^{T}\mathbf{\Delta
x}\|^{2}_{\boldsymbol{\Sigma}}.$ (7)

The optimal solution is given by,

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

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

$\mathbf{x}^{\star}=\operatorname*{arg\,min}_{\mathbf{L}\leq\mathbf{x}\leq\mathbf{U}}\|\mathbf{A}\mathbf{x}-\mathbf{B}\|^{2}_{\boldsymbol{\Sigma}},$
(9)

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

$\displaystyle\mathbf{\Delta
x}^{\star}=\operatorname*{arg\,min}_{\mathbf{\Delta
x}}\frac{1}{2}\|(\mathbf{A}\hat{\mathbf{x}}-\mathbf{B})+\mathbf{A}^{T}\mathbf{\Delta
x}\|^{2}_{\boldsymbol{\Sigma}},$ (10) $\displaystyle\textit{s.t.}\
\mathbf{L}\leq\hat{\mathbf{x}}+\mathbf{\Delta x}\leq\mathbf{U}.$

###  IV-C Lidar calibration in base frame

Let, $\mathbf{T}_{\mathtt{{B}}_{i}}$ be the pose of the base frame obtained
using the GNSS measurements at timestamp $t_{i}$. And,
$\mathbf{T}_{\mathtt{L}_{i}^{(k)}}$ be the pose of the $k^{th}$ lidar in its
corresponding sensor frame obtained from a state of the art state estimator
[25] at timestamp $t_{i}$. Then our problem is to estimate the optimal
transformation matrix between 2 pair of poses accumulated over $N$ timestamps:

$\displaystyle\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}^{\star}=\left[\begin{array}[]{cc}\mathbf{R}^{*}_{3\times
3}&\mathbf{t}^{*}_{3\times 1}\\\ \mathbf{0}^{\top}&1\end{array}\right]$ (13)
$\displaystyle=\operatorname*{arg\,min}_{\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}}\sum_{i=1}^{N}\|\mathbf{T}_{\mathtt{{B}}\mathtt{L}^{(k)}}\mathbf{T}_{\mathtt{{B}}_{i}}-\mathbf{T}_{\mathtt{L}_{i}^{(k)}}\|^{2}_{F}$
(14)
$\displaystyle=\operatorname*{arg\,min}_{\mathbf{R}_{\mathtt{{B}}\mathtt{L}^{(k)}},\mathbf{t}_{\mathtt{{B}}\mathtt{L}^{(k)}}}\sum_{i=1}^{N}\leavevmode\resizebox{255.8342pt}{}{$\left\|\begin{array}[]{c|c}\mathbf{R}_{\mathtt{{B}}\mathtt{L}^{(k)}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}}&\mathbf{R}_{\mathtt{{B}}\mathtt{L}^{(k)}}\mathbf{t}_{\mathtt{{B}}_{i}}+\mathbf{t}_{\mathtt{{B}}\mathtt{L}^{(k)}}-\mathbf{t}_{\mathtt{L}_{i}}\\\
\\\ \hline\cr\\\
\mathbf{0}^{\top}&\mathbf{0}^{\top}\end{array}\right\|_{F}^{2}$}.$ (19)

####  IV-C1 Initial calibration estimation

The initial extrinsics can be recovered by aligning the initial set of
translation components, $\mathbf{t}_{\mathtt{{B}}_{i}}$ and
$\mathbf{t}_{\mathtt{L}_{i}}$, $\forall i=1(1)N$, using a 3D-point cloud
alignment method proposed by Kabsch et al. [17] or Horn et al. [18], by
breaking the problem into rotation and translation parts. Note that we omit
the $k^{th}$ superscript for brevity in the rest of our discussion.

####  IV-C2 Rotation estimation

For Frobenius norm,

$\displaystyle\|\mathbf{A}+\mathbf{B}\|_{\mathrm{F}}^{2}\stackrel{{\scriptstyle
def}}{{=}}\|\mathbf{A}\|_{\mathrm{F}}^{2}+\|\mathbf{B}\|_{\mathrm{F}}^{2}+2\langle\mathbf{A},\mathbf{B}\rangle_{\mathrm{F}}$
(20) $\displaystyle\text{where,
}\langle\mathbf{A},\mathbf{B}\rangle_{\mathbf{F}}\stackrel{{\scriptstyle
def}}{{=}}\sum_{i,j}\overline{A_{ij}}B_{ij}=\operatorname{Tr}\left(\mathbf{A}^{T}\mathbf{B}\right).$
(21)

After recovering the initial parameters, the optimal rotation optimization can
be re-written as,

$\displaystyle\mathbf{R}^{\star}_{\mathtt{{B}}\mathtt{L}}=\operatorname*{arg\,min}_{\mathbf{R}_{\mathtt{{B}}\mathtt{L}}}\sum_{i=1}^{N}\|\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}}\|^{2}_{F}$
(22)
$\displaystyle=\leavevmode\resizebox{411.93767pt}{}{$\underset{\mathbf{R}_{\mathtt{{B}}\mathtt{L}}}{\arg\min}\sum_{i=1}^{N}\left[\|\mathbf{R}_{\mathtt{{B}}_{i}}^{T}\mathbf{R}_{\mathtt{{B}}_{i}}\|+\|\mathbf{R}_{\mathtt{L}_{i}}^{T}\mathbf{R}_{\mathtt{L}_{i}}\|-2\operatorname{Tr}\left(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}\mathbf{R}_{\mathtt{L}_{i}}^{T}\right)\right]$}$
$\displaystyle=\operatorname*{arg\,min}_{\mathbf{R}_{\mathtt{{B}}\mathtt{L}}}\sum_{i=1}^{N}2\left[1-\operatorname{Tr}\left(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}\mathbf{R}_{\mathtt{L}_{i}}^{T}\right)\right],$
(23)

where, the rotation components are in $\mathrm{SE}(3)$,
$\mathbf{R}\mathbf{R}^{T}=\mathbf{I}$. Because the constant terms do not
affect the $\operatorname{argmin}$ operator, we can discard those and
introduce our proposed cost function as,

$\displaystyle
J(\mathbf{R}_{\mathtt{{B}}\mathtt{L}})=-\sum_{i=1}^{N}\operatorname{Tr}\left(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}\mathbf{R}_{\mathtt{L}_{i}}^{T}\right).$
(24)

####  IV-C3 Q-method formulation

We formulate our problem as Q-method [20] and follow their ideas to convert
the cost function based on rotation matrices to quaternion form as follows,

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

where, $\mathbf{\Delta}_{3\times
3}=\sum_{i=1}^{N}\left(\mathbf{R}_{\mathtt{{B}}_{i}}\mathbf{R}_{\mathtt{L}_{i}}^{T}\right)$.
The rotation matrix can be expressed as a quaternion as,

$\displaystyle\mathbf{R}_{\mathtt{{B}}\mathtt{L}}=\left(q_{s}^{2}-\mathbf{q}_{v}^{T}\mathbf{q}_{v}\right)\mathbf{I}+2\mathbf{q}_{v}\mathbf{q}_{v}^{T}-2q_{s}\left[\mathbf{q}_{v}\right]_{\times},$
(26)

where, $[.]_{\times}$ is the skew-symmetric matrix. So, if we rewrite the cost
function of Eq. 25 in quaternion space we get,

$\displaystyle J(\mathbf{q})\stackrel{{\scriptstyle
Eq.~{}\ref{eq:rot_to_quat}}}{{=}}-\operatorname{Tr}\left[\left((q_{s}^{2}-\mathbf{q}_{v}^{T}\mathbf{q}_{v})\mathbf{I}+2\mathbf{q}_{v}\mathbf{q}_{v}^{T}-2q_{s}\left[\mathbf{q}_{v}\right]_{\times}\right)\mathbf{\Delta}\right]$
$\displaystyle=\leavevmode\resizebox{411.93767pt}{}{$-\left(q_{s}^{2}-\mathbf{q}_{v}^{T}\mathbf{q}_{v}\right)\operatorname{Tr}\left(\mathbf{\Delta}\right)-\mathbf{q}_{v}^{T}\left(\mathbf{\Delta}^{T}+\mathbf{\Delta}\right)\mathbf{q}_{v}+2q_{s}\operatorname{Tr}\left[\left(\mathbf{q}_{v}\right)_{\times}\mathbf{\Delta}\right]$}.$
(27)

Expanding the last term in the Eq. IV-C3 yields,

$\displaystyle\operatorname{Tr}\left[\left(\mathbf{q}_{v}\right)_{\times}\mathbf{\Delta}\right]$
$\displaystyle=\leavevmode\resizebox{411.93767pt}{}{$q_{x}(\mathbf{\Delta}(3,2)-\mathbf{\Delta}(2,3))+q_{y}(\mathbf{\Delta}(1,3)-\mathbf{\Delta}(3,1))+q_{z}(\mathbf{\Delta}(2,1)-\mathbf{\Delta}(1,2))$}$
$\displaystyle=\mathbf{\Lambda}\mathbf{q}_{v},$ (28)

where,
$\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]$. Hence, in
quaternion form the cost function can be re-written as,

(29)

Defining, $\mathbf{\Gamma}=\mathbf{\Delta}^{T}+\mathbf{\Delta}$ and
$\mu=\operatorname{Tr}(\mathbf{\Delta})$ we get,

$\displaystyle J(\mathbf{q})\stackrel{{\scriptstyle
Eq.~{}\ref{eq:quat_cost_fn}}}{{=}}\leavevmode\resizebox{346.89731pt}{}{$-\left(q_{s}^{2}-\mathbf{q}_{v}^{T}\mathbf{q}_{v}\right)\operatorname{Tr}\left(\mathbf{\Delta}\right)-\mathbf{q}_{v}^{T}\left(\mathbf{\Delta}^{T}+\mathbf{\Delta}\right)\mathbf{q}_{v}+2q_{s}\mathbf{\Lambda}\mathbf{q}_{v}$}$
$\displaystyle=-\left(\mathbf{q}_{v}^{T}(\mathbf{\Gamma}-\mu\mathbf{I})\mathbf{q}_{v}+q_{s}\mathbf{\Lambda}\mathbf{q}_{v}+q_{s}\textasciidieresis\mathbf{\Lambda}^{T}+q_{s}^{2}\mu\right)$
$\displaystyle=-\left[\begin{array}[]{ll}\mathbf{q}_{v}^{T}&q_{s}\end{array}\right]\left[\begin{array}[]{cc}\mathbf{\Gamma}-\mu\mathbf{I}&\mathbf{\Lambda}\\\
\mathbf{\Lambda}^{T}&\mu\end{array}\right]\left[\begin{array}[]{l}\mathbf{q}_{v}\\\
q_{s}\end{array}\right]$ (35)
$\displaystyle=-\mathbf{q}^{T}\mathbf{K}\mathbf{q},$ (36)

where, $\mathbf{q}$ is the attitude quaternion (vector first), and
$\mathbf{K}$ is the Davenport matrix denoted as
$\mathbf{K}=\left[\begin{array}[]{cc}\mathbf{\Gamma}-\mu\mathbf{I}&\mathbf{\Lambda}\\\
\mathbf{\Lambda}^{T}&\mu\end{array}\right]$. Since, $\mathbf{\Gamma}$ is
symmetric, the Davenport matrix is also symmetric. From spectral theorem [31],
we know that for any symmetric matrix, the eigenvalues are real and the
associated eigenvectors form an orthonormal basis. We use this property to
ensure a deterministic solution in the next steps of the process.

####  IV-C4 Q-method optimization with Lagrange multiplier

We can now perform a constrained optimization by considering the orthogonal
constraints of the rotation, $\mathbf{q}\mathbf{q}^{T}=\mathbf{I}$, which can
be introduced using a Lagrange multiplier, $\lambda$. The optimization
function for Eq. 36 can then be re-written as,

$\displaystyle\min_{\mathbf{q},\lambda}J(\mathbf{q},\lambda)$
$\displaystyle=-\mathbf{q}^{T}\mathbf{K}\mathbf{q}+\lambda\left(\mathbf{q}^{T}\mathbf{q}-1\right)$
(37) $\displaystyle\text{For minima, }\frac{\partial J}{\partial\mathbf{q}}$
$\displaystyle=0\implies\mathbf{K}\mathbf{q}=\lambda\mathbf{q}.$ (38)

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).

$\displaystyle\mathbf{q}^{\star}$
$\displaystyle=\max_{\operatorname{Eigenvalues}(\mathbf{K})}{\operatorname{Eigenvectors}(\mathbf{K})}$
(39) $\displaystyle\implies\min_{\mathbf{q},\lambda}J(\mathbf{q},\lambda)$
$\displaystyle\stackrel{{\scriptstyle
Eq.~{}\ref{eq:davenport_opt}}}{{=}}-\lambda\mathbf{I}+\lambda(\mathbf{I}-1)=-\lambda.$
(40)

We only update the estimated extrinsic rotation parameters if it improves the
rotational cost function, as described in IV-C7.

####  IV-C5 Translation estimation

Note that, because the motion of the lidar is estimated in the sensor frame,
it is not possible to solve for the translation component without the relative
transformations and the rigid body constraint provided by Hand-Eye method
[12].

Figure 3: Illustration of Hand-eye calibration between $\mathtt{{B}}$ and
$\mathtt{L}^{(\mathtt{F}_{\mathtt{L}})}$ frame.

From Fig. 3 we can write:

${\mathbf{T}}{{}_{{\mathtt{{B}}_{i-1}\mathtt{{B}}_{i}}}}{\mathbf{T}}{{}_{\mathtt{{B}}\mathtt{L}}}={\mathbf{T}}{{}_{\mathtt{{B}}\mathtt{L}}}{\mathbf{T}}{{}_{\mathtt{L}_{i-1}\mathtt{L}_{i}}},$
(41)

which can be further decomposed into the translation part with the pre-
computed optimal rotation in previous step as,

$\underbrace{\left(\mathbf{R}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}-\mathbf{I}\right)}_{\mathbf{A}_{i}}\mathbf{t}{{}_{\mathtt{{B}}\mathtt{L}}}\stackrel{{\scriptstyle
Eq.~{}\ref{eq:R_estimated}}}{{=}}\underbrace{\left(\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}^{\star}}\mathbf{t}_{{\mathtt{L}_{i-1}}{\mathtt{L}_{i}}}-\mathbf{t}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}\right)}_{\mathbf{B}_{i}},$
(42)

where, $\forall i=1(1)N$, which can be computed using similar principle of
solving Eq. 9 with known prior bounds.

Input: Lidar point cloud in $\mathtt{L}$ frame, GNSS reference poses in
$\mathtt{{B}}$ frame = $\mathbf{T}_{\mathtt{{B}}_{i}}$, Angular velocity
signals of IMUs embedded in lidar and GNSS =
$\\{\boldsymbol{\omega}_{\mathtt{I}_{i}},\boldsymbol{\omega}_{\mathtt{{B}}_{i}}\\}$,
Batch size = $N$, Pose error threshold = $\beta$, Information threshold =
$\epsilon$.

1

Output: $\mathbf{T}^{\star}_{\mathtt{{B}}\mathtt{L}}$.

2

3if  _not initialized_ then

4

5 if  _$\mathbf{T}_{\mathtt{L}\mathtt{I}}$ is unknown_ then

// Estimate $\mathbf{T}_{\mathtt{L}_{1}\mathtt{L}_{N}}$ with lidar odometry
and $\mathbf{T}_{\mathtt{I}_{1}\mathtt{I}_{N}}$ using Eq. 1 for Hand-Eye

6

7 Estimate $\mathbf{T}_{\mathtt{L}\mathtt{I}}$ using Eq. 3 and Eq. 5.

8

9 Estimate $\mathbf{T}_{\mathtt{L}_{i}}$ using lidar odometry.

// Since, initial $\mathbf{t}_{\mathtt{{B}}\mathtt{L}}$ can be measured, only
initial $\mathbf{R}_{\mathtt{{B}}\mathtt{L}}$ computation is necessary

10

11 Compute initial $\mathbf{R}_{\mathtt{{B}}\mathtt{L}}$ and
$\mathbf{t}_{\mathtt{{B}}\mathtt{L}}$ by aligning the set of
$\mathbf{t}_{\mathtt{L}_{i}}$ wrt $\mathbf{t}_{\mathtt{{B}}_{i}}$, $\forall
i=1(1)N$ by Umeyama [32] or Horn [18] alignment.

12

13Let,
$\mathtt{Error}({\mathbf{T}}_{\mathtt{{B}}\mathtt{L}})\stackrel{{\scriptstyle
Eq.~{}\ref{eq:error_term}}}{{=}}\frac{1}{N}\sqrt{\sum_{i=1}^{N}\xi_{\mathbf{T}_{\mathtt{{B}}\mathtt{L}}}({\mathbf{T}}{{}_{\mathtt{{B}}_{i}}},{\mathbf{T}}{{}_{\mathtt{L}_{i}}})}$

14and,
$\mathtt{R_{Error}}({\mathbf{R}}_{\mathtt{{B}}\mathtt{L}})\stackrel{{\scriptstyle
Eq.~{}\ref{eq:error_term}}}{{=}}\frac{1}{N}\sqrt{\sum_{i=1}^{N}\xi_{\mathbf{R}_{\mathtt{{B}}\mathtt{L}}}({\mathbf{R}}{{}_{\mathtt{{B}}_{i}}},{\mathbf{R}}{{}_{\mathtt{L}_{i}}})}$.

15Let, ${\mathbf{T}}_{\mathtt{{B}}}=\\{\\}$ and
${\mathbf{T}}_{\mathtt{L}}=\\{\\}$, be the set of all GNSS and lidar poses
which satisfy observability criteria.

16while  _$\mathtt{Error}(\mathbf{T}_{\mathtt{{B}}\mathtt{L}})\geq\beta$_ do

17 Estimate $\mathbf{T}_{\mathtt{L}_{i}}$ using lidar odometry.

18 Find closest $\mathbf{T}_{\mathtt{{B}}_{i}}$ wrt
$\mathbf{T}_{\mathtt{L}_{i}}$ based on timestamp correspondance of the poses.

19 if  _No. of corresponding poses = $N$_ then

20

21 Compute $\mathcal{I}_{N\times
N}(\boldsymbol{\omega}_{\mathtt{{B}}_{i}},\boldsymbol{\omega}_{\mathtt{I}_{i}})$
using Eq. 44.

22 if  _Minimum singular value of $\mathcal{I}_{N\times N}$ $\geq$ $\epsilon$_
then

23

24
$\mathbf{T}_{\mathtt{{B}}}:=\mathbf{T}_{\mathtt{{B}}}\cup\\{\mathbf{T}_{\mathtt{{B}}_{i}}\\}$,
$\forall i=1(1)N$.

25
$\mathbf{T}_{\mathtt{L}}:=\mathbf{T}_{\mathtt{L}}\cup\\{\mathbf{T}_{\mathtt{L}_{i}}\\}$,
$\forall i=1(1)N$.

26 Compute $\hat{\mathbf{R}}_{\mathtt{{B}}\mathtt{L}}$ between
$\mathbf{T}_{\mathtt{{B}}}$ and $\mathbf{T}_{\mathtt{L}}$ by solving for
$\mathbf{K}$ using Eq. 39.

27 if
_$\mathtt{R_{Error}}(\hat{\mathbf{R}}_{\mathtt{{B}}\mathtt{L}})\leq\mathtt{R_{Error}}({\mathbf{R}}_{\mathtt{{B}}\mathtt{L}})$_
then

28
$\mathbf{R}_{\mathtt{{B}}\mathtt{L}}:=\hat{\mathbf{R}}_{\mathtt{{B}}\mathtt{L}}$.

29 Solve for $\hat{\mathbf{t}}$ using Eq. 42.

30 if  _$\mathtt{Error}(\hat{\mathbf{T}}_{\mathtt{{B}}\mathtt{L}})$
$\leq\mathtt{Error}(\mathbf{T}_{\mathtt{{B}}\mathtt{L}})$_ then

// Update current extrinsics

31

32
$\mathbf{T}_{\mathtt{{B}}\mathtt{L}}:=\hat{\mathbf{T}}_{\mathtt{{B}}\mathtt{L}}$.

33

34

35 else

36 Discard the set of current $N$ poses.

37

38

39$\mathbf{T}^{\star}_{\mathtt{{B}}\mathtt{L}}:={\mathbf{T}}_{\mathtt{{B}}\mathtt{L}}$

Algorithm 1 Online lidar extrinsic calibration

####  IV-C6 Observability analysis

Not all motions of the test vehicle sufficiently excite the degrees of freedom
of the calibration technique. To ensure that online calibration is reliable it
is necessary to identify the dataset segments which is suitable and ignore the
others.

The extrinsics of $\mathtt{I}^{(k)}$ wrt $\mathtt{{B}}$ frame is strongly
correlated with the extrinsics of $\mathtt{L}^{(k)}$ wrt $\mathtt{{B}}$ frame.
Moreover, the angular velocity at all the points in a rigid body are identical
and can be correlated to the motion estimate of the vehicle. Thus, we
formulate a cost function for maximizing the likelihood of matching the
angular velocity between $\mathtt{I}^{(k)}$ and $\mathtt{{B}}$ frame as,

$\displaystyle\mathbf{R}_{\mathtt{{B}}\mathtt{I}}^{\star}$
$\displaystyle=\operatorname*{arg\,min}_{\mathbf{R}_{\mathtt{{B}}\mathtt{{I}}}}\sum_{i=1}^{N}\|\mathbf{R}_{\mathtt{{B}}\mathtt{{I}}}\boldsymbol{\omega}_{\mathtt{{B}}_{i}}-\boldsymbol{\omega}_{\mathtt{I}_{i}}\|^{2}_{\boldsymbol{\sum_{i}}},$
(43) $\displaystyle\textit{s.t.}\ \
\mathbf{R}_{\mathtt{{B}}\mathtt{I}}\mathbf{R}_{\mathtt{{B}}\mathtt{I}}^{T}=\mathbf{I}_{3},$

where, $\boldsymbol{\omega}_{\mathtt{I}_{i}^{(k)}}$ and
$\boldsymbol{\omega}_{\mathtt{{B}}_{i}}$ are the angular velocities of
$\mathtt{I}^{(k)}$ and $\mathtt{{B}}$ at timestamp $t_{i}$. Since, Eq. 43 has
similar form as Eq. 6 we solve it iteratively with the update step as,

$\displaystyle\underbrace{\left(\sum_{i}\mathbf{J}_{i}^{\top}\Sigma_{i}^{-1}\mathbf{J}_{i}\right)}_{\text{Fisher
information
matrix}}\mathbf{\Delta}{\mathbf{\mathbf{R}}}_{\mathtt{{B}}\mathtt{{I}}}$
$\displaystyle\stackrel{{\scriptstyle
Eq.~{}\ref{eq:bvls_iterative_solution}}}{{=}}-\sum_{i}\mathbf{J}_{i}^{\top}\Sigma_{i}^{-1}(\hat{\mathbf{\mathbf{R}}}_{\mathtt{{B}}\mathtt{{I}}}\boldsymbol{\omega}_{\mathtt{{B}}_{i}}-\boldsymbol{\omega}_{\mathtt{I}_{i}})$
(44)

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. Since, we compute this online we do processing
in a pre-defined batch size of $N$. We perform a Singular Value Decomposition
of $\mathcal{I}_{N\times N}$ for each batch as:

$\mathcal{I}_{N\times N}=\textbf{USU}^{T},$ (45)

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. The value of the minimum
singular value gives an indication about the information in the batch data. If
the minimum singular value is more than an threshold (design choice) we
conclude the data in the batch is sufficiently excited for reliable extrinsics
computation. If the batch of IMU signals provide enough information, the
estimated poses from the lidar odometry are selected for online calibration
computation as described in Algorithm 1.

####  IV-C7 Stopping criteria

In our Algorithm 1, we trigger the optimization process when a pre-defined
batch of poses have been accumulated. However, instead of considering only the
current batch of poses we consider the full set of accumulated poses from the
beginning of operation to encapsulate wider excitation of different degrees of
freedom. At each iteration we compute the normalized cost function and update
the estimated extrinsic parameters, if it improves the cost function. Based on
our empirical analysis we stop the process once our cost function is below a
certain threshold. We define our cost function in $\mathrm{SE}(3)$ using
properties from Riemannian geometry [33] as,

$\displaystyle\leavevmode\resizebox{381.5877pt}{}{$\xi_{{\mathbf{T}}^{\star}_{\mathtt{{B}}\mathtt{L}}}({\mathbf{T}}{{}_{\mathtt{{B}}_{i}}},{\mathbf{T}}{{}_{\mathtt{L}_{i}}})=\underbrace{\|\log(\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}^{\star}}\mathbf{R}{{}_{\mathtt{{B}}_{i}}}\mathbf{R}{{}_{\mathtt{L}_{i}}^{T}})\|^{2}}_{\mathtt{Rotation\
Cost}=\xi_{{\mathbf{R}}_{\mathtt{{B}}\mathtt{L}}}}+\underbrace{\|\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}^{\star}}\mathbf{t}_{\mathtt{{B}}_{i}}+\mathbf{t}_{\mathtt{{B}}\mathtt{L}}^{\star}-\mathbf{t}_{\mathtt{L}_{i}}\|^{2}}_{\mathtt{Translation\
Cost}=\xi_{{\mathbf{t}}_{\mathtt{{B}}\mathtt{L}}}}$},$ (46)
$\displaystyle\text{where,
}\log(\mathbf{R})=\left\\{\begin{array}[]{cc}0,&\theta=0\\\
\frac{\theta}{2\sin\theta}\left(\mathbf{R}-\mathbf{R}^{\top}\right)&|\theta|\in(0,\pi)\end{array}\right.$
(49) $\displaystyle\text{and, }\operatorname{Tr}(\mathbf{R})=1+2\cos\theta.$

####  IV-C8 Sensitivity analysis

Here we provide an analytical study of the effect of additive noise on our
optimization problem.

(i) Rotation noise component: First we consider the rotational noisy
components. We assume that the additive noise still constructs a rotation
matrix in $\mathrm{SE}(3)$. Considering the GNSS and the estimated lidar
rotational components with noise as
$\mathbf{R}_{\mathtt{{B}}_{i}}+\boldsymbol{\eta}_{\mathtt{{B}}_{i}}$ and
$\mathbf{R}_{\mathtt{{B}}\mathtt{L}_{i}}+\boldsymbol{\eta}_{\mathtt{L}_{i}}$,
our optimization problem becomes,

$\displaystyle\mathbf{R}_{\mathtt{{B}}\mathtt{L}}^{\star}$
$\displaystyle=\operatorname*{arg\,min}_{\mathbf{R}_{\mathtt{{B}}\mathtt{L}},\boldsymbol{\eta}_{\mathtt{{B}}_{i}},\boldsymbol{\eta}_{\mathtt{L}_{i}}}\sum_{i=1}^{N}\left(\|\boldsymbol{\eta}_{\mathtt{{B}}_{i}}\|^{2}_{\Sigma^{-1}_{\mathtt{{B}}\mathtt{{B}}}}+\|\boldsymbol{\eta}_{\mathtt{L}_{i}}\|^{2}_{\Sigma^{-1}_{\mathtt{L}\mathtt{L}}}\right)$
(50) $\displaystyle\textit{s.t.}\ \
\mathbf{R}_{\mathtt{{B}}\mathtt{L}}(\mathbf{R}_{\mathtt{{B}}_{i}}+\boldsymbol{\eta}_{\mathtt{{B}}_{i}})=(\mathbf{R}_{\mathtt{L}_{i}}+\boldsymbol{\eta}_{\mathtt{L}_{i}})\;\;\forall
i.$ (51)

Thus, rearranging Eq. 51 would yield,

$\boldsymbol{\eta}_{\mathtt{L}_{i}}=(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}})+\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}}.$
(52)

Considering equal co-variance norms we can re-write the error term, $\Xi$ at
$t_{i}$ as,

$\displaystyle\Xi(\boldsymbol{\eta}_{\mathtt{{B}}_{i}},\boldsymbol{\eta}_{\mathtt{L}_{i}})=\|\boldsymbol{\eta}_{\mathtt{{B}}_{i}}\|^{2}+\|\boldsymbol{\eta}_{\mathtt{L}_{i}}\|^{2}$
(53)
$\displaystyle=\leavevmode\resizebox{385.92152pt}{}{$2\|\boldsymbol{\eta}_{\mathtt{{B}}_{i}}\|^{2}+2(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}})^{T}\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}}+\|\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}}\|^{2}$},$
$\displaystyle\leavevmode\resizebox{429.28616pt}{}{$\text{since},\|\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}}\|^{2}=(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}})^{T}\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}}=\boldsymbol{\eta}_{\mathtt{{B}}_{i}}^{T}\mathbf{R}_{\mathtt{{B}}\mathtt{L}}^{T}\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\boldsymbol{\eta}_{\mathtt{{B}}_{i}}=\|\boldsymbol{\eta}_{\mathtt{{B}}_{i}}\|^{2}$}.$

To minimize, we equate the Jacobian of
$\Xi(\boldsymbol{\eta}_{\mathtt{{B}}_{i}},\boldsymbol{\eta}_{\mathtt{L}_{i}})$
to zero.

$\displaystyle\implies$
$\displaystyle\frac{\partial\Xi}{\partial\boldsymbol{\eta}_{\mathtt{{B}}_{i}}}=0$
or, $\displaystyle
4\boldsymbol{\eta}_{\mathtt{{B}}_{i}}+2\mathbf{R}_{\mathtt{{B}}\mathtt{L}}^{T}(\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{L}_{i}})=0$
or,
$\displaystyle\boldsymbol{\eta}_{\mathtt{{B}}_{i}}=-\frac{1}{2}(\mathbf{R}_{\mathtt{{B}}_{i}}-\mathbf{R}_{\mathtt{{B}}\mathtt{L}}^{-1}\mathbf{R}_{\mathtt{L}_{i}}).$
(54)

So, it turns out that under noisy conditions the optimal rotation finding
problem has a similar form as our original rotation optimization problem in
Eq. 22.

(ii) Translation noise component: Let the relative noisy translation
components for the lidar and the GNSS measurements between timestamps
$t_{i-1}$ and $t_{i}$ be $\boldsymbol{\delta}_{\mathtt{L}_{i}}$ and
$\boldsymbol{\delta}_{\mathtt{{B}}_{i}}$. Let,
$\boldsymbol{\tau}_{\mathtt{L}_{i}}=\mathbf{t}_{{\mathtt{L}_{i-1}}{\mathtt{L}_{i}}}$
and
$\boldsymbol{\tau}_{\mathtt{{B}}_{i}}=\mathbf{t}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}$.
Our optimization problem becomes,

$\displaystyle\mathbf{t}_{\mathtt{{B}}\mathtt{L}}^{\star}=\operatorname*{arg\,min}_{\mathbf{t}_{\mathtt{{B}}\mathtt{L}},\boldsymbol{\delta}_{\mathtt{{B}}_{i}},\boldsymbol{\delta}_{\mathtt{L}_{i}}}\sum_{i=1}^{N-1}\left(\|\boldsymbol{\delta}_{\mathtt{{B}}_{i}}\|^{2}_{\Sigma^{-1}_{\mathtt{{B}}\mathtt{{B}}}}+\|\boldsymbol{\delta}_{\mathtt{L}_{i}}\|^{2}_{\Sigma^{-1}_{\mathtt{L}\mathtt{L}}}\right)$
(55) (56)

Rearranging Eq. 56 we get,

(57)

Considering equal co-variance norms we can minimize the error term,
$\Xi(\boldsymbol{\delta}_{\mathtt{{B}}_{i}},\boldsymbol{\delta}_{\mathtt{L}_{i}})$
by setting the Jacobians equal to zero.

$\displaystyle\implies\frac{\partial\Xi}{\partial\boldsymbol{\delta}_{\mathtt{{B}}_{i}}}=0$
or,

$2\boldsymbol{\delta}_{\mathtt{{B}}_{i}}+2\mathbf{R}_{\mathtt{{B}}\mathtt{L}}\left[\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}^{T}}(\mathbf{R}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}-\mathbf{I})\mathbf{t}{{}_{\mathtt{{B}}\mathtt{L}}}+\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}^{T}}(\tau_{\mathtt{{B}}_{i}}+\boldsymbol{\delta}_{\mathtt{{B}}_{i}})-\tau_{\mathtt{L}_{i}}\right]=0$

$\displaystyle\text{or,
}2\boldsymbol{\delta}_{\mathtt{{B}}_{i}}+(\mathbf{R}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}-\mathbf{I})\mathbf{t}{{}_{\mathtt{{B}}\mathtt{L}}}-(\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}}\tau_{\mathtt{L}_{i}}-\tau_{\mathtt{{B}}_{i}})=0$
$\displaystyle\text{or,
}\boldsymbol{\delta}_{\mathtt{{B}}_{i}}=-\frac{1}{2}\left[(\mathbf{R}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}}-\mathbf{I})\mathbf{t}{{}_{\mathtt{{B}}\mathtt{L}}}-(\mathbf{R}{{}_{\mathtt{{B}}\mathtt{L}}}\mathbf{t}_{{\mathtt{L}_{i-1}}{\mathtt{L}_{i}}}-\mathbf{t}_{{\mathtt{{B}}_{i-1}}{\mathtt{{B}}_{i}}})\right].$
(58)

Thus, for the translation case we can also show that the original optimization
problem described in Eq. 42 holds true under noisy conditions as well.

##  V Experimental Results

To demonstrate the perforance of the online calibration system, we conducted
several experiments using real world data collected from the two vehicles
illustrated in Fig. 2 which have a different configuration.

###  V-A Dataset

Dataset collection details for the experiments
---
Data | Vehicle | Sensor setup | Length ($\mathrm{K}\mathrm{m}$) | Duration ($\mathrm{s}\mathrm{e}\mathrm{c}\mathrm{s}$)
Seq-1 | Bus | $\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}},\mathtt{R}_{\mathtt{L}},\mathtt{R}_{\mathtt{R}}$ | 1.473 | 136
Seq-2 | Bus | $\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}}$ | 2.086 | 135
Seq-3 | Bus | $\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}}$ | 1.089 | 50
Seq-4 | Truck | $\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}},\mathtt{R}_{\mathtt{L}},\mathtt{R}_{\mathtt{R}}$ | 0.478 | 67
Seq-5 | Truck | $\mathtt{F}_{\mathtt{L}},\mathtt{F}_{\mathtt{R}},\mathtt{R}_{\mathtt{L}},\mathtt{R}_{\mathtt{R}}$ | 1.212 | 73
Seq-6† | Truck | $\mathtt{F}_{\mathtt{L}}-\text{Top},\mathtt{F}_{\mathtt{R}}-\text{Top}$ | 0.224 | 164
Seq-7† | Truck | $\mathtt{F}_{\mathtt{L}}-\text{Top},\mathtt{F}_{\mathtt{R}}-\text{Top}$ | 1.088 | 263
†Front 2 top lidars mounted on the cab were used here.

TABLE I:

We use data from a GNSS receiver to produce ground truth (GT) motion
trajectories of the $\mathtt{{B}}$ frame. We analyze our results on 7
collected test sequences described in Table I.

###  V-B Online calibration results and repeatability analysis

For the online calibration process we start from a set of un-calibrated lidars
for which only the translation components from the vehicle CAD parameters are
known. A GT calibration is performed offline by refining the vehicle CAD
parameters. The refinement process analyzes known static feature positions
around the vehicle and matches the corresponding detected features between the
lidars. We compare our results to these GT parameters for both vehicles. We
measured translation and rotation error as:

$\displaystyle\Delta t$
$\displaystyle=\frac{1}{3}\sqrt{\|\widehat{\mathbf{t}}-\mathbf{t}_{\operatorname{GT}}\|^{2}_{F}},$
(59) $\displaystyle\Delta R$
$\displaystyle=\frac{180}{\pi}\operatorname{cos}^{-1}\left[\frac{1}{2}\left(\operatorname{Tr}(\widehat{\mathbf{R}}^{-1}\mathbf{R}_{\operatorname{GT}})-1\right)\right],$
(60)

where, $\Delta R$ is the rotation along the principle eigenvector of
$(\widehat{\mathbf{R}}^{-1}\mathbf{R}_{\operatorname{GT}})$. We summarize our
results in Table II and compare our performance to the Kabsch alignment [17]
technique.

In Table II, we show the average of $\Delta t$ and $\Delta R$ for the bus and
truck sequences. It can be seen that our online method produces better results
for translation components because of the bounded variable least square
optimization 9. Our method also produces comparable results for rotation
components outperforming Kabsh alignment in most scenarios.

Average performance comparison of calibration routines
---
Vehicle | Lidar position | Kabsch alignment | Ours online method
$\Delta t$ [$\mathrm{m}\text{/}$] |  $\Delta R$ [$\mathrm{\SIUnitSymbolDegree}\text{/}$] |  $\Delta t$ [$\mathrm{m}\text{/}$] |  $\Delta R$ [$\mathrm{\SIUnitSymbolDegree}\text{/}$]
Bus | $\mathtt{F}_{\mathtt{L}}$ | 10.603 | 4.370 | 0.766 | 4.619
$\mathtt{F}_{\mathtt{R}}$ | 16.484 | 2.167 | 0.333 | 2.069
$\mathtt{R}_{\mathtt{R}}$ | 3.301 | 0.249 | 0.413 | 0.285
$\mathtt{R}_{\mathtt{L}}$ | 3.246 | 0.257 | 0.359 | 0.500
Truck | $\mathtt{F}_{\mathtt{L}}$ | 9.384 | 11.968 | 0.318 | 5.301
$\mathtt{F}_{\mathtt{R}}$ | 8.583 | 2.566 | 0.340 | 2.006
$\mathtt{R}_{\mathtt{R}}$ | 2.536 | 1.192 | 0.311 | 2.113
$\mathtt{R}_{\mathtt{L}}$ | 2.453 | 0.594 | 0.506 | 1.118
| $\mathtt{F}_{\mathtt{L}}-\mathtt{Top}$ | 9.637 | 2.462 | 0.346 | 2.091
| $\mathtt{F}_{\mathtt{R}}-\mathtt{Top}$ | 8.907 | 3.387 | 0.388 | 2.141

TABLE II:  Figure 4: Estimated $t_{x}$, $t_{y}$ and Yaw for bus sequence-1
over different pose sizes. The GT calibration parameters are represented in
red color. Figure 5: Estimated $t_{x}$, $t_{y}$ and Yaw for truck sequence-4
over different pose sizes. The GT calibration parameters are represented in
red color.

We also show the calibration results for the individual transformation
components in Fig. 4 for bus and Fig. 5 truck for different trajectories.
Since, we compare the estimated lidar pose in sensor frame to the
corresponding GNSS pose in $\mathtt{{B}}$ frame for calibration we study the
effect of the size of the associated corresponding poses used for calibration.

As seen in both the sequences, the rotation components converges to the prior
GT calibration as the number of corresponding poses in the optimization
increases. We solved for the translation components using a bounded variable
problem ($\pm 0.3$\mathrm{m}$$) since the initial CAD estimates are reliable
and found it converged towards the GT parameters as well.

After running our algorithm we calibrate the lidars online as seen in Fig. 6.
Additionally, we demonstrate the online calibration process in real-time in
our supplementary video.

Figure 6: Online calibration results for sequence-4.

###  V-C Observability analysis

For our observability analysis we extract the information matrix from the
angular velocity signals.

Figure 7: Illustration of observability analysis for Seq-7 with angular
velocity.

We only choose to estimate calibration in segments for which there is enough
excitation in the angular velocity. These segments are highlighted in green in
Fig. 7.

The corresponding trajectory is shown in Fig. 8. We can see that the
trajectory segments selected for calibration correspond are those with
significant turning maneuvers.

Figure 8: Illustration of selected poses with observability analysis for
Seq-7.

##  VI Conclusion

We have presented an observability-aware online multi-lidar calibration
algorithm. The observability aware module selects trajectory sequences that
carry enough excitation for the different degrees of freedom required for
calibration. Additionally, we provide an analytical study of the effect of
noise on the estimated states used for calibration. We also provide a stopping
measure which halts the calibration when a satisfactory performance has been
achieved.

Our method offers equivalent performance for rotation estimate and superior
outcomes for translation estimate when compared to trajectory alignment
method. For future work we will develop a method which can generate a driving
route containing observability-aware trajectories which the vehicle can follow
to perform online calibration in a minimum amount of time possible.

##  VII Acknowledgements

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

## References

  * [1] M. He, H. Zhao, F. Davoine, J. Cui, and H. Zha, “Pairwise lidar calibration using multi-type 3D geometric features in natural scene,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2013, pp. 1828–1835.
  * [2] D.-G. Choi, Y. Bok, J.-S. Kim, and I. S. Kweon, “Extrinsic calibration of 2-d lidars using two orthogonal planes,” _IEEE Transactions on Robotics_ , vol. 32, no. 1, pp. 83–98, 2016.
  * [3] J. Brookshire and S. Teller, “Extrinsic calibration from per-sensor egomotion,” _Robotics: Science and Systems VIII_ , pp. 504–512, 2013.
  * [4] L. Heng, B. Li, and M. Pollefeys, “Camodocal: Automatic intrinsic and extrinsic calibration of a rig with multiple generic cameras and odometry,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2013, pp. 1793–1800.
  * [5] P. Furgale, J. Rehder, and R. Siegwart, “Unified temporal and spatial calibration for multi-sensor systems,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2013, pp. 1280–1286.
  * [6] K. Huang and C. Stachniss, “On geometric models and their accuracy for extrinsic sensor calibration,” in _International Conference on Robotics and Automation (ICRA)_. IEEE, 2018, pp. 4948–4954.
  * [7] J. Jiao, H. Ye, Y. Zhu, and M. Liu, “Robust odometry and mapping for multi-lidar systems with online extrinsic calibration,” _IEEE Transactions on Robotics_ , vol. 38, no. 1, pp. 351–371, 2021.
  * [8] J. Lv, X. Zuo, K. Hu, J. Xu, G. Huang, and Y. Liu, “Observability-aware intrinsic and extrinsic calibration of lidar-imu systems,” _IEEE Transactions on Robotics_ , 2022.
  * [9] J. Maye, P. Furgale, and R. Siegwart, “Self-supervised calibration for robotic systems,” in _IEEE Intelligent Vehicles Symposium_ , 2013, pp. 473–480.
  * [10] K. Hausman, J. Preiss, G. S. Sukhatme, and S. Weiss, “Observability-aware trajectory optimization for self-calibration with application to uavs,” _IEEE Robotics and Automation Letters_ , vol. 2, no. 3, pp. 1770–1777, 2017\.
  * [11] T. Schneider, M. Li, C. Cadena, J. Nieto, and R. Siegwart, “Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation,” _IEEE Sensors Journal_ , vol. 19, no. 10, pp. 3846–3860, 2019.
  * [12] R. Tsai and R. Lenz, “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration,” _IEEE Transactions on Robotics and Automation_ , vol. 5, no. 3, pp. 345–358, 1989.
  * [13] R. Horaud and F. Dornaika, “Hand-eye calibration,” _International Journal of Robotics Research_ , vol. 14, no. 3, pp. 195–210, 1995.
  * [14] M. Shah, R. D. Eastman, and T. Hong, “An overview of robot-sensor calibration methods for evaluation of perception systems,” in _Workshop on Performance Metrics for Intelligent Systems_ , 2012, pp. 15–20.
  * [15] J. Jiao, Y. Yu, Q. Liao, H. Ye, R. Fan, and M. Liu, “Automatic calibration of multiple 3D lidars in urban environments,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2019, pp. 15–20.
  * [16] S. Das, N. Mahabadi, A. Djikic, C. Nassir, S. Chatterjee, and M. Fallon, “Extrinsic calibration and verification of multiple non-overlapping field of view lidar sensors,” in _IEEE International Conference on Robotics and Automation_ , 2022, pp. 919–925.
  * [17] W. Kabsch, “A discussion of the solution for the best rotation to relate two sets of vectors,” _Crystallographica: Crystal Physics, Diffraction, Theoretical and General Crystallography_ , pp. 827–828, 1978.
  * [18] B. K. Horn, “Closed-form solution of absolute orientation using unit quaternions,” _Journal of the Optical Society of America_ , vol. 4, no. 4, pp. 629–642, 1987.
  * [19] K. Huang and C. Stachniss, “Extrinsic multi-sensor calibration for mobile robots using the Gauss-Helmert model,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 1490–1496.
  * [20] P. B. Davenport, _A vector approach to the algebra of rotations with applications_. National Aeronautics & Space Admin., 1968, vol. 4696.
  * [21] J. Zhang and S. Singh, “Low-drift and real-time lidar odometry and mapping,” _Autonomous Robots_ , vol. 41, no. 2, pp. 401–416, Feb. 2017\.
  * [22] D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,” _IEEE Transactions on Robotics_ , pp. 1–18, 2022.
  * [23] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2018, pp. 4758–4765.
  * [24] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2020, pp. 5135–5142.
  * [25] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” _IEEE Transactions on Robotics_ , vol. 38, no. 4, pp. 2053–2073, 2022.
  * [26] C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Faster-LIO: Lightweight tightly coupled lidar-inertial odometry using parallel sparse incremental voxels,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4861–4868, 2022.
  * [27] M. Pedley, “Tilt sensing using a three-axis accelerometer,” _Freescale semiconductor application note_ , vol. 1, pp. 2012–2013, 2013.
  * [28] D. W. Allan, “Statistics of atomic frequency standards,” _Proceedings of the IEEE_ , vol. 54, no. 2, pp. 221–230, 1966.
  * [29] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” _IEEE Transactions on Robotics_ , vol. 33, no. 1, pp. 1–21, 2017.
  * [30] J. Nocedal and S. J. Wright, _Numerical optimization_. Springer, 1999.
  * [31] K. Hoffman, _Linear algebra_. Prentice-Hall, 1971.
  * [32] S. Umeyama, “Least-squares estimation of transformation parameters between two point patterns,” _IEEE Transactions on Pattern Analysis & Machine Intelligence_, vol. 13, no. 04, pp. 376–380, 1991.
  * [33] P. Petersen, _Riemannian geometry_. Springer, 2006, vol. 171.