Algorithm 1 Optimization of GVO

Require: Positioning results of GPS ${\mathit{x}}_k^{enu}$ and poses of camera ${\mathit{T}}_k$.

1: Optimize $\mathit{R}$ and $\mathit{K}$ by minimizing error between GPS measurements ${\mathit{x}}_k^{enu}$ and the positions estimated by VO ${\mathit{x}}_k^{\mathrm{ve}}$ with initial values obtained by (10) and (11).

2: Optimize the poses ${\mathit{T}}_k$ of camera by minimizing error between GPS measurements ${\mathit{x}}_k^{enu}$ and the positions estimated by VO ${\mathit{x}}_k^{\mathrm{ve}}$.

3: Use BA (Bundle Adjustment) to optimize the camera poses ${\mathit{T}}_k$ again by minimizing the image reprojection error between the matched 3D points in ENU frame and their keypoints in the image.

4: if The optimization converges then

5: return The optimized $\mathit{R}$, $\mathit{K}$ and ${\mathit{T}}_k$.

6: else

7: Repeat step1 to step 6

8: end if