# A Robust Localization System Fusion Vision-CNN
## 3 System Overview and Methodology
### Scan Matching Optimization  
Graph optimization may be used for pose optimization during the robot's global initial positioning and pose tracking phases. When the initial pose estimation is outside the branch-and-bound search window during the global initial positioning of the robot, the optimal solution cannot be found through search. However, through the following simulation, it was found that in a structured environment the use of graph optimization methods can increase the proportion of successful relocalization. Additionally, in the pose tracking stage, when the tracking error is large (the highest score of the particle set is lower than the preset threshold) scan matching based on graph optimization can quickly converge the estimated pose to near the true value. The scanning matching method based on nonlinear optimization can be described as a least squares problem:  
\[E(T)=\underset{\xi}{min}\sum_{k=1:n}\left(1-M(I(P(z_{t}^{k})))\right)^{2}= \underset{\xi}{min}\sum_{k=1:n}\left(f_{k}(\xi)\right)^{2} \tag{8}\]  
where \(\xi=\left(x,y,\theta\right)^{T}\) is the variable to be optimized and \(n\) is the number of laser points; \(I(P(z_{t}^{k}))\) represents the conversion of the coordinates \(P(z_{t}^{k})\) of the laser measurement point \(z_{k}^{i}\) in the world coordinate system to the grid coordinates in the map coordinate system; and \(P(z_{t}^{k})\) and \(I(P(z_{t}^{k}))\) are, respectively:  
\[P(z_{t}^{k})=\left[ \begin{array}{c}x_{z_{t}^{k}}\\ y_{z_{t}^{k}}\end{array} \right]=\left[ \begin{array}{c}x\\ y\end{array} \right]+\left[ \begin{array}{cc}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\end{array} \right]\left[ \begin{array}{c}x_{k,sens}\\ y_{k,sens}\end{array} \right]+z_{k}^{t}\bigg{[} \begin{array}{c}\cos( \theta+\theta_{k,sens})\\ \sin(\theta+\theta_{k,sens})\end{array} \right] \tag{9}\]  
\[I(P(z_{t}^{k}))=(x_{z_{t}^{k}}^{index},y_{z_{t}^{k}}^{index})^{T}=((x_{z_{t}^{k }}-x_{origin})/scale,(y_{z_{t}^{k}}-y_{origin})/scale)^{T} \tag{10}\]  
This least squares problem can be solved using the Gauss-Newton method, the LM (Levenberg-Marquardt) method, or the Dogleg method. The algorithm proposed in this paper uses the framework based on the Ceres library [40] to complete the calculation. However, before using each method to solve the problem, we first need to derive the derivative of the residual \(1-M(I(x_{z_{t}^{k}},y_{z_{t}^{k}}))\) with respect to the pose \(T\):  
\[J_{k}=\frac{\partial f_{k}(\xi)}{\partial\xi}=-\
abla M(I(P(z_{t}^{k})))\frac{ \partial I(P(z_{t}^{k}))}{\partial P(z_{t}^{k})}\frac{\partial P(z_{t}^{k})}{ \partial\xi} \tag{11}\]  
where:  
\[ \begin{array}{l}\frac{\partial P(z_{t}^{k})}{\partial\xi}=\left[\begin{array} []{cc}1&0&-x_{k,sens}\sin\theta-y_{k,sens}\cos\theta-z_{t}^{k}\sin(\theta+ \theta_{k,sens})\\ 0&1&x_{k,sens}\cos\theta-y_{k,sens}\sin\theta+z_{k}^{t}\cos(\theta+\theta_{k, sens})\end{array}\right]\\ \frac{\partial I(P(z_{t}^{k}))}{\partial P(z_{t}^{k})}=\frac{1}{scale}\end{array}  \tag{12}\]In the above formula, only \(\
abla M(I(P(z_{t}^{k})))\) is unknown. Since the grid map is discrete, each grid area will only record the score of its center. Generally, the grid coordinate \(I(P(z_{t}^{k}))\) projected by the laser point onto the map does not necessarily completely coincide with the grid cell center, and the discrete form is not conducive to the calculation of a differential. Here, we can use the bilinear interpolation method to calculate the score and the differentiation. Bilinear interpolation uses grid coordinates to calculate four adjacent grids, as shown in the Figure 5.  
For the convenience of marking, suppose the grid coordinates of the laser point projected into the map coordinate system is \(P(x,y)\) and its four adjacent grid coordinates are \(P(x_{0},y_{0})\), \(P(x_{1},y_{0})\), \(P(x_{1},y_{1})\), and \(P(x_{0},y_{1})\) by extension.