# MIM_SLAM: A Multi-Level ICP Matching Method for Mobile Robot in Large-Scale and Sparse Scenes
## 3 Algorithm Description  
### SLAM as an Incremental Optimization Problem  
This part analyzes the SLAM problem from the probabilistic model and transforms the problem into a least squares problem based on the maximum a posteriori estimate. Then, the incremental QR decomposition method [17] is used to solve the least squares problem. We denote robot pose by \(X=\{x_{i}\}\) with \(i\in 0\ldots M\), the landmark by \(L=\{l_{j}\}\) with \(j\in 1\ldots N\), control input given through wheeled odometry and IMU by \(U=\{u_{i}\}\) with \(i\in 1\ldots M\), landmark measurement by \(Z=\{z_{i}\}\) with \(i=1\ldots M\). The SLAM problem is equivalent to estimating the posterior probability distribution:  
\[ \begin{array}{l}P(X,L|Z,U)\\ =P(L|Z,X,U)P(X|Z,U)\\ =P(L|Z,X)P(X|Z,U)\end{array}  \tag{1}\]  
From Equation (1), it can be seen that the landmark \(L\) depends on the observations and robot pose sequences, so the SLAM problem is further simplified to the pose estimation problem utilizing [PERSON]'s rule \(P(X|Z,U)=P(Z|X,U)P(X|U)/P(Z|U)\). In this paper, we just consider the online SLAM problem [4], which only involves estimating the posterior over the momentary pose, this is \(P(x_{M}|z_{1:M},u_{1:M})\). \[\begin{array}{l}P(x_{M}|z_{1:M},u_{1:M})\\  
To estimate the pose of the robot, we transform the SLAM problem into the least-squares problem based on the maximum a posteriori estimate. The maximum a posteriori estimate \(X^{*}\) and \(L^{*}\) for the trajectory and map are obtained by minimizing the negative log of the joint probability from Equation (2):  
\[X^{*}=-\underset{X}{\text{argmin}}\eta\left\{\sum\limits_{i=1}^{M}\log P(x_{i}| x_{i-1},u_{i})\right. \tag{3}\] \[\left.+\sum\limits_{i=1}^{M}\log P(z_{i}|x_{i})\right\}\] \[L^{*}=\left\{(x_{1}^{*},z_{1}),\ldots,(x_{i}^{*},z_{i})\right\} \tag{4}\]  
Both of the motion model and measurement model are generally assumed such that they meet the Gaussian distribution. Then, both of them can be converted to the following form:  
\[x_{j}\sim N(x_{i}\oplus T_{ij},\Sigma_{ij}) \tag{5}\]  
where the operator \(\oplus\) denotes the coordinate transformation. \(T_{ij}\) and \(\Sigma_{ij}\) represent respectively the transformation matrix and covariance matrix between pose \(x_{i}\) and pose \(x_{j}\), which will be obtained through the multi-level ICP matching method and uncertainty estimation method in the next two paragraphs. Additionally, it can be written as follows:  
\[\left\{ \begin{array}{l}x_{i}=f(x_{i-1},u_{i})+m_{i}\\ z_{i}=g(x_{i})+n_{i}\end{array} \right. \tag{6}\]  
where \(m_{i}\) is the motion noise, and \(n_{i}\) is the measurement noise. This leads to the following nonlinear least squares problem:  
\[X^{*}=\underset{X}{\text{argmin}}\left\{\sum\limits_{i=1}^{M}\left\|f(x_{i-1 },u_{i})-x_{i}\right\|_{m_{i}}^{2}\right. \tag{7}\] \[\left.+\sum\limits_{i=1}^{M}\left\|g(x_{i})-z_{i}\right\|_{n_{i}} ^{2}\right\}\]  
Since the above model has non-linear functions that are not easy to solve, they must be linearized, as [PERSON] [18] and [PERSON] [19] has done. It can eventually be transformed into the general least-squares problem as follows, and the specific derivation can refer to the literature [14]. \[\theta^{*}=\underset{\theta}{\text{argmin}}\|A\theta-b\|^{2} \tag{8}\]  
where the vector \(\theta\) contains all pose variables, and the matrix \(A\) is a large and sparse Jacobian matrix. The problem in Equation (8) is apparently solved by the Cholesky decomposition method, but the essential problem is the number of calculations involved in solving the information matrix \(A^{T}A\). Thus, applying standard QR decomposition to matrix \(A\):  
\[A=Q\left[ \begin{array}{c}R\\ 0\end{array} \right] \tag{9}\]Apply Equation (9) to the least squares problem in Equation (8). As \(Q\) is an orthonormal matrix, \(Q^{T}Q=I\) (\(I\) is identity matrix). Thus, an additional term \(Q^{T}\) is added to the second line in (10), which doesn't change the length of the vector \(A\theta-b\). \[