# GNSS-Assisted Visual Dynamic Localization Method in Unknown Environments
## 3 The Overall Flow of the Algorithm  
Based on the sorting and deduction of the above related contents, an overall framework diagram and the implementation steps of the visual dynamic positioning algorithm flow with GNSS assistance were created, as Figure 4 and Table 1 show. Figure 3: \(I_{4}\) positional solution schematic diagram. **Implementation Steps of the Visual Dynamic Positioning Algorithm with GNSS Assistance**  
Step 1. With the known three frames as constraints, the camera coordinate system of Frame 1 was used as the world coordinate system. Step 2. Frame 1 and Frame 2 features were extracted and matched, and Frame 1 and Frame 3 features were ex-tracted and matched. Step 3. The epipolar geometry constraint and homography matrix models were evaluated, and different models were selected for the specific scenarios for the relative position estimation to obtain \(\mathbf{R}_{\text{C2}^{i}}^{\text{C}_{i}}\), \(\mathit{\Gamma}_{\text{C2}^{i}}^{\text{C}_{i}}\), \(\mathbf{\Gamma}_{\text{C3}^{i}}^{\text{C}_{i}}\), and \(\mathit{\Gamma}_{\text{C3}}^{\text{C}_{i}}\). Step 4. The GNSS position coordinates \(\mathit{X}_{\text{C1}^{n}}^{\text{C}_{i}}\), \(\mathit{X}_{\text{C2}^{n}}^{\text{C}_{i}}\), and \(\mathit{X}_{\text{C3}}^{\text{n}}\) of Frame 1, Frame 2, and Frame 3 were used as the constraint data and combined with \(\mathit{\Gamma}_{\text{C2}}^{\text{C}_{i}}\) and \(\mathit{\Gamma}_{\text{C2}}^{\text{C}_{i}}\) to calculate the scale. Step 5. Feature point triangulation was performed using \(\mathbf{R}_{\text{C2}^{i}}^{\text{C}_{i}}\), \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C2}^{i}}}\), \(\mathbf{\Gamma}_{\text{C3}^{i}}^{\text{C}_{i}}\), \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C3}^{i}}}\), and \(\mathbf{K}\), and the first three frames' matching feature points in the world coordinate system coordinates were obtained and put into the map point library. Step 6. The transformation matrix \(\mathbf{R}_{W}^{n}\) and \(t_{W}^{n}\) between the GNSS coordinate system and the world coordinate system was calculated using the known position coordinates of the three GNSS frames \(\mathit{X}_{\text{C1}^{n}}^{\text{C}_{i}}\), \(\mathit{X}_{\text{C2}^{n}}^{\text{C}_{i}}\), and \(\mathit{X}_{\text{C3}}^{\text{n}}\) and \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C2}^{i}}}\), \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C3}^{i}}}\), \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C3}^{i}}}\), and \(\mathit{\widehat{\mathit{\Gamma}}_{\text{C3}^{i}}}\). Step 7. We added the next frame, determined if there were GNSS data, and if there were, we continued to Step 7; if GNSS data were available for three consecutive frames, we returned to Step 1, and if not, we moved on to Step 8. Step 8. The Frame 4 feature points were extracted, and feature matching was performed with the previous keyframe. RPNP was utilized to solve the bitmap and BA optimization was performed. We took Frame 4 as an ex-ample to obtain its bit position in the world coordinate system \(\mathbf{R}_{\text{C2}^{i}}^{\text{C}_{i}}\), \(\mathit{\Gamma}_{\text{C1}^{i}}^{\text{C}_{i}}\). Step 9. We determined if Frame 4 was a keyframe, and if it was, the feature points were triangulated with the pre-vious keyframe and reconstructed and added to the map library. BA optimization was used to improve the keyframe bitmap and map point location coordinates. Step 10. Using the GNSS coordinates \(\mathit{X}_{\text{C1}}^{\text{n}}\) of Frame 1 and \(\mathbf{R}_{W}^{n}\), \(t_{W}^{n}\) obtained by Step 6, the coordinates of Frame 4 in the GNSS coordinate system were calculated. Step 11. We continued with Step 7 to obtain the GNSS coordinates for subsequent frames.