## 3 Materials and Methods
### MP-IDSSC: Matching and Pose Estimation under Inverse Depth and Structural Semantic Constraints
#### 3.3.2 Fast P5P Method  
The pose of the image was used to constrain image matching and incorporate the pose estimation algorithm into RANSAC. Therefore, it was necessary to improve the computing speed as much as possible while still ensuring estimation algorithm accuracy. However, the number of points involved in the computation must not be excessive. Therefore, this study adopted the P5P fast pose estimation method to solve the problem.  
The mathematical relationship for the standard central projection is expressed as follows:  
\[\mathbf{s}\mathbf{u}=\mathbf{K}\mathbf{P}\mathbf{X} \tag{8}\]  
where \(\mathbf{K}\) is the intrinsic camera parameter matrix, and the intrinsic camera parameters can be obtained by calibration; \(s\) is an unknown scale parameter; \(\mathbf{u}=\begin{bmatrix}u&v&1\end{bmatrix}\); and \(\mathbf{P}=[\mathbf{R}|\mathbf{t}]\) is a \(3\times 4\) matrix that contains rotation and translation information, expressed as follows:  
\[P=[\mathbf{R}|\mathbf{t}]=\begin{bmatrix}r_{1}&r_{2}&r_{3}&t_{1}\\ r_{4}&r_{5}&r_{6}&t_{2}\\ r_{7}&r_{8}&r_{9}&t_{3}\end{bmatrix} \tag{9}\]  
Using \(u_{i}{}^{\wedge}u_{i}=0\) and eliminating the scale variable \(s\), we obtain:  
\[\begin{bmatrix}0&-1&v\\ 1&0&-u\\ -v&u&0\end{bmatrix}\begin{bmatrix}r_{1}&r_{2}&r_{3}&t_{1}\\ r_{4}&r_{5}&r_{6}&t_{2}\\ r_{7}&r_{8}&r_{9}&t_{3}\end{bmatrix}\begin{bmatrix}X\\ Y\\ Z\\ 1\end{bmatrix}=0 \tag{10}\]  
Expanding the third row yields:  
\[-v(r_{1}X+r_{2}Y+r_{3}Z+t_{1})+u(r_{4}X+r_{5}Y+r_{6}Z+t_{2})=0 \tag{11}\]  
where \(r_{i}\)_and_\(t_{i}\) are unknown variables to be determined. Each feature point provides a linear constraint equation; five points construct a \(5\times 8\) coefficient matrix. Let \(w=\left[r_{1},r_{2},r_{3},t_{1},r_{4},r_{5},r_{6},t_{2}\right]^{T}\). Through the above system of linear equations, we can obtain a solution consisting of three non-zero basis vectors:  
\[w=\lambda_{1}n_{1}+\lambda_{2}n_{2}+\lambda_{3}n_{3} \tag{12}\]  
Let \(\lambda_{3}=1\); based on the constraint that \(R\) is an orthogonal matrix, we can quickly obtain four groups of candidate solutions [33]. For each solution group, we can further solve \(\left[r_{7},r_{8},r_{9},t_{3}\right]^{T}\). From  
\[\begin{cases}r_{1}r_{7}+r_{2}r_{8}+r_{3}r_{9}=0\\ r_{4}r_{7}+r_{5}r_{8}+r_{6}r_{9}=0\end{cases} \tag{13}\]  
\(r_{7}\) and \(r_{8}\) can be linearly represented by \(r_{9}\), indicating that only \(r_{9}\) and \(t_{3}\) are real unknowns. Using the first or second row of (10),  
\[(r_{1}X+r_{2}Y+r_{3}Z+t_{1})-u(r_{7}X+r_{8}Y+r_{9}Z+t_{3})=0 \tag{14}\]  
We can construct a system of linear equations with a coefficient matrix of size \(5\times 2\) and finally obtain the optimal solution by minimizing the projection error.  
\[\begin{bmatrix}\alpha_{1}u_{1}&u_{1}\\ \vdots&\vdots\\ \alpha_{i}u_{i}&u_{i}\end{bmatrix}\begin{bmatrix}r_{9}\\ t_{3}\end{bmatrix}=\begin{bmatrix}\beta_{1}\\ \vdots\\ \beta_{i}\end{bmatrix} \tag{15}\]where \(\alpha_{i}=\frac{P_{2}F_{4}-P_{3}P_{4}}{P_{1}P_{5}-P_{2}P_{4}}X_{i}+\frac{P_{2}F_{ 4}-P_{3}P_{4}}{P_{1}P_{5}-P_{2}P_{4}}Y_{i}+Z_{i}\), and \(\beta_{i}=r_{1}X_{i}+r_{2}Y_{i}+r_{3}Z_{i}+t_{1}\).  
Random point selection is a key part in this process. We adopted a uniform random sampling method for the image plane. First, the image plane was evenly divided into 2 \(\times\) 2 grids, and each rectangle covered exactly one-quarter of the image. Thereafter, a region of equal size was placed at the center of the image, and the image plane was divided into regions named from A to E, as shown in Figure 2. During sampling, five points were randomly selected from these five regions.