# A Frame-to-Frame Scan Matching Algorithm for 2D Lidar Based on Attention
## 4 Pose Matching Module  
After the attention area has been selected, two-point cloud sets from the attention area of the reference frame and from the attention area of the current frame are available. The pose matching module needs to calculate the relative poses of these two-point cloud sets. There are many ways to solve point cloud registration problems. To minimize the impact of sensor noise, a scan-to-model method [9] that uses the implicit moving least squares method (IMLS) was adopted to model the plane of the point cloud obtained from the reference frame. The distance from the laser point \(p\) in the current frame to the plane is shown in Equation (7) [29]:  
\[ \begin{split} d(p)=\frac{\sum\limits_{p_{i}\in S}W_{i}(p)((p-p_{i })\cdot n_{i})}{\sum\limits_{p_{i}\in S}W_{i}(p)}\\ W_{i}(p)=e^{\frac{-\|p-p_{i}\|^{2}}{\sigma^{2}}},\end{split}  \tag{7}\]  
where \(S\) represents the point cloud of the attention area in the reference frame, \(n_{i}\) represents the normal vector of the corresponding laser point, \(\sigma\) is the artificially set attenuation factor, and \(W\) is an exponential function, and it decays quickly with distance and with the consideration of the reference frame points where the distance \(p\) exceeds \(3\sigma\) is not necessary. Equation (7) implicitly expresses the hidden surface in the reference frame. It assumes that the sampling noise of the point cloud to the surface obeys Gaussian distribution, so the number and distribution of the sampling points on both sides of the surface are the same. Figure 3: Data association in the attention area of the current frame. Red is the reference frame, green is the current frame, and the purple area is the attention area. Moreover, the points on the surface are substituted into Equation (7), and the obtained distance value d is theoretically equal to 0; that is to say, all of the points satisfying \(d=0\) constitute a hidden surface. Based on Equation (7), the objective function of scan-to-model matching method is obtained and is presented as follows:  
\[f=\sum_{p_{i}\in\mathbb{C}}\left|d(T\cdot p_{i})\right|^{2}, \tag{8}\]  
where \(C\) represents all of the point clouds in the attention area of the current frame, \(T\) represents the transformation matrix corresponding to the robot pose, and its expression was shown earlier in Equation (6). Equation (8) represent an exponential function that is a nonlinear least square problem. In order to solve the registration problem more effectively, a point \(q_{i}\) on the surface is obtained as a matching point for each point \(p_{i}\) in the current frame, and the objective function of the matching method is shown in Equation (9). \[f=\sum_{p_{i}\in\mathbb{C}}\left|T\cdot p_{i}-q_{i}\right|^{2} \tag{9}\]  
Since the initial pose of the current frame is determined by wheel odometry, the error rate can be controlled within a small range. When the angle error is small, a small angle approximation can be introduced [30], and \(T\) can be expressed as  
\[T=\left[ \begin{array}{ccc}1&-\theta&x\\ \theta&1&y\\ 0&0&1\end{array} \right] \tag{10}\]  
By introducing the approximation shown in Equation (10), Equation (9) can be simplified as follows:  
\[ \begin{array}{rcl}T\cdot p_{i}-q_{i}&=&\left[\begin{array}{ccc}1&-\theta &x\\ \theta&1&y\\ 0&0&1\end{array}\right]\left[\begin{array}{c}p_{ix}\\ p_{iy}\\ 1\end{array}\right]-\left[\begin{array}{c}q_{ix}\\ q_{iy}\\ 1\end{array}\right]\\ &=&\left[\begin{array}{c}p_{ix}-p_{iy}\theta+x-q_{ix}\\ p_{ix}\theta+p_{iy}+y-q_{iy}\\ 0\end{array}\right]\end{array}  \tag{11}\]  
Obviously, Equation (11) is a linear equation system, and the independent variables can be extracted:  
\[ \begin{array}{rcl}T\cdot p_{i}-q_{i}&=&\left[\begin{array}{ccc}1&0&-p_{iy} \\ 0&1&p_{ix}\end{array}\right]\left[\begin{array}{c}x\\ y\\ \theta\end{array}\right]-\left[\begin{array}{c}q_{ix}-p_{ix}\\ q_{iy}-p_{iy}\end{array}\right]\\ &=&A_{i}X-b_{i}\end{array}