# Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation
## 3 Methodology
### Point-Cloud De-Skewing
We artificially add different noises together to highlight the contribution of the noise and reformulate the zero-mean noise term \(\delta\mathbf{n}_{l}\). It can be seen that in the specific form of the noise term, the third term of \(\mathbf{n}_{\beta}\) is zero, and the first two terms of \(\mathbf{n}_{z}\) are zero. Due to the influence of these zero elements, only the part involved with out-SE(2) perturbations in the Jacobian matrix is applied to the noise term. Accordingly, the covariance matrix is approximately represented as

\[ \begin{split}\sum_{\delta\mathbf{n}_{l}}&=\mathbf{j}_{n_{ \beta}}^{\mu}\phi_{\beta}\sum_{\beta_{xy}}\Big{(}\mathbf{j}_{n_{\beta}}^{\mu}\phi_ {\beta}\Big{)}^{T}\\ &\quad\quad\quad+\sigma_{z}^{2}\mathbf{j}_{n_{z}}^{\mu}\phi_{z}\Big{(} \mathbf{j}_{n_{z}}^{\mu}\phi_{z}\Big{)}^{T}+\sigma_{l}^{2}\mathbf{I},\end{split}  \tag{17}\]where the coefficients \(\phi_{\beta}\) and \(\phi_{z}\) are appropriate for eliminating useless matrix components and have no other practical meaning. Now, we return the focus to the previously defined error function (13), which finds the link between features from \(\hat{\mathcal{P}}_{k}\) and map \(\mathbf{m}\) and calculates the distance sum consisting of point-to-edge and point-to-planar displacement as the new error term. The optimal robot state can be calculated by using the Gauss-Newton method to minimize the distance iteratively:

\[\min_{\mathbf{T}_{k}}\left\{\sum_{\mathbf{F}_{k}^{c}\in\hat{\mathcal{P}}_{k}}d _{k}^{c}+\sum_{\mathbf{F}_{k}^{d}\in\hat{\mathcal{P}}_{k}}d_{k}^{p}\right\}, \tag{18}\]

where \(d_{k}^{c}\) and \(d_{k}^{p}\) are the -to-edge distance and point-to-planar distance, respectively. The general description of the iteration formula is as follows:

\[\mathbf{J}^{\mathrm{T}}\mathbf{J}\cdot\Delta T=-\mathbf{J}^{\mathrm{T}}\mathbf{e}, \tag{19}\]

where \(\mathbf{J}\) is the Jacobian matrix of the residual with respect to the optimization variables \(\mathbf{T}_{k}^{*}\), and \(\Delta T\) is the increment. According to the formulation of the error function, the chain rule is used to infer the Jacobian \(\mathbf{J}\):

\[\mathbf{J}=\frac{\partial\mathbf{c}_{L}}{\partial\mathbf{F}_{k}^{w}}\frac{ \partial\mathbf{F}_{k}^{w}}{\partial\mathbf{T}_{k}^{w}}, \tag{20}\]

where \(\frac{\partial\mathbf{c}_{L}}{\partial\mathbf{F}_{k}^{w}}\) is the Jacobian matrix of the edge and planar residual. As the optimization variable is composed of rotation and translation, the Jacobian matrix can be divided into the following forms:

\[ \begin{split}\mathbf{J}&=\left[\frac{\partial\mathbf{c }_{L}}{\partial\mathbf{k}_{k}}\frac{\partial\mathbf{c}_{L}}{\partial\theta_{k}} \right]\\ &=\left[\frac{\partial\mathbf{c}_{L}}{\partial\mathbf{F}_{k}^{w}} \mathbf{I}_{3}\mathbf{\phi}_{\beta}\right.\left.-\frac{\partial\mathbf{c}_{L}}{ \partial\mathbf{F}_{k}^{w}}\left(\mathrm{Exp}\big{(}\mathbf{n}_{\beta}\big{)} \mathbf{R}_{k}\tilde{\mathbf{F}}_{k}^{b}\right)^{\wedge}\mathbf{\phi}_{z}\right] \end{split}  \tag{21}\]

Finally, the coefficients \(\phi_{\beta}\) and \(\phi_{z}\) are used to hold the dimension consistent, since the robot is assumed to perform SE(2) movement in this paper. The optimal pose \(\mathbf{T}_{k}^{w}\) can be derived by non-linear optimization based on the above constraint. Following the tightly coupled Lidar inertial odometry structure, the estimated Lidar odometry factors can be utilized to correct the bias of the IMU. ### Smoothness Scan Context

Lidar SLAM may degenerate into simple odometry if there is no loop closure mechanism to eliminate the accumulated error. We recall the previously redefined data structure \(p_{k}^{i,j}=[x,y,z,s]^{T}\) of the feature, which adds the smoothness \(s\) elements based on the raw geometry \([x,y,z]^{T}\). The smoothness can reveal the texture of the environmental surface over a local region to a certain extent because it corresponds to all features \(\tilde{\mathbf{F}}_{k}^{b}\) from the frame.