Extrinsic LiDAR/Ground Calibration Method Using 3D Geometrical Plane-Based Estimation

Abstract

This paper details a new extrinsic calibration method for scanning laser rangefinder that is precisely focused on the geometrical ground plane-based estimation. This method is also efficient in the challenging experimental configuration of a high angle of inclination of the LiDAR. In this configuration, the calibration of the LiDAR sensor is a key problem that can be be found in various domains and in particular to guarantee the efficiency of ground surface object detection. The proposed extrinsic calibration method can be summarized by the following procedure steps: fitting ground plane, extrinsic parameters estimation (3D orientation angles and altitude), and extrinsic parameters optimization. Finally, the results are presented in terms of precision and robustness against the variation of LiDAR’s orientation and range accuracy, respectively, showing the stability and the accuracy of the proposed extrinsic calibration method, which was validated through numerical simulation and real data to prove the method performance.

1. Introduction

1.1. Overview

With the evolution of technology, the 3D intelligent sensors have posed great challenges in signal processing, especially in their outstanding acquisition performance even in rough environment. Moving to road networks maintenance and transportation safety, the responsibility imposes itself in detecting and locating the road distortion (cracking, patching, potholes, rutting, shoving, etc.). The literature review in [1] presents different automated detection experiments and extensive research conducted on pavement adversity in recent years. The work shows the importance and the incredible progress of 3D sensors compared with the other sensors, especially the laser profiler that is characterized by its high precision measurement capability, high spatial resolution and acquisition flexibility.

In a related context, road defects present a big danger on the traffic circulation ending up with possible traffic accidents. Some traffic accidents result from the presence of disabilities or small obstacles on the roads. In fact, this is one of the major problems that people suffer from in their daily lives. The key problem in this research concerns the characterization of the road surface by detection, localization, and tracking the presence of potentially dangerous areas and road defects using a 3D LiDAR sensor. Although 2D LiDAR sensors can provide 3D data, they require the use of an additional instrument in the form of a tilt unit.

Various promising applications, which rely on LiDAR sensors, are developed in different fields: intelligent transportation systems, mobile robotics, and connected vehicles. Thus, LiDAR is a fundamental sensor contributing in multi-vehicles tracking [2], simultaneous localization and mapping [3,4], road and road boundaries detection [5,6], autonomous vehicles [7,8], recognition [9,10], and 3D reconstruction [11,12]. Almost all of these applications appear in world challenges such as DARPA Urban and Grand Challenge [13,14,15,16,17].

Basically, LiDAR sensor operation relies on the calibration process which improves the defect detection and other study procedure. Two different types of calibration exist: intrinsic and extrinsic calibration. The intrinsic calibration considers the modeling between the beam creation and measurement of the environment to estimate the sensor–environment relationship in terms of the sensor internal parameters. On the other hand, the extrinsic calibration considers the determination of the relationship between the sensor frame and the world reference frame by rotation and translation transformation.

1.2. Related Works

Numerous authors investigated intrinsic and extrinsic calibration methods in LiDAR sensors. An intrinsic calibration technique is presented in [18]. The calibration process is based on an optimization method, where the calibration pattern is a wide planar wall on a flat surface scanned using Velodyne HDL-64E. In addition, Glennie and Lichti [19] presented a static calibration technique to derive an optimal solution for the laser’s intrinsic calibration parameters by a planar feature-based least squares in advantage of minimal constrained network. The study in [20] shows a correlation between the internal operating temperature of the LiDAR and the Laser scanner ranging error (intrinsic parameter). The calibration process considers a planar calibration approach to estimate the internal parameters for Velodyne VLP-16.

On the other hand, an extrinsic calibration technique is presented in [21]. In this technique, a flat plane is used for the calibration and an algorithm based on the inequality of two symmetric rays in azimuth with respect to the origin is proposed. This inequality is due to the shift angle of the center line. Another extrinsic calibration technique is presented in [22], where the authors worked on a 2D laser scanner and on the rotating platform to extract the rotation axis and radius using point-plane constraint. The Levenberg–Marquardt optimization method is applied in the two above extrinsic calibration methods to solve the non-linear least squares function problem.

In [23], a numerical algorithm is presented to compute both of the intrinsic and extrinsic parameters by minimizing the systematic errors due to the geometric calibration factors. Another approach introduced in [24] computes the intrinsic and extrinsic parameters of LiDAR sensor (Velodyne HDL-64E) by unsupervised calibration for each of multi-laser beams. An optimization function seeking to minimize the point-to-plane iterated closest point is then proposed.

1.3. Proposed Method

In transport applications, many articles use LiDAR to detect and track objects of interest (vehicle, pedestrians, etc.) from 3D measurements. The LiDAR sensor is also used to detect the road, often in addition to camera sensors. In these applications, the idea is to have a thorough view of the driver’s environment over the widest possible horizon. This therefore involves a LiDAR sensor with a low angle of inclination (horizontally oriented sensor).

The perspective of our paper is to propose a calibration method (and road plane estimation) that works under difficult experimental conditions (high angle of inclination). Indeed, we aim at developing a calibration method that allows to determine precisely the road plane in a very close vicinity of the vehicle. The idea in the long term is to detect road defects when driving on the road network. Although developed with this in mind (i.e., with a high degree of accuracy in determining the road plan), our method is general enough to be applicable in any wider operational context.

In the context of this study (road defects detection), the LiDAR sensor is rotated toward the ground to increase the points’ density covering the defects by the multi elevation laser. This causes a complicated modification in the ground 3D view scene with respect to the LiDAR frame. Therefore, extrinsic calibration was adopted in order to transform the LiDAR frame into a global reference frame, thus modifying the ground impact points transformation into an understandable view scene.

To attain the above key objective, a first method was applied by Zaiter et al. [25] on simulation data, which was restricted to the Euler angles estimation. In this conference paper, we proposed a first approach to extrinsic calibration of LRF sensors. It has been developed for the Velodyne VLP-16 LiDAR and the theoretical approach have been evaluated on some simulation results.

This paper addresses a new flexible extrinsic calibration method as compared to the previous plane-based methods. The approach developed is generalized to all types of scanning laser rangefinders. It now presents an optimized estimation of all extrinsic calibration parameters (angles and height). This global method can be implemented on different LiDAR sensors (low-cost 3D and full 3D) with various range accuracy. In addition, the proposed technique out performs in high orientation scenarios, which is a very interesting and challenging task that aims to increase the points’ density coverage. The proposed calibration method can be summarized by the following two-fold contributions: (1) ground plane model estimation; and (2) rotation transformation matrix estimation from world ground reference to LiDAR sensor frame. The 3D Euler’s angles (sensor orientation) and the height (sensor altitude above the ground) are two essential extrinsic parameters required to calibrate the full 3D LiDAR sensors, in order to improve the capability of road defect detection as explained in Section 2.1. In addition, the problem is modeled by 4-DOF (degree of freedom) transformation, namely 3-DOF rotation and 1-DOF height, instead of 6-DOF transformation, namely 3-DOF rotation and 3-DOF translation. This modeling advantage provides the simplicity in the optimization process of the extrinsic parameters.

The structure of this paper is as follows. Section 2 presents the correlation between extrinsic parameters and the geometrical pattern reflection on the ground, the rotated multi-laser beams projection modeling on the ground and the associated measurement errors on the 3D points cloud position. Section 3 presents in detail the different steps of the proposed LiDAR/Ground Calibration Method. Then, the method is evaluated on LiDAR’s synthetic and real data in Section 4.

2. LiDAR/Ground Geometrical Impact Modeling

The synthetic data are generated depending on the features of multi-laser rangefinder or 3D LiDAR sensor, where the environment impact points can be modeled as an intersection between the LiDAR laser beams and the environmental surrounding surfaces. In this work, the LiDAR sensor must be oriented toward the ground to study the road defects. Therefore, the LiDAR laser beams are represented as straight lines and the ground surface as a flat plane in a 3D frame.

Depending on the application situation, two concepts can represent the geometrical reflection model between the LiDAR sensor and the ground surface, as shown in Figure 1:

• Practical orientation concept: The LiDAR laser beams $\left(d\right)$ are supposed to be rotated and the ground’s real plane $\left(P_{re}\right)$ is a fixed horizontal plane, as shown in Figure 1b.
• Scientific orientation concept: The LiDAR laser beams $\left(d\right)$ are supposed to be fixed and the virtual horizontal ground surface $\left(P_H\right)$ must be rotated by the LiDAR’s inverse orientation in the practical concept, to get the real oblique ground plane $\left(P_{re}\right)$ in LiDAR frame, as shown in Figure 1c.

2.1. Extrinsic Parameters vs. Practical Concept

The four extrinsic parameters (altitude and orientation angles) affect the research goals, where the orientation parameter is the strongest influence factor on the ground points distribution process, as shown in Figure 2a,b. The proposed calibration method must satisfy two contradictory conditions in relation to the final research objectives:

• Goal: The plane-based extrinsic calibration needs large sparsity area to improve the plane estimation, which requires high altitude and low orientation angles.
• Constraint: The stated finality of road surface object detection needs high density points to improve the capability of defect coverage points, which requires low altitude and high orientation angles.

Therefore, a trade-off is needed to optimize the extrinsic parameters (altitude and orientation angles), providing the suitable coverage points distribution over the ground.

Four geometric view patterns are summarized in three cases depending on the variation of pitch angle ${\varphi }_x$ with respect to the LiDAR’s vertical field of view “$vFOV$”, as shown in Figure 2: (1) circles patterns (Figure 2c); (2) combination of ellipses, parabola, and hyperbolas patterns (Figure 2d); and (3) hyperbolas patterns (Figure 2e).

The study case in this problem focuses on the hyperbolas case, as shown in Figure 2e, in order to increase the points density on the ground.

2.2. LiDAR Laser Beams and Oblique Ground Surface Intersection

In the following part, the scientific orientation concept is chosen to model the reflections of the laser beams $\left(d\right)$ on the real oblique ground surface $\left(P_{re}\right)$. Therefore, the LiDAR is supposed to be fixed and the parametric equations of the fixed straight lines $\left(d\right)$ in LiDAR frame are given by:

$$\left(d\right):\left\{\begin{array}{c}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{0}^{\circ }<\alpha <{90}^{\circ }or\hfill \\ {270}^{\circ }<\alpha <{360}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\ge 0\hfill \end{array}\right.\hfill \\ \left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ -t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{90}^{\circ }<\alpha <{270}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\le 0\hfill \end{array}\right.\hfill \end{array}\right.$$

(1)

where $\alpha$ and $\beta$ describe the azimuth and the elevation angles of each laser beam and t is the parameter of the parametric representation.

The virtual horizontal ground plane $\left(P_H\right)$ must be rotated by the 3D Euler’s angles ${\psi }_z,{\theta }_y,{\varphi }_x$, so that the equation of the rotated real ground plane $\left(P_{re}\right)$ is expressed as a function of the horizontal ground plane $\left(P_H\right)$ with height h and rotational matrix $R_{z,y,x}({\psi }_z,{\theta }_y,{\varphi }_x)$. This transformation is expressed as:

$$P_{re}=R_{z,y,x}({\psi }_z,{\theta }_y,{\varphi }_x)P_H$$

(2)

The parametric and Cartesian equations of the horizontal ground plane $\left(P_H\right)$ are expressed as follows:

$$\left(P_H\right):\left\{\begin{array}{cc}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t+aw\\ t+bw\\ -h\end{array}\right]\mathrm{parametric}\mathrm{equation}\hfill & \forall a,b\in \mathbb{R}\hfill \\ z+h=0\mathrm{Cartesian}\mathrm{equation}\hfill \end{array}\right.$$

(3)

The rotational matrix $R_{z,y,x}({\psi }_z,{\theta }_y,{\varphi }_x)$ is expressed as:

$$\begin{array}{c}{R}_{z,y,x}({\psi }_z,{\theta }_y,{\varphi }_x)=R_z\left({\psi }_z\right)R_y\left({\theta }_y\right)R_x\left({\varphi }_x\right)\hfill \\ =\left[\begin{array}{ccc}cos{\psi }_{z}cos{\theta }_y& \begin{array}{c}cos{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x\\ -sin{\psi }_{z}cos{\varphi }_x\end{array}& \begin{array}{c}cos{\psi }_{z}sin{\theta }_{y}cos{\varphi }_x\\ +sin{\psi }_{z}sin{\varphi }_x\end{array}\\ sin{\psi }_{z}cos{\theta }_y& \begin{array}{c}sin{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x\\ +cos{\psi }_{z}cos{\varphi }_x\end{array}& \begin{array}{c}sin{\psi }_{z}sin{\theta }_{y}cos{\varphi }_x\\ -cos{\psi }_{z}sin{\varphi }_x\end{array}\\ -sin{\theta }_y& cos{\theta }_{y}sin{\varphi }_x& cos{\theta }_{y}cos{\varphi }_x\end{array}\right]\hfill \end{array}$$

(4)

Therefore, the Cartesian coordinates of the real points cloud $c_{re}$ obtained from the intersection between the fixed straight lines $\left(d\right)$ and the rotated real plane $\left(P_{re}\right)$ are expressed as:

$$\left(c_{re}\right):\left\{\begin{array}{c}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{0}^{\circ }<\alpha <{90}^{\circ }or\hfill \\ {270}^{\circ }<\alpha <{360}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\ge 0\hfill \end{array}\right.\hfill \\ \forall t=\frac{(u{l}^{\prime }-u^{\prime }l)h{k}^{″}+(u^{\prime }k-u{k}^{\prime })h{l}^{″}+(l{k}^{\prime }-l^{\prime }k)h{u}^{″}}{(k^{\prime }{l}^{″}-l^{\prime }{k}^{″})tan\alpha +(l{k}^{″}-k{l}^{″})+(l^{\prime }k-l{k}^{\prime })\sqrt{tan{\alpha }^2+1}tan\beta }\hfill \\ \left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ -t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{90}^{\circ }<\alpha <{270}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\le 0\hfill \end{array}\right.\hfill \\ \forall t=\frac{(u{l}^{\prime }-u^{\prime }l)h{k}^{″}+(u^{\prime }k-u{k}^{\prime })h{l}^{″}+(l{k}^{\prime }-l^{\prime }k)h{u}^{″}}{(k^{\prime }{l}^{″}-l^{\prime }{k}^{″})tan\alpha +(l{k}^{″}-k{l}^{″})-(l^{\prime }k-l{k}^{\prime })\sqrt{tan{\alpha }^2+1}tan\beta }\hfill \end{array}\right.$$

(5)

where

$$\left.\begin{array}{cc}\hfill k& =cos{\psi }_{z}cos{\theta }_y+cos{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x-sin{\psi }_{z}cos{\varphi }_x\hfill \\ \hfill l& =acos{\psi }_{z}cos{\theta }_y+bcos{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x-bsin{\psi }_{z}cos{\varphi }_x\hfill \\ \hfill u& =cos{\psi }_{z}sin{\theta }_{y}cos{\varphi }_x+sin{\psi }_{z}sin{\varphi }_x\hfill \\ \hfill k^{\prime }& =sin{\psi }_{z}cos{\theta }_y+sin{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x+cos{\psi }_{z}cos{\varphi }_x\hfill \\ \hfill l^{\prime }& =asin{\psi }_{z}cos{\theta }_y+bsin{\psi }_{z}sin{\theta }_{y}sin{\varphi }_x+bcos{\psi }_{z}cos{\varphi }_x\hfill \\ \hfill u^{\prime }& =sin{\psi }_{z}sin{\theta }_{y}cos{\varphi }_x+cos{\psi }_{z}sin{\varphi }_x\hfill \\ \hfill k^{″}& =-sin{\theta }_y+cos{\theta }_{y}sin{\varphi }_x\hfill \\ \hfill l^{″}& =-asin{\theta }_y+bcos{\theta }_{y}sin{\varphi }_x\hfill \\ \hfill u^{″}& =cos{\theta }_{y}cos{\varphi }_x\hfill \end{array}\right\}\forall a\mathrm{and}b\in \mathbb{R}$$

(6)

2.3. Error Modeling in Polar and Cartesian Coordinates

In this section, the systematic and random errors $w_{\rho },w_{\alpha },w_{\beta }$ are taken in consideration as a source of error to represent the modeling of the additive white Gaussian noise for each polar coordinates $\rho ,\alpha ,\beta$ of the real points cloud $c_{re}$ in Equation (7), where ${\rho }_w,{\alpha }_w,{\beta }_w$ are the real measurements of the range $\rho$, azimuth $\alpha$ and elevation $\beta$, respectively, for each reflecting point.

$$\left(c_w\right):\left[\begin{array}{c}{\rho }_w\\ {\alpha }_w\\ {\beta }_w\end{array}\right]=\left[\begin{array}{c}\rho +w_{\rho }\\ \alpha +w_{\alpha }\\ \beta +w_{\beta }\end{array}\right]$$

(7)

The 3D transformation from polar coordinates ${\rho }_w,{\alpha }_w,{\beta }_w$ to Cartesian coordinates $x_w,y_w,z_w$ of the ground noisy points cloud $c_w$ is given by:

$$\left(c_w\right):\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{\rho }_{w}cos{\beta }_{w}sin{\alpha }_w\\ {\rho }_{w}cos{\beta }_{w}cos{\alpha }_w\\ {\rho }_{w}sin{\beta }_w\end{array}\right]$$

(8)

Then, the standard deviation of the error can be derived from polar to Cartesian parameters in Equation (9), assuming that ${\sigma }_{x_w},{\sigma }_{y_w},{\sigma }_{z_w},{\sigma }_{{\rho }_w},{\sigma }_{{\alpha }_w},{\sigma }_{{\beta }_w}$ are, respectively, the standard deviations of the added noise on $x_w,y_w,z_w,{\rho }_w,{\alpha }_w,{\beta }_w$. The terms of the standard deviations ${\sigma }_{{\rho }_w},{\sigma }_{{\alpha }_w},{\sigma }_{{\beta }_w}$ with a power higher than two can be neglected in this derivation to obtain this approximation:

$$\left[\begin{array}{c}{\sigma }_{x_w}^2\\ {\sigma }_{y_w}^2\\ {\sigma }_{z_w}^2\end{array}\right]\simeq \left[\begin{array}{c}{\sigma }_{{\rho }_w}^2{cos}^2{\beta }_w{sin}^2{\alpha }_w+{\rho }_w^2({\sigma }_{{\beta }_w}^2{sin}^2{\beta }_w{sin}^2{\alpha }_w+{\sigma }_{{\alpha }_w}^2{cos}^2{\beta }_w{cos}^2{\alpha }_w)\\ {\sigma }_{{\rho }_w}^2{cos}^2{\beta }_w{cos}^2{\alpha }_w+{\rho }_w^2({\sigma }_{{\beta }_w}^2{sin}^2{\beta }_w{cos}^2{\alpha }_w+{\sigma }_{{\alpha }_w}^2{cos}^2{\beta }_w{sin}^2{\alpha }_w)\\ {\sigma }_{{\rho }_w}^2{sin}^2{\beta }_w+{\rho }_w^2{\sigma }_{{\beta }_w}^2{cos}^2{\beta }_w\end{array}\right]$$

(9)

Glennie et al. [20] showed in particular that the error of a scanning LiDAR sensor is mainly manifested over range. In this type of sensor, angles are not directly measured, but the error is mainly related to the reproducibility of the measurement for a given angle. The hypothesis of neglecting the scanning angle error is a very common assumption in the field of LiDAR detection: it is part of the manufacturers’ specifications and is commonly used in the literature. This is particularly related to the very small influence of the angle reproducibility errors on the range measurement of the object of interest.

In this study, we then focus on the range error ${\sigma }_{{\rho }_w}$ and neglect the azimuth and elevation errors ${\sigma }_{{\alpha }_w},{\sigma }_{{\beta }_w}$, respectively, in the simulation data as given by the constructor. The transformation in Equation (9) is then simplified as:

$$\left[\begin{array}{c}{\sigma }_{x_w}\\ {\sigma }_{y_w}\\ {\sigma }_{z_w}\end{array}\right]=\left[\begin{array}{c}{\sigma }_{{\rho }_w}cos{\beta }_{w}sin{\alpha }_w\\ {\sigma }_{{\rho }_w}cos{\beta }_{w}cos{\alpha }_w\\ {\sigma }_{{\rho }_w}sin{\beta }_w\end{array}\right]$$

(10)

3. LiDAR/Ground Extrinsic Calibration Method

In multi-sensor applications, data acquired from the different sensors must be fused in one common reference frame. In this application, the calibration of LiDAR frame scans is necessary to merge them in one world reference frame, in order to increase the points density coverage on the ground, which facilitates the road defect detection. Therefore, the extrinsic calibration aims to model the relationship between the LiDAR frame and the world reference frame.

We thus propose the LiDAR/Ground Calibration Method (LGCM) presented in Figure 3. The method includes the following procedures: fitting ground plane by Least Squares estimator, rotation about axis by Rodrigues formula, Least Squares Conic Algorithm, and height estimation. The proposed method is supplemented by Levenberg–Marquardt optimization algorithm as opt-LGCM. The main role of LGCM procedure is to estimate the extrinsic parameters: the Euler’s rotational angles ${\psi }_z,{\theta }_y,{\varphi }_x$ and the height h. Then, the opt-LGCM is initialized by the estimated extrinsic parameters to optimize them. Finally, the distributed ground noisy points $c_w$ along the real plane $\left(P_{re}\right)$ are rotated along the horizontal plane $\left(P_H\right)$ by the optimized extrinsic parameters in the frame of fixed LiDAR.

The proposed method consists mainly in two steps. The first, totally unsupervised step, consists of estimating a first value of the steering angles. This first estimate is then used as a basis for the optimization step, which seeks the best orientation parameters. The developed method is therefore totally unsupervised and does not require a priori knowledge of the orientation of the sensor by a tilt unit for example.

3.1. Fitting Plane

The first step aims to fit an estimated plane $\left(P_{est}\right)$ with the rotated ground noisy points $c_w$. The Least Squares estimator is used to obtain the normal vector of the plane $\left(P_{est}\right)$.

The equation of the estimated plane $\left(P_{est}\right)$ in the LiDAR frame is expressed by:

$$f(x,y)=z=Ax+By+D+w$$

(11)

where A, B, and D are the plane parameters and w is an additive white Gaussian noise with standard deviation ${\sigma }_w$.

Therefore, Equation (11) of the estimated plane $\left(P_{est}\right)$ can be written in linear form as:

$$Z=H\mathcal{O}+w$$

(12)

where

$Z={\left[\begin{array}{ccc}z\left(0\right)& \cdots & z(N-1)\end{array}\right]}^T$

$H=\left[\begin{array}{ccc}x\left(0\right)& y\left(0\right)& 1\\ ⋮& ⋮& ⋮\\ x(N-1)& y(N-1)& 1\end{array}\right]$

$\mathcal{O}={\left[\begin{array}{ccc}A& B& D\end{array}\right]}^T$

$w={\left[\begin{array}{ccc}w\left(0\right)& \cdots & w(N-1)\end{array}\right]}^T$ where N is the number of reflected points.

The solution of Least Squares estimator for this linear model is expressed as:

$${\widehat{\mathcal{O}}}_{LS}={\left(H^{T}H\right)}^{-1}{H}^{T}Z$$

(13)

3.2. Rotation about Axis

Rodrigues formula is an efficient rotation transformation that computes the rotation matrix $R_{rod}$, which rotates a vector into another vector in 3D frame around a fixed axis vector $\overrightarrow{Axis}$ by rotational angle $\eta$ [26]. Therefore, after having estimated the parameter vector of the oblique estimated plane $\left(P_{est}\right)$ according to Section 3.1, the next step is to compute the rotational matrix $R_{rod}$ from the normal vector ${\overrightarrow{n}}_1$ of the oblique estimated plane $\left(P_{est}\right)$ to the normal vector ${\overrightarrow{n}}_2$ of the horizontal plane $\left(P_H\right)$ that is parallel to $X_{{}_L}{Y}_{{}_L}$-plane with height $-h$ (cf. Figure 1c). The objective of this step is to use Rodrigues formula in order to estimate the first two Euler’s angles pitch ${\widehat{\varphi }}_x$, roll ${\widehat{\theta }}_y$, and the first partial yaw angle ${\widehat{\mathsf{\Psi }}}_{z1}$—due to the incomplete calibration in yaw rotation, which is solved by the next step—from Rodrigues Matrix $R_{rod}$.

Assuming that ${\overrightarrow{n}}_1$, ${\overrightarrow{n}}_2$, and $\overrightarrow{Axis}$ are expressed as:

${\overrightarrow{n}}_1(-\widehat{A},-\widehat{B},1)$

${\overrightarrow{n}}_2(0,0,1)$

$\overrightarrow{Axis}(m,n,p)=\frac{{\overrightarrow{n}}_1\times {\overrightarrow{n}}_2}{\parallel {\overrightarrow{n}}_1\times {\overrightarrow{n}}_2\parallel }$, the Rodrigues rotation formula $R_{rod}$ can be then written as:

$$R_{rod}=I_3+sin\eta K+(1-cos\eta K^2)$$

(14)

where

$I_3=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

$K=\left[\begin{array}{ccc}0& -p& n\\ p& 0& -m\\ -n& m& 0\end{array}\right]$

$sin\eta =\frac{\parallel {\overrightarrow{n}}_1\times {\overrightarrow{n}}_2\parallel }{\parallel {\overrightarrow{n}}_1\parallel ·\parallel {\overrightarrow{n}}_2\parallel }$

$cos\eta =\frac{{\overrightarrow{n}}_1·{\overrightarrow{n}}_2}{\parallel {\overrightarrow{n}}_1\parallel ·\parallel {\overrightarrow{n}}_2\parallel }$

Now, by using Equation (15),

$$R_{rod}=R_{x,y,z}({\widehat{\mathsf{\Psi }}}_{z1},{\widehat{\theta }}_y,{\widehat{\varphi }}_x)=R_x\left({\widehat{\mathsf{\Psi }}}_{z1}\right)R_y\left({\widehat{\theta }}_y\right)R_z\left({\widehat{\varphi }}_x\right)$$

(15)

Then, the Rodrigues matrix $R_{rod}$ provides the computation of ${\widehat{\mathsf{\Psi }}}_{z1}$, ${\widehat{\theta }}_y$, and ${\widehat{\varphi }}_x$ as expressed in the equations below:

$$\begin{array}{cc}\hfill {\widehat{\mathsf{\Psi }}}_{z1}& =arctan({\left(R_{rod}\right)}_{21}/{\left(R_{rod}\right)}_{11})\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill {\widehat{\theta }}_y& =arcsin(-{\left(R_{rod}\right)}_{31})\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\widehat{\varphi }}_x& =arctan({\left(R_{rod}\right)}_{32}/{\left(R_{rod}\right)}_{33})\hfill \end{array}$$

(18)

where $ij$ represents the matrix element index of ${\left(R_{rod}\right)}_{ij}$. As a graphical result, the ground noisy points $c_w$ are rotated by Rodrigues matrix $R_{rod}$ to the distributed points $c_{{}_{H1}}$ along the horizontal plane $\left(P_H\right)$ by Equation (19), as shown in Figure 4.

$$c_{{}_{H1}}=R_{rod}{c}_w$$

(19)

3.3. Yaw Angle Estimation

After rotating the noisy points $c_w$ to the horizontal points $c_{{}_{H1}}$, the second partial yaw angle ${\widehat{\mathsf{\Psi }}}_{z2}$ is estimated by the efficient Algorithm 1 that we propose in Figure 5 to rotate the points $c_{{}_{H1}}$ to $c_{{}_{H2}}$ about z-axis. This algorithm is called Least Squares Conic Algorithm (LSCA), which takes advantage of the center s characteristic of the geometrical impact patterns (hyperbolas, parabolas, and circles) formed by the points $c_{{}_{H1}}$, as shown in Figure 5. The aim of this part is to compute yaw angle ${\widehat{\psi }}_z$ from the partial angles ${\widehat{\mathsf{\Psi }}}_{z1}$ and ${\widehat{\mathsf{\Psi }}}_{z2}$, as shown in Equation (20).

Algorithm 1: Least Squares Conic Algorithm.

Input: x,y,z,$\alpha$,$\beta$ of the distributed points $c_{{}_{H1}}$    Output: ${\widehat{\mathsf{\Psi }}}_{z2}$ 1 Fit the lines $\left(l\right)$ and $\left(l^{\prime }\right)$ that pass through the points at each $\zeta ={10}^{\circ }$ consecutive azimuth by Least Squares estimator. The solution of Least Squares estimator for linear model: ${\widehat{\mathcal{O}}}_{LS}={\left(H^{T}H\right)}^{-1}{H}^{T}Y$ where $Y=\left[\begin{array}{c}y\left(0\right)\\ ⋮\\ y(N-1)\end{array}\right]$, $H=\left[\begin{array}{cc}x\left(0\right)& 1\\ ⋮& ⋮\\ x(N-1)& 1\end{array}\right]$, ${\widehat{\mathcal{O}}}_{LS}=\left[\begin{array}{c}\widehat{m}\\ \widehat{b}\end{array}\right]$ 2 Compute the coordinates of the intersection points s of each two symmetric lines of $\left(l\right)$ and $\left(l^{\prime }\right)$. Assume that: $\left(l\right):y={\widehat{m}}_{1}x+{\widehat{b}}_1$ $\left(l^{\prime }\right):y={\widehat{m}}_{2}x+{\widehat{b}}_2$ Therefore, the intersection points s of the straight lines $\left(l\right)$ and $\left(l^{\prime }\right)$ are computed as follows: $x_s=\frac{{\widehat{b}}_2-{\widehat{b}}_1}{{\widehat{m}}_1-{\widehat{m}}_2}$,$y_s={\widehat{m}}_1\frac{{\widehat{b}}_2-{\widehat{b}}_1}{{\widehat{m}}_1-{\widehat{m}}_2}+{\widehat{b}}_1$ 3 Fit a line $\left(v\right)$ that passes through the intersection points s and the origin O by Least Squares estimator. The solution of Least Squares estimator: ${\widehat{\mathcal{O}}}_{LS}={\left(H^{T}H\right)}^{-1}{H}^{T}Y$ where $Y=\left[\begin{array}{c}y\left(0\right)\\ ⋮\\ y(N-1)\end{array}\right]$, $H=\left[\begin{array}{c}x\left(0\right)\\ ⋮\\ x(N-1)\end{array}\right]$, ${\widehat{\mathcal{O}}}_{LS}=\left[\begin{array}{c}\widehat{m}\end{array}\right]$ 4 Finally, compute the angle ${\widehat{\mathsf{\Psi }}}_{z2}$ formed by the fitting line $\left(v\right)$ and y-axis: ${\widehat{\mathsf{\Psi }}}_{z2}=arctan\widehat{m}-{90}^{\circ }\mathrm{if}\widehat{m}>0$ ${\widehat{\mathsf{\Psi }}}_{z2}=arctan\widehat{m}+{90}^{\circ }\mathrm{if}\widehat{m}<0$

Therefore, the third Euler’s angle of rotation (yaw angle) ${\widehat{\psi }}_z$ is computed as follows:

$${\widehat{\psi }}_z={\widehat{\mathsf{\Psi }}}_{z1}-{\widehat{\mathsf{\Psi }}}_{z2}$$

(20)

Finally, the points $c_{{}_{H1}}$ are rotated to points $c_{{}_{H2}}$ by an angle $-{\widehat{\mathsf{\Psi }}}_{z2}$ around z-axis, as shown in Figure 6.

$$c_{{}_{H2}}=R_z(-{\widehat{\mathsf{\Psi }}}_{z2})c_{{}_{H1}}$$

(21)

3.4. Height Estimation

At the end of LGCM approach, a suitable way to estimate the height is to compute the altitude mean of the points $c_{{}_{H2}}$, due to the ground geometrical model used in this paper. Therefore, the estimated height is then expressed as:

$$\widehat{h}=\frac{1}{N}\sum _{i=0}^{N-1}{z}_i\mathrm{where}N\mathrm{is}\mathrm{the}\mathrm{number}\mathrm{of}\mathrm{calibrated}\mathrm{points}$$

(22)

3.5. Extrinsic Parameters Optimization

Levenberg–Marquardt algorithm is an optimization algorithm that combines gradient descent and Gauss–Newton methods [27]. In addition, it is a very efficient technique to find the minima and it performs well on most non-linear functions. Therefore, the role of opt-LGCM is to optimize the extrinsic parameters ${\psi }_z,{\theta }_y,{\varphi }_x,h$ by Levenberg–Marquardt algorithm, which is initialized by the estimated extrinsic parameters ${\widehat{\psi }}_z,{\widehat{\theta }}_y,{\widehat{\varphi }}_x,\widehat{h}$ to obtain the optimized extrinsic parameters ${\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″},{\widehat{h}}_{opt}$, in order to minimize the mean square error $mse$ that represents the square difference between the position of noisy points $c_w$ and the optimized position of points $c_{opt}$ in Equation (24). The optimized points $c_{opt}$ represent the intersection between all the LiDAR beams $\left(d\right)$ and the optimized plane $\left(P_{opt}\right)$, which is the rotation of the horizontal plane $\left(P_H\right)$ by the new optimized Euler’s angles ${\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″}$ and the optimized height ${\widehat{h}}_{opt}$ in Equation (25). In other words, the importance of the above procedure is to get the optimized height ${\widehat{h}}_{opt}$ and the optimized Euler’s angles ${\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″}$ that rotate in the inverse ordering orientation the horizontal plane $\left(P_H\right)$, to fit the noisy points $c_w$ that are distributed along the oblique real plane $\left(P_{re}\right)$ with minimum $mse$ on the position.

$$({\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″},{\widehat{h}}_{opt})=\underset{({\psi }_z,{\theta }_y,{\varphi }_x,h)}{argmin}mse$$

(23)

The non-linear function $mse$ is expressed by:

$$mse=\frac{1}{m}\sum _{i=1}^m\left(\begin{array}{c}{(x_{c_{opt}}-x_{c_w})}^2+{(y_{c_{opt}}-y_{c_w})}^2+{(z_{c_{opt}}-z_{c_w})}^2\end{array}\right)$$

(24)

The optimized points $c_{opt}$ represent the intersection between the straight lines $\left(d\right)$ and the rotated optimized plane $\left(P_{opt}\right)$, where the plane $\left(P_{opt}\right)$ is the rotation of the fixed ground horizontal plane $\left(P_H\right)$ of height ${\widehat{h}}_{opt}$ by $-{\widehat{\psi }}_z^{″},-{\widehat{\theta }}_y^{″},-{\widehat{\varphi }}_x^{″}$ based on $R_{x,y,z}$ rotation matrix, as shown in Equation (25):

$$P_{opt}=R_{x,y,z}(-{\widehat{\psi }}_z^{″},-{\widehat{\theta }}_y^{″},-{\widehat{\varphi }}_x^{″})P_H$$

(25)

where the rotation matrix $R_{x,y,z}$ is the reverse of $R_{z,y,x}$.

$$\begin{array}{c}{R}_{x,y,z}(-{\widehat{\psi }}_z^{″},-{\widehat{\theta }}_y^{″},-{\widehat{\varphi }}_x^{″})=R_x(-{\widehat{\varphi }}_x^{″})R_y(-{\widehat{\theta }}_y^{″})R_z(-{\widehat{\psi }}_x^{″})\hfill \\ =\left[\begin{array}{ccc}cos{\widehat{\psi }}_x^{″}cos{\widehat{\theta }}_y^{″}& sin{\widehat{\psi }}_x^{″}cos{\widehat{\theta }}_y^{″}& -sin{\widehat{\theta }}_y^{″}\\ \begin{array}{c}cos{\widehat{\psi }}_x^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\\ -sin{\widehat{\psi }}_x^{″}cos{\widehat{\varphi }}_x^{″}\end{array}& \begin{array}{c}sin{\widehat{\psi }}_x^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\\ +cos{\widehat{\psi }}_x^{″}cos{\widehat{\varphi }}_x^{″}\end{array}& cos{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\\ \begin{array}{c}cos{\widehat{\psi }}_x^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\\ +sin{\widehat{\psi }}_x^{″}sin{\widehat{\varphi }}_x^{″}\end{array}& \begin{array}{c}sin{\widehat{\psi }}_x^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\\ -cos{\widehat{\psi }}_x^{″}sin{\widehat{\varphi }}_x^{″}\end{array}& cos{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\end{array}\right]\hfill \end{array}$$

(26)

Finally, the Cartesian coordinates of rotated optimized points cloud $c_{opt}$ in the reverse orientation sense are estimated by ${\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″},{\widehat{h}}_{opt}$, as expressed below:

$$\left(c_{opt}\right):\left\{\begin{array}{c}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{0}^{\circ }<\alpha <{90}^{\circ }or\hfill \\ {270}^{\circ }<\alpha <{360}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\ge 0\hfill \end{array}\right.\hfill \\ \forall t=\frac{(u{l}^{\prime }-u^{\prime }l){\widehat{h}}_{opt}{k}^{″}+(u^{\prime }k-u{k}^{\prime }){\widehat{h}}_{opt}{l}^{″}+(l{k}^{\prime }-l^{\prime }k){\widehat{h}}_{opt}{u}^{″}}{(k^{\prime }{l}^{″}-l^{\prime }{k}^{″})tan\alpha +(l{k}^{″}-k{l}^{″})+(l^{\prime }k-l{k}^{\prime })\sqrt{tan{\alpha }^2+1}tan\beta }\hfill \\ \left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}t\ast tan\alpha \\ t\\ -t\ast \sqrt{{tan}^2\alpha +1}\ast tan\beta \end{array}\right]\mathrm{for}\left\{\begin{array}{c}{90}^{\circ }<\alpha <{270}^{\circ }\hfill \\ -{90}^{\circ }<\beta <{90}^{\circ }\hfill \\ t\le 0\hfill \end{array}\right.\hfill \\ \forall t=\frac{(u{l}^{\prime }-u^{\prime }l){\widehat{h}}_{opt}{k}^{″}+(u^{\prime }k-u{k}^{\prime }){\widehat{h}}_{opt}{l}^{″}+(l{k}^{\prime }-l^{\prime }k){\widehat{h}}_{opt}{u}^{″}}{(k^{\prime }{l}^{″}-l^{\prime }{k}^{″})tan\alpha +(l{k}^{″}-k{l}^{″})-(l^{\prime }k-l{k}^{\prime })\sqrt{tan{\alpha }^2+1}tan\beta }\hfill \end{array}\right.$$

(27)

where

$$\left.\begin{array}{cc}\hfill k& =cos{\widehat{\psi }}_z^{″}cos{\widehat{\theta }}_y^{″}+sin{\widehat{\psi }}_z^{″}cos{\widehat{\theta }}_y^{″}\hfill \\ \hfill l& =acos{\widehat{\psi }}_z^{″}cos{\widehat{\theta }}_y^{″}+bsin{\widehat{\psi }}_z^{″}cos{\widehat{\theta }}_y^{″}\hfill \\ \hfill u& =-sin{\widehat{\theta }}_y^{″}\hfill \\ \hfill k^{\prime }& =-sin{\widehat{\psi }}_z^{″}cos{\widehat{\varphi }}_x^{″}+cos{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}+cos{\widehat{\psi }}_z^{″}cos{\widehat{\varphi }}_x^{″}+sin{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\hfill \\ \hfill l^{\prime }& =-asin{\widehat{\psi }}_z^{″}cos{\widehat{\varphi }}_x^{″}+acos{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}+bcos{\widehat{\psi }}_z^{″}cos{\widehat{\varphi }}_x^{″}+bsin{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\hfill \\ \hfill u^{\prime }& =cos{\widehat{\theta }}_y^{″}sin{\widehat{\varphi }}_x^{″}\hfill \\ \hfill k^{″}& =sin{\widehat{\psi }}_z^{″}sin{\widehat{\varphi }}_x^{″}+cos{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}-cos{\widehat{\psi }}_z^{″}sin{\widehat{\varphi }}_x^{″}+sin{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\hfill \\ \hfill l^{″}& =asin{\widehat{\psi }}_z^{″}sin{\widehat{\varphi }}_x^{″}+acos{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}-bcos{\widehat{\psi }}_z^{″}sin{\widehat{\varphi }}_x^{″}+bsin{\widehat{\psi }}_z^{″}sin{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\hfill \\ \hfill u^{″}& =cos{\widehat{\theta }}_y^{″}cos{\widehat{\varphi }}_x^{″}\hfill \end{array}\right\}\forall a\mathrm{and}b\in \mathbb{R}$$

(28)

4. Experimental Results

The proposed calibration method LGCM was applied on two types of data: simulation data were obtained by the modeling mentioned in Section 2 and real data acquisition from the Velodyne VLP-16 LiDAR. The most important features of Velodyne VLP-16 LiDAR for modeling are shown in

Table 1. VLP-16 features.

Features	VLP-16
Laser beams	16
Horizontal FOV	${360}^{\circ }$
Vertical FOV	$-{15}^{\circ }\to +{15}^{\circ }$
Azimuth angular resolution	${0.1}^{\circ }–{0.2}^{\circ }–{0.4}^{\circ }$
Elevation angular resolution	$2^{\circ }$
Maximum range accuracy ${\sigma }_{\rho }$	$3 \mathrm{cm}$

.

The extrinsic calibration results are presented in terms of precision and robustness. According to our application, the precision shows the stability of the method with respect to the variation of pitch angle ${\varphi }_x$ toward the ground, while the robustness shows the method strength with respect to the variation of range accuracy ${\sigma }_{\rho }$ of the measurements.

Therefore, the evaluation parameters of the results focus on the point cloud features of the real points $c_{re}$ on the real plane $\left(P_{re}\right)$, noisy points $c_w$ distributed along the real plane $\left(P_{re}\right)$, estimated points $c_{est}$ on the estimated plane $\left(P_{est}\right)$ obtained by LGCM, and the optimized points $c_{opt}$ on the optimized plane $\left(P_{opt}\right)$ obtained by opt-LGCM, as described below:

• The real height h, estimated height $\widehat{h}$, and the optimized height ${\widehat{h}}_{opt}$.
• The standard deviation ${\sigma }_{d_{w/i}}$ of the noisy points $c_w$ orthogonal Euclidean distance with respect to the real plane $\left(P_{re}\right)$, the estimated plane $\left(P_{est}\right)$ and the optimized plane $\left(P_{opt}\right)$.

  $$\begin{array}{cc}\hfill & {\sigma }_{d_{w/i}}=\sqrt{\frac{1}{N}\sum {(d_{w/i}-\overline{d_{w/i}})}^2}\hfill \end{array}$$

  (29)

  $$\begin{array}{cc}\hfill & d_{w/i}=\frac{|A_i{x}_w+B_i{y}_w+C_i{z}_w+D_i|}{\sqrt{A_i^2+B_i^2+C_i^2}}\hfill \end{array}$$

  (30)

  where $x_w,y_w,z_w$ are the Cartesian coordinates of the noisy points $c_w$, $A_i,B_i,C_i,D_i$ are the coefficient parameters of the planes, $i=\{re,est,opt\}$, and N is the number of impact points.
• The standard deviation ${\sigma }_{{\rho }_{re}/{\rho }_i}$ of the real points $c_{re}$ range difference with respect to the noisy points $c_w$, the estimated points $c_{est}$, and the optimized points $c_{opt}$.

  $${\sigma }_{{\rho }_{re}/{\rho }_i}=\sqrt{\frac{1}{N}\sum {(({\rho }_{re}-{\rho }_i)-\left(\overline{{\rho }_{re}-{\rho }_i}\right))}^2}$$

  (31)

  where $i=\{w,est,opt\}$ and N is the number of impact points.
• The standard deviation ${\sigma }_{{\rho }_w/{\rho }_i}$ of the noisy points $c_w$ range difference with respect to the real points $c_{re}$, the estimated points $c_{est}$, and the optimized points $c_{opt}$.

  $${\sigma }_{{\rho }_w/{\rho }_i}=\sqrt{\frac{1}{N}\sum {(({\rho }_w-{\rho }_i)-\left(\overline{{\rho }_w-{\rho }_i}\right))}^2}$$

  (32)

  where $i=\{re,est,opt\}$ and N is the number of impact points.
• The gain in performance $P{F}_i$ that describes the range accuracy enhancement obtained from the Levenberg–Marquardt optimization algorithm which is defined as:

  $$P{F}_i={\sigma }_{{\rho }_w/{\rho }_i}-{\sigma }_{{\rho }_w/{\rho }_{re}}$$

  (33)

  where ${\sigma }_{{\rho }_w/{\rho }_{re}}$ is the LiDAR range accuracy and $i=\{est,opt\}$.

4.1. Simulation Data Results

Using the simulation data, the setups used to validate the proposed calibration method are separated in two categories:

• In term of precision, the real height $h=2 \mathrm{m}$, roll angle ${\theta }_y=2^{\circ }$, yaw angle ${\psi }_z=2^{\circ }$, and LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}=0.03 \mathrm{m}$, with respect to the variation of pitch angle ${\varphi }_x=[-{70}^{\circ },{70}^{\circ }]$.
• In term of robustness, the real height $h=2 \mathrm{m}$, pitch angle ${\varphi }_x={45}^{\circ }$, roll angle ${\theta }_y=2^{\circ }$, and yaw angle ${\psi }_z=2^{\circ }$, with respect to the variation of ${\sigma }_{{\rho }_w/{\rho }_{re}}=[0,0.095 \mathrm{m}]$.

4.1.1. Standard Deviation ${\sigma }_{d_{w/i}}$ in Terms of Precision and Robustness

After analyzing Figure 7a, the increasing of standard deviation ${\sigma }_{d_{w/i}}$ along the planes is due to the orientation effect of the LiDAR by the pitch angle ${\varphi }_x$ on ${\sigma }_{d_{w/i}}$. Thus, as pitch angle ${\varphi }_x$ tends to ${90}^{\circ }$, the standard deviation ${\sigma }_{d_{w/i}}$ tends to the LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$. In Figure 7b, the increasing of the standard deviation ${\sigma }_{d_{w/i}}$ is due to increasing of LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$. Moreover, Equation (34) describes the relation of ${\sigma }_{d_{w/i}}$ with ${\varphi }_x$ and ${\sigma }_{{\rho }_w/{\rho }_{re}}$,which proves the increasing of ${\sigma }_{d_{w/i}}$.

$${\sigma }_{d_{w/i}}=sin({\varphi }_x+{\phi }_k){\sigma }_{{\rho }_w/{\rho }_{re}}$$

(34)

where ${\phi }_k$ is the elevation angle of each VLP-16 LiDAR laser, and $k=\{1,2,\dots ,16\}$ represents the laser index.

In Figure 7c,d, we can see that the standard deviation ${\sigma }_{d_{w/opt}}$ is closer to the the standard deviation ${\sigma }_{d_{w/re}}$ than the standard deviation ${\sigma }_{d_{w/est}}$. This shows that the optimized plane $\left(P_{opt}\right)$ is better fit to the real plane $\left(P_{re}\right)$ than the estimated plane $\left(P_{est}\right)$.

4.1.2. Standard Deviation ${\sigma }_{{\rho }_{re}/{\rho }_i}$ and ${\sigma }_{{\rho }_w/{\rho }_i}$ in Terms of Precision and Robustness

In terms of precision and robustness, Figure 8 shows the increasing behavior of the range standard deviations ${\sigma }_{{\rho }_{re}/{\rho }_{est}}$ and ${\sigma }_{{\rho }_w/{\rho }_{est}}$ after the LGCM calibration, due to:

• The increase of pitch angle ${\varphi }_x$ on positive and negative sides decreases the sparsity of impact points on the ground. This leads to decrease the precision of plane fitting estimation, as shown in Figure 8a,c.
• The increase of LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$ decreases the precision of plane fitting estimation, as shown in Figure 8b,d.

The standard deviation ${\sigma }_{{\rho }_{re}/{\rho }_{opt}}$ is lower than the standard deviation ${\sigma }_{{\rho }_{re}/{\rho }_{est}}$, as shown in Figure 8a,b, which indicate how the optimized points $c_{opt}$ are closer to the real points $c_{re}$ than the estimated points $c_{est}$. On the other hand, the standard deviation ${\sigma }_{{\rho }_w/{\rho }_{opt}}$ is closer to LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$ than the standard deviation ${\sigma }_{{\rho }_w/{\rho }_{est}}$, as shown in Figure 8c,d, which indicate the equality of the noisy points $c_w$ range distribution along the real plane $\left(P_{re}\right)$ and the optimized plane $\left(P_{opt}\right)$.

The negligible standard deviation ${\sigma }_{{\rho }_{re}/{\rho }_{opt}}$ in Figure 8a,b and the coincident standard deviations ${\sigma }_{{\rho }_w/{\rho }_{opt}}$ and ${\sigma }_{{\rho }_w/{\rho }_{re}}$ in Figure 8c,d prove the similarity of the real plane $\left(P_{re}\right)$ and the optimized plane $\left(P_{opt}\right)$ compare to the estimated plane $\left(P_{est}\right)$.

4.1.3. Height Recovering in Terms of Precision and Robustness

In terms of precision and robustness, Figure 9 highlights the recovering of the height parameter and how the optimized height ${\widehat{h}}_{opt}$ is closer to the real height h than the estimated height $\widehat{h}$, which presents the height optimization importance and the strength of the Levenberg–Marquardt optimization algorithm.

4.1.4. Performance Gain $P{F}_i$ in Terms of Precision and Robustness

Figure 10 shows the gain in performance of the optimized plane points $c_{opt}$ against the estimated plane points $c_{est}$ distributed by the noisy points $c_w$ compared to the LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$ as expressed in Equation (33), with respect to the variation of pitch angle ${\varphi }_x$ and LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$. Moreover, the negligibility of the method performance $P{F}_{opt}$ appears after the optimization, which means that the standard deviation ${\sigma }_{{\rho }_w/{\rho }_{opt}}$ after optimization is closer to the standard deviation ${\sigma }_{{\rho }_w/{\rho }_{est}}$ before optimization with respect to the LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$. In addition, it presents the recovering of noisy points $c_w$ range distribution along the real plane $\left(P_{re}\right)$ after the optimization algorithm, taking advantage of maintaining the standard deviation ${\sigma }_{{\rho }_w/{\rho }_{opt}}$ value as negligible. The gain feature $P{F}_i$ proves again the better fit between the optimized plane $\left(P_{opt}\right)$ and the real plane $\left(P_{re}\right)$ rather than the estimated plane $\left(P_{est}\right)$.

4.2. Real Data Results

The 3D point cloud acquisitions were obtained using a multi-lasers rangefinder VLP-16 LiDAR mounted on a vehicle. To obtain a telemetric information about the ground surface and to achieve the application goal, the VLP-16 LiDAR was rotated toward the ground direction with a pitch angle ${\varphi }_x\simeq {70}^{\circ }$, and it was at a height of $h\simeq 1.05 \mathrm{m}$ above the ground surface. The real setup is shown in Figure 11.

The proposed method was applied to two different acquisitions:

• Acquisition 1: The vehicle was at rest on the road.
• Acquisition 2: The vehicle was moving at a slow speed on the road.

Standard Deviation ${\sigma }_{{\rho }_w/{\rho }_i}$ per LiDAR Frames

In the absence of real plane $\left(P_{re}\right)$ when using real data, the results focus on the range distribution of the noisy points $c_w$ along the estimated plane $\left(P_{est}\right)$ and the optimized plane $\left(P_{opt}\right)$. It is clear that the standard deviations ${\sigma }_{{\rho }_w/{\rho }_{opt}}$ curve is lower than the ${\sigma }_{{\rho }_w/{\rho }_{est}}$ in the two acquisitions, as shown in Figure 12. The optimization algorithm is thus proved to be more efficient for real data as well in decreasing the range distribution of the noisy points $c_w$ along the fitting planes.

4.3. Results Discussion

In general, the results prove the efficiency of the optimization algorithm, which is represented by the optimized plane $\left(P_{opt}\right)$, versus the estimated plane $\left(P_{est}\right)$, compared with the real plane $\left(P_{re}\right)$, in terms of precision and robustness. On the other hand, the convergence of the optimization algorithm is granted automatically by the suitable initialization parameters, namely the estimated Euler’s angles ${\widehat{\psi }}_z,{\widehat{\theta }}_y,{\widehat{\varphi }}_x$ and the estimated height $\widehat{h}$, which are computed in Stage 1 (LGCM) to obtain the estimated plane $\left(P_{est}\right)$, and then optimized by Levenberg–Marquardt optimization algorithm (opt-LGCM) in Stage 2 to get the optimized Euler’s angles ${\widehat{\psi }}_z^{″},{\widehat{\theta }}_y^{″},{\widehat{\varphi }}_x^{″}$ and the optimized height ${\widehat{h}}_{opt}$ to obtain the optimized plane $\left(P_{opt}\right)$. Finally, the results show the strength and the method performance in terms of precision and robustness against the variation of pitch angle ${\varphi }_x$ and LiDAR range accuracy ${\sigma }_{{\rho }_w/{\rho }_{re}}$, respectively, in order to achieve the application’s aim, as shown in Figure 13.

5. Conclusions

A new extrinsic LiDAR/Ground calibration method for 3D LiDARs is presented in this paper. The solution relies on plane-based modeling of the ground, which allows the estimation of the LiDAR’s orientation and altitude using Rodrigues formula, Least Squares Conic Algorithm for yaw angle estimation and height estimation. The proposed method (LGCM) is extended to an optimized derivation (opt-LGCM) using the Levenberg–Marquardt algorithm and is shown to be a suitable solution to LiDAR/Ground calibration problem. It is implemented on synthetic and real LiDAR telemetric data. The results show the performance in terms of precision and robustness against the variation of LiDAR’s orientation and range accuracy, respectively, proving the stability and the accuracy of the proposed calibration method.