# An Improved MbICP Algorithm for Mobile Robot Pose Estimation
## 2 Scan-Matching Algorithm
### Metric-based Distance  
Assume that two successive laser scans with large rotation are given (Figure 2): the reference scan \(S_{ref}\) with \(n_{p}\) points, \(P=\left\{p_{i},i=1,\ldots,n_{p}\right\}\), and the object scan \(S_{obj}\) with \(n_{m}\) points, \(M=\left\{m_{j},\ j=1,\ldots,n_{m}\right\}\). When establishing the correspondence, the standard ICP algorithm searches for the closest point in \(M\) for each point in \(P\), which is under the framework of Euclidean space. Points in \(P\) that are far from the center are also far from their actual correspondence points because of the angular difference, which easily creates false correspondence and ICP failure. Thus, finding the closest point in Euclidean space is not reliable for establishing correspondence when large rotation exists.  
To overcome the defect of Euclidean distance in merely considering the translation component, MbICP defines a new distance measure called the metric-based distance, which simultaneously considers both translation and rotation. Given two points in \(R^{2}\), the reference point \(p_{i}=\left(p_{ix},\ p_{iy}\right)\) and the object point \(m_{j}=\left(m_{jx},\ m_{jy}\right)\). A set of rigid-body transformations \(q=\left(x,\ y,\ \theta\right)\) can be acquired that satisfies \(q\left(m_{j}\right)=p_{i}\) (Equation (2)).  
\[q\left(m_{j}\right)=\left(\begin{array}{c}x+cos\theta\times p_{1x}-sin \theta\times p_{1y}\\ y+sin\theta\times p_{1x}+cos\theta\times p_{1y}\end{array}\right) \tag{2}\]  
The metric distance \(d\left(p_{i},m_{j}\right)\) between \(m_{j}\) and \(p_{i}\) is defined as the minimum norm among the transformations (Equation (3)).  
\[d\left(p_{i},m_{j}\right)=min\left\{norm(q),\ q\left(m_{j}\right)=p_{i}\right\} \tag{3}\]  
where  
\[norm(q)=\sqrt{x^{2}+y^{2}+\theta^{2}L^{2}} \tag{4}\]  
\(L\) is a balance parameter that is homogeneous to a length, which is used to balance the translational and rotational components.  
A set of iso-distance curves relative to \(P_{i}\) and \(M_{j}\) is depicted by using the metric-based distance and Euclidean distance to visually represent the geometric features of metric-based distance (Figure 3a). The iso-distance curves that use the metric-based distance are claimed to be ellipses in the literature [10], while those that use the Euclidean distance are inscribed circles, which provides the metric-based distance a larger search zone than that of the Euclidean distance for the same search radius. The farther the object point is from the center, the longer the long axis of the ellipse becomes and, therefore, the more likely the object point will find its correspondence point in the rotational direction. This feature of the metric-based distance improves the probability of finding the correct correspondence for points that are far from the sensor, as shown in Figure 3b.  
Figure 2: Two scans that were captured at the beginning and end of robot motion. The blue scan is the object scan (\(S_{obj}\)) and the red scan is the reference scan (\(S_{ref}\)). The dotted lines are the correspondence based on the criterion of the closest point.