# Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation
## 3 Methodology
### Point-Cloud De-Skewing
For convenience, we still apply the equal sign to represent with \(\tilde{\mathrm{F}}_{k}^{b}=\tilde{\mathrm{F}}_{k}^{e}\cup\tilde{\mathrm{F}}_{k }^{p}\) and \(\tilde{\mathrm{F}}_{k}^{b}\) the set of all features in the \(b\) frame and \(w\) frame at time \(k\), respectively. The transform process is corrupted by a zero mean additive Gaussian noise \(\boldsymbol{n}_{l}\). The pose estimation strategy is to match the edge features \(\tilde{\mathrm{F}}_{k}^{e}\) and planar features \(\tilde{\mathrm{F}}_{k}^{p}\) from the current frame with the global map \(\boldsymbol{m}\), which consists of an edge map and planar map updated increasingly. Once the transformed feature points are associated with the map using the KD-tree method, the distance from point to edge and point to planar can be calculated and then used as the residual for optimization. Thus, the error function is formulated as

\[\mathbf{e}=\mathbf{r}\big{(}\tilde{\mathbf{F}}_{k}^{w},\mathbf{m}\big{)}\, \tag{13}\]

where \(\mathbf{r}(\cdot)\) is the cost function, and \(\mathbf{e}\) is the residual. The current optimal pose \(\mathbf{T}_{k}^{w}\) can be solved by iteratively minimizing the distance until it converges. Although the environments are generally in 2D, the robot model is sensitive to perturbations from slipping and complex terrain and can then be corrupted, making it challenging to be consistent with the dimensions of the SE(2) model. It is observed that these perturbations are part of the robot's state, which is more critical for the accuracy of the pose. Therefore, we propose an efficient constraint model to integrate these perturbations. To balance the complexity and accuracy of the pose, the perturbations, including rotation perturbations \(\mathbf{n}_{\beta_{xy}}\sim\mathcal{N}\Big{(}\mathbf{0}_{2\times 1},\Sigma_{\beta_{xy}} \Big{)}\) and vertical translation perturbation \(n_{z}\sim\mathcal{N}\big{(}0,\sigma_{z}^{2}\big{)}\), are incorporated into the pose model as the noise terms. The rotation matrix and the translation exist in non-Euclidean space and Euclidean space, respectively. The parameterization of the perturbation must be consistent with the space to which it belongs. \[ \begin{split}\mathbf{R}_{k}\leftarrow\text{Exp}\big{(}\mathbf{n}_{\beta} \big{)}\mathbf{R}_{k},\quad\mathbf{n}_{\beta}=\big{[}\mathbf{n}_{\beta_{w}}\ 0\big{]}^{T}\in\mathbb{R}^{3}\\ \mathbf{t}_{k}\leftarrow\mathbf{t}_{k}+\mathbf{n}_{z},\quad\mathbf{n}_{z}=[0\ 0\ n_{z}]^{T}\in\mathbb{R}^{3},\end{split}  \tag{14}\]

since the disturbance in the z direction is a Euclidean translation that can be directly used for robot poses according to the principle of standard vector addition. The perturbations are integrated into the transformation process of feature points via dimension extension. In this case, we rewrite (12) as

\[ \begin{split}\tilde{\mathbf{F}}_{k}^{w}&=\mu\left( \tilde{\mathbf{F}}_{k}^{b}\right)=\text{Exp}\big{(}\mathbf{n}_{\beta}\big{)}\mathbf{R}_{k} \tilde{\mathbf{F}}_{k}^{b}+\mathbf{t}_{k}+\mathbf{n}_{z}+\mathbf{n}_{l}\\ &\approx\mu\left(\tilde{\mathbf{F}}_{k}^{b}\right)+\mathbf{j}_{n_{\beta }}^{\mu}\mathbf{n}_{\beta}+\mathbf{j}_{n}^{\mu}\mathbf{n}_{z}+\mathbf{n}_{l}.\end{split}  \tag{15}\]

The perturbations are considered noise terms in the model and can be reached by a general mathematical method (first-order Taylor expansion). Exploiting the left perturbation model, the Jacobian matrix of \(\mu\) with respect to \(\mathbf{n}_{z}\) and \(\mathbf{n}_{\beta}\) can be derived as follows:

\[ \begin{split}\mathbf{J}_{\mathbf{n}}&=\Big{[}\mathbf{j}_{n_{ \beta}}^{\mu}\quad\mathbf{j}_{n_{z}}^{\mu}\Big{]}\\ &=\Big{[}-\Big{(}\text{Exp}\big{(}\mathbf{n}_{\beta}\big{)}\mathbf{R}_{k} \tilde{\mathbf{F}}_{k}^{b}\Big{)}^{\wedge}\ \mathbf{I}_{3}\Big{]},\end{split}  \tag{16}\]

where the meaning of hat operator \((\cdot)^{\wedge}\) is redefined to map a vector into a skew-symmetric matrix and no longer represents the element of Lie algebra. The noise consists of process noise, rotation perturbations, and vertical translation perturbation.