# MIM_SLAM: A Multi-Level ICP Matching Method for Mobile Robot in Large-Scale and Sparse Scenes
## 3 Algorithm Description
### The Multi-Level ICP Matching Method  
Before detailing this method, it is useful to understand the ICP algorithm. The standard ICP [20] (Iterative Closest Point) algorithm firstly establishes the point-to-point correspondence between the two point clouds by the nearest neighbor principle and then establishes the matching error function. Finally, it computes a transformation matrix so that the error function is minimized. In the past 30 years, many ICP variant algorithms [21] have been proposed. In this paper, an ICP variant algorithm that calls point-to-plane ICP [22] is used. Compared with the standard ICP, the robustness and accuracy are better. This algorithm is listed as Algorithm 1. ```
0: Two point cloud: \(A=\{a_{i}\}\), \(B=\{b_{i}\}\); An initial transformation: \(T_{0}\)
0: Transformation \(T_{AB}\) which aligns \(A\) and \(B\); Fitness Score: \(Score_{fitness}\)
1:\(T\gets T_{0}\)
2:while not converged do
3:for\(i\gets 1\) to \(N\)do
4:\(m_{i}\leftarrow\text{FindClosestPointInA}(T\cdot b_{i})\)
5:if\(\|m_{i}-T\cdot b_{i}\|\leq d_{\text{max}}\)then\(w_{i}\gets 1\)
6:else\(w_{i}\gets 0\)
7:end
8:end
9:\(T\leftarrow\text{argmin}\bigg{\{}\sum\limits_{i}w_{i}\|\eta_{i}\cdot(T\cdot b_ {i}-m_{i})\|^{2}\bigg{\}}\)
10:end
11:\(T_{AB}\gets T\)
12:\(Score_{fitness}=\sum\limits_{i}w_{i}\|\eta_{i}\cdot(T\cdot b_{i}-m_{i})\|^{2}\)
```  
**Algorithm 1**Point-to-plane ICP. The observation of the 3D LiDAR sensor is denoted as \(z_{k}\) at time \(k\). There are data associations between all previous observations \(z_{1:k-1}\) and the current observation included in \(z_{k}\). A traditional scan matching method associates \(z_{k}\) with \(z_{k-1}\) at last time, or associates \(z_{k}\) with all previous observations \(z_{1:k-1}\). These two methods either have a substantial accumulated error, or have large amounts of calculations which cannot meet online applications. In this paper, the data association is divided into two categories: continuous in time and continuous in area. For observations that are continuous in the time, the method firstly utilizes the ICP algorithm to associate \(z_{k}\) and \(z_{k-1}\), and a rough estimation of the current robot's pose can be obtained. The initial value of ICP is obtained by using a Kalman filter to merge the wheeled odometry with IMU. Because the inter-frame matching cannot obtain the accurate pose estimation, the next step will match the current frame with the map to further eliminate the accumulated error. As shown in Figure 2, \(M_{i}\) denotes the global map at time \(i\) and \(T_{i}^{W}\) denotes the pose of the robot in the world coordinate system at time \(i\). \(T_{i+1}^{L}\) denotes the transformation matrix from time \(i+1\) to time \(i\) (that is, the output of inter-frame matching). \(z_{i+1}\) denotes the observation at time \(i+1\). The process for matching the current frame with the map is as follows:  
* Finding the nearest neighbor in \(M_{i}\) for each point in \(z_{i+1}\), and saving it as \(m_{i+1}\). We use the combination of octree and approximate nearest neighbor algorithm in PCL [23] for speeding up. * Taking \(z_{i+1}\) and \(m_{i+1}\) as the input of Algorithm 1, the transformation matrix \(T_{opt}\) can be obtained after registering the two point clouds. * The pose of the robot at time \(k+1\) is \(T_{i+1}^{W}=T_{i}^{W}\cdot T_{i+1}^{L}\cdot T_{opt}\). The effectiveness of the above scan matching method is verified as shown in Figure 3. The white point clouds denote the map that SLAM has established. The blue point clouds denote the point clouds after inter-frame matching. The redpoint clouds denote the point clouds after matching the current frame with the map. It can be seen that the accumulated error in the process of inter-frame matching is obviously eliminated. For observations that are continuous in the area, the robot returns to a region that it has previously explored.