# An Improved MbICP Algorithm for Mobile Robot Pose Estimation
## 2 Scan-Matching Algorithm
### Resampling  
Laser scanners have a settled viewing angle and resolution, so a longer scanning distance increases the distance between neighboring scanned points. When a mobile robot moves forward in a long narrow corridor, the density distribution of point clouds from the laser scanner greatly varies according to the distance from the sensor. Sensors scan fewer points at greater distances (as shown in Figure 4a). However, such a different density distribution of point clouds may slow down the iteration process and distort the calculation of the rejection threshold, as mentioned in chapter 2.4.1. Therefore, we utilize a resampling method that is based on grid partitioning to realize a relatively even density distribution.  
First, we build a spatial grid for the object point cloud. We use a coordinate system that is centered at the sensor's position to calculate the coordinates of all the scanned points and then obtain the minimum bounding rectangle of the point cloud. The rectangle is partitioned into grids according to a given grid length (0.1 m is used according to the sensor's resolution), as shown in Figure 4b.  
Second, we calculate a sampling percentage for points in each grid. Given two occupied grids whose row and column indices are \((r_{1},c_{1})\) and \((r_{2},c_{2})\), respectively, the distance \(d\) between the grids is  
Figure 4: (a) Different density distributions of point clouds; (b) spatial partition.  
Figure 3: (a) Iso-distance curves: the blue curve is from the metric-based distance, and the red curve is from the Euclidean distance; (b) associations between scans with large scans that are built by the metric-based distance.  
defined as \(\sqrt{\left(r_{1}-r_{2}\right)^{2}+\left(c_{1}-c_{2}\right)^{2}}\). The sensor is contained in the center grid, so its row and column indices are both zero. We calculate the distance \(d_{i}\) from each grid to the center grid according to the distance definition and obtain the max distance (\(d_{max}\)). The sampling ratio for points in each grid is thus defined as the ratio of \(d_{i}\) and \(d_{max}\), which guarantees that distant grids from the center grid have a relatively high sampling percentage and nearby grids have a low sampling percentage.  
Finally, we resample points in the units of the grids according to the assigned sampling percentage. Given a grid with \(n\) points and sampling percentage \(\eta\), the sampling size \(n^{\prime}\) is calculated as the ceiling of \(n\times\eta\). To preserve as much topological information as possible for the points, the first and last points are preserved and the remaining \(n^{\prime}-2\) points are sampled at regular intervals, as shown in Figure 5a.  
This resampling method is based on a grid partition, which determines the sampling percentage according to the distance from the occupied grid to the center grid and then samples points in the units of the grid according to the topological information. This method can effectively dilute dense points nearby and retain their topological information, therefore accelerating the iteration process and preventing the calculation of the rejection threshold from becoming distorted by the greatly varying density distributions of point clouds.