# Fast Lidar Inertial Odometry and Mapping for Mobile Robot SE(2) Navigation
## 3 Methodology
### Point-Cloud De-Skewing
Let \(y=\big{\{}\mathbf{T}_{k+t_{i}}\mid t_{i}\in(t_{s},t_{e}],i=1,\cdots,N_{n}\big{\}}\) be the set of IMU preintegration within this period, and let the timestamp \(t_{r,i}\in(t_{s},t_{e}]\) be the \(i\)th point on the \(r\)th ring of the Lidar frame. ``` Input:\(X:=t^{\prime}\big{(}t^{\prime}_{1},t^{\prime}_{N_{n}}\big{)}\) (training set), \(\quad y\) (targets), \(\quad\mathbf{x}_{*}:=t\in(t_{s},t_{e}]\) (test inputs). 1: Determining the hyperparameters from training data; 2:for\(t_{N_{n}}\gets t_{1}\)to\(t_{i}\)do for\(t_{N_{n}}\gets t_{1}\)to\(t_{j}\)do \(\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \quad\quad\quad\quad\



time \(k\). The raw point-cloud measurement is first converted into a range image, and then the smoothness \(s_{k}^{i,j}\) of points is calculated as follows:

\[s_{k}^{i,j}=\frac{1}{|\mathbb{C}|\cdot\left\|p_{k}^{i,j}\right\|}\left\|\sum_{m \in C,m\
eq j}\left(p_{k}^{i,m}-p_{k}^{i,j}\right)\right\| \tag{7}\]

where \(\mathbb{C}\) denotes the set of the nearest points adjacent to the left and right of the point \(p_{k}^{i,j}\) in the same row of the range image, \(|\mathbb{C}|\) is the size of set, and \(\left\|p_{k}^{i,j}\right\|\) is the Euclidean distance from point \(p_{k}^{i,j}\) to the scan center. According to the given threshold, the points with a smoothness exceeding the threshold are classified as edge/corner features, and those with a smoothness lower than the threshold are ranked as planar points. For convenience, we denote the edge feature set and the planar feature set as \(\mathrm{F}_{k}^{e}\) and \(\mathrm{F}_{k}^{p}\), respectively. We redefine the data structure of the features as \(p_{k}^{i,j}=[x,y,z,s]^{T}\), including the raw geometry \([x,y,z]^{T}\) and the smoothness \(s\) calculated above. The newly added dimension is significant for the front-end place recognition algorithm and will be explained in a later section. ### Pose Estimation

For mobile robot SE(2) navigation, existing works have mostly assumed that the robot's changes in the z direction and the pitch and roll dimensions are insignificant for navigation on the ground. Most of them only paid attention to the changes in the x, y, and yaw dimensions, and the keyframe pose was set as \(\mathbf{T}_{k}^{w}\in\mathrm{SE}(2)\), where the vector tangents and Lie algebra can be presented by elements of the type

\[\tau=\left[ \begin{array}{c}\rho\\ \theta\end{array} \right]\in\mathbb{R}^{3},\quad\tau^{\wedge}=\left[ \begin{array} []{cc}[\theta]_{\mathrm{x}}&\rho\\ \mathbf{0}&0\end{array} \right]\in\mathrm{se}(2) \tag{8}\]

where the operator \([]_{x}\) is a skew-symmetric matrix, and the superscript \({}^{\wedge}\) indicates that the variable is an element of Lie algebra. Meanwhile, the pose is also extended to \(\mathbf{T}_{k}^{w}\) for specific conditions to maintain the dimension consistently:

\[\mathbf{T}_{k}^{w}=\left[ \begin{array}{cc}\mathbf{R}_{k}&\mathbf{t}_{k}\\ \mathbf{0}&1\end{array} \right]\in\mathrm{SE}(3)\subset\mathbb{R}^{4\times 4}, \tag{9}\]

where rotation and translation are redefined as

\[ \begin{split}&\mathbf{R}_{k}=\left\{\mathrm{Exp}(\xi)\in \mathrm{SO}(3)\ |\ \xi=\left[0\ 0\ \theta_{k}\right]^{T}\right\}\\ &\mathbf{t}_{k}=\left[x\ y\ 0\right]^{T}=\left\{\mathrm{V}(\xi)\varepsilon \in\mathbb{R}^{3}\ |\ \varepsilon=\left[\rho\ 0\right]^{T}\right\}\end{split}  \tag{10}\]

with

\[\mathrm{V}(\xi)=\mathbf{I}+\frac{1-\cos\xi}{\xi^{2}}[\xi]_{\times}+\frac{\xi- \sin\xi}{\xi^{3}}[\xi]_{\times}^{2} \tag{11}\]

When a Lidar scan arrives at time \(k\), the current frame is first transformed from the \(b\) frame to the \(w\) frame by the pose \(\mathbf{T}_{k}^{w}\):

\[\tilde{\mathrm{F}}_{k}^{w}=\mu\left(\tilde{\mathrm{F}}_{k}^{b}\right)=\mathbf{ T}_{k}^{w}\tilde{\mathrm{F}}_{k}^{b}+\boldsymbol{n}_{l}=\mathbf{R}_{k}\cdot \tilde{\mathrm{F}}_{k}^{b}+\mathbf{t}_{k}+\boldsymbol{n}_{l}. \tag{12}\]

Note that the transformation from homogeneous coordinates to non-homogeneous coordinates is implicit.