# MIM_SLAM: A Multi-Level ICP Matching Method for Mobile Robot in Large-Scale and Sparse Scenes
## 3 Algorithm Description
### Uncertainty Estimation  
After multi-level ICP matching, the transformation matrix \(T_{ij}\) between pose \(x_{i}\) and pose \(x_{j}\) can be obtained, but we still need to know the exact uncertainty estimation, namely the covariance matrix \(\Sigma_{ij}\). However, an incorrect covariance matrix may damage the established map.  
In this paper, the inversion of the Fisher information matrix is used as the covariance. The Fisher information matrix is defined as the function of the expected measurement and the surface slope scanned by the laser sensor [24]. [PERSON] [24] and [PERSON] [25] give the derivation of the Fisher information matrix based on a two-dimensional probability grid. It is now extended to three-dimensions. The Fisher information matrix is discretized:  
\[\hat{L}(p)=\sum\limits_{i=1}^{N}\frac{1}{\sigma_{i}^{2}}(\frac{\Delta r_{iE}}{ \Delta p})^{\top}(\frac{\Delta r_{iE}}{\Delta p}) \tag{11}\]  
\[\frac{\Delta r_{iE}}{\Delta p}=\left[\frac{\Delta r_{iE}}{\Delta x},\frac{ \Delta r_{iE}}{\Delta y},\frac{\Delta r_{iE}}{\Delta z},\frac{\Delta r_{iE}}{ \Delta y},\frac{\Delta r_{iE}}{\Delta\theta}\right] \tag{12}\]  
where \(p=[x,y,z,\varphi,\psi,\theta]\) is the pose of the robot, and \(\sigma_{i}^{2}\) is the noise variance of the \(i\)-th 3D laser range finder scan ray. \(N\) is the total number of scan rays. \(r_{iE}\) is the expected distance from the robot to the nearest obstacle along the \(i\)-th scan ray. To calculate Equation (12) efficiently, the point cloud map is converted into a three-dimensional grid map. Thus \(r_{iE}\) can be computed as follows:  
\[r_{iE}=\frac{\sum\limits_{j=1}^{s}r_{ij}\mu_{ij}}{\sum\limits_{j=1}^{s}\mu_{ ij}} \tag{13}\]where \(r_{ij}\) is the distance between the robot and the \(j\)-th voxel along the direction of an \(i\)-th scan ray. \(\mu_{ij}\) is the occupancy value of the corresponding voxel. \(s\) is the sequence number of the ending voxel. By combining Equation (11) and Equation (12), we obtain:  
\[\hat{L}(p)=\sum_{i=1}^{N}\frac{1}{\sigma_{i}^{2}}\begin{bmatrix}\frac{\Delta r _{IE}^{2}}{\Delta x^{2}}&\frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{ \Delta r_{IE}^{2}}{\Delta x\Delta z}&\frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}& \frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}\\ \frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y \Delta z}&\frac{\Delta r_{IE}^{2}}{\Delta y\Delta z}&\frac{\Delta r_{IE}^{2}}{ \Delta y\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y\Delta y}&\frac{\Delta r_{IE }^{2}}{\Delta y\Delta y}\\ \frac{\Delta r_{IE}^{2}}{\Delta x\Delta z}&\frac{\Delta r_{IE}^{2}}{\Delta y \Delta z}&\frac{\Delta r_{IE}^{2}}{\Delta z^{2}}&\frac{\Delta r_{IE}^{2}}{ \Delta z\Delta z\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta z\Delta y}&\frac{ \Delta r_{IE}^{2}}{\Delta z\Delta z\Delta y}\\ \frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y \Delta z}&\frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{ \Delta z\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta z\Delta y}&\frac{\Delta r_{IE }^{2}}{\Delta z\Delta y}\\ \frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y \Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{ \Delta z\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y\Delta z\Delta y}&\frac{ \Delta r_{IE}^{2}}{\Delta z\Delta y}\\ \frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta y \Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta x\Delta y}&\frac{\Delta r_{IE}^{2}}{ \Delta z\Delta y}&\frac{\Delta r_{IE}^{2}}{\Delta z\Delta y}&\frac{\Delta r_{IE }^{2}}{\Delta z\Delta y}\\ \end{bmatrix} \tag{14}\]  
According to the Cramer-Rao Bound theory [26], the lower bound of covariance can be determined by the inversion of the Fisher information matrix. Finally, the uncertainty of the pose estimation can be obtained:  
\[\mathrm{cov}(p)=\hat{L}^{-1}(p) \tag{15}\]