# Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation
## 3 Methodology
### Point-Cloud De-Skewing  
Due to the mechanical rotation mechanism of the 3D Lidar, the movement of the sensor can occur while scanning, which directly impacts the point in a frame scan, making it differ from the true position. At present, the scanning frequency of Lidar is generally 10 Hz, and existing works [12; 18] have mostly assumed that the robot moves at a constant angular velocity and linear velocity within one frame scanning period, which does not always hold. In this paper, we jointly combine IMU preintegration and the GP to de-skew the point cloud by taking the measurements from IMU propagation as the training points in the GP IMU preintegration can avoid repeated integrations once the linearization point at time \(t_{i}\) changes, and it is more convenient to present the relative motion increments between two time steps. The formulation of IMU preintegration can be simplified as follows:  
\[ \begin{array}{l}\Delta\mathbf{R}_{ij}=\mathbf{R}_{i}^{T}\mathbf{R}_{j}\\ \Delta\mathbf{v}_{ij}=\mathbf{R}_{i}^{T}\big{(}\mathbf{v}_{j}-\mathbf{v}_{i}- \mathbf{v}_{i}\Delta t_{ij}\big{)}\\ \Delta\mathbf{p}_{ij}=\mathbf{R}_{i}^{T}\big{(}\mathbf{p}_{j}-\mathbf{p}_{i}- \mathbf{v}_{i}\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}\big{)}\end{array}  \tag{1}\]  
where \(\mathbf{v}_{ij}\), \(\mathbf{p}_{ij}\), and \(\mathbf{R}_{ij}\) are used to represent the velocity, position, and acceleration of mobile robots at time \(i\); \(\mathbf{g}\) is the constant gravity vector in world coordinates; and \(\Delta\mathbf{v}_{ij}\), \(\Delta\mathbf{p}_{ij}\), and \(\Delta\mathbf{R}_{ij}\) denote the preintegrated measurements between times \(i\) and \(j\). Detailed information on IMU preintegration can be found in [23], and we do not repeat it here. On the one hand, thanks to the development of SLAM back-end optimization, IMU preintegration factors and Lidar odometry factors can be directly added to the factor graph as different constraint types for joint optimization. On the other hand, we use IMU preintegration as the training set in the GP to correct the distortion Lidar scan, given the point-wise timestamp of each Lidar frame. The GP can be regarded as a generalization of a Gaussian random variable to the continuous-time domain, described by the mean function \(m(K)\) and covariance function \(k(k,k^{\prime})\)[28]. The process \(f(t)\) over time dimension \(t\) can be defined by the mean function \(m(t)\) and the covariance function \(k(t,t^{\prime})\) as  
\[m(t)=\mathbb{E}[f(t)] \tag{2}\]  
Figure 1: The system structure. \[k\big{(}t,t^{\prime}\big{)}=\mathbb{E}\big{[}(f(t)-m(t))\big{(}f\big{(}t^{\prime} \big{)}-m\big{(}t^{\prime}\big{)}\big{)}\big{]}, \tag{3}\]  
with the GP written as  
\[f(t)\sim\mathcal{GP}\big{(}m(t),k\big{(}t,t^{\prime}\big{)}\big{)} \tag{4}\]  
without a loss of generality, where the index set of the random variables is the time dimension \(t\). The covariance function uses the squared exponential as the kernel function, involving two time variables to account for cross-temporal relations. Assuming that \(t_{s}\) and \(t_{e}\) are the measurement timestamps of the beginning and end of a frame scan, respectively, the relative transform \(\Delta\mathbf{T}_{t}\in\text{SE}(3)\subset\mathbb{R}^{4\times 4},t_{i}\in(t_{s},t_{e}]\) between two sequential frames from \(k-1\) to \(k\) can be accurately estimated using (2). Let \(\mathbf{T}_{k}\) be the keyframe pose at time \(k\); the transform within the period \((t_{s},t_{e}]\) can be calculated as follows:  
\[\mathbf{T}_{k+t_{i}}=\mathbf{T}_{k}\cdot\Delta\mathbf{T}_{t_{i}}. \tag{5}\]  
Since the timestamp of the IMU preintegration within the period \((t_{s},t_{e}]\) and the corresponding pose information are known, the GP can correct the distortion efficiently by referring to the timestamp information of the point cloud. The detailed process of point-cloud de-skewing is shown in Algorithm 1.