*4.4. LiDAR Factor*

As mentioned in Section 3.3, after the LiDAR-assisted monocular visual depth correlation, the VIO will provide the LiDAR with visual initial positional estimates to correct

the motion distortion of the LiDAR point cloud and improve the scan matching accuracy. The scanning matching error between adjacent keyframes of LiDAR involved in this study can be expressed by the distance from the feature point to the matched edge line and feature plane as:

$$\begin{cases} \begin{array}{l} d\_{k}^{\operatorname{e}} = \frac{\left| \left( \boldsymbol{X}\_{(k+1,i)}^{\boldsymbol{e}} - \boldsymbol{X}\_{(k,i)}^{\boldsymbol{e}} \right) \times \left( \boldsymbol{X}\_{(k+1,i)}^{\boldsymbol{e}} - \boldsymbol{X}\_{(k,i)}^{\boldsymbol{e}} \right) \right|}{\left| \boldsymbol{X}\_{(k,a)}^{\boldsymbol{e}} - \boldsymbol{X}\_{(k,b)}^{\boldsymbol{e}} \right|} \end{array} \\\hline \quad \left( \boldsymbol{X}\_{(k+1,i)}^{p} - \boldsymbol{X}\_{(k,b)}^{p} \right) \\\ d\_{k}^{p} = \frac{\left( \left( \boldsymbol{X}\_{(k,a)}^{p} - \boldsymbol{X}\_{(k,b)}^{p} \right) \times \left( \boldsymbol{X}\_{(k,a)}^{p} - \boldsymbol{X}\_{(k,c)}^{p} \right) \right)}{\left| \left( \boldsymbol{X}\_{(k,a)}^{p} - \boldsymbol{X}\_{(k,b)}^{p} \right) \times \left( \boldsymbol{X}\_{(k,c)}^{p} - \boldsymbol{X}\_{(k,c)}^{p} \right) \right|} \end{cases} \tag{14}$$

where *X<sup>e</sup>* (*k*+1,*i*) represents the edge feature point at the *<sup>k</sup>* <sup>+</sup> 1th time, *<sup>X</sup><sup>e</sup>* (*k*,*a*) and *<sup>X</sup><sup>e</sup>* (*k*,*b*) are the endpoint of the edge line matched with the feature point at the *k*th time, *X<sup>p</sup>* (*k*+1,*i*) represents the plane feature point at the *k* + 1th time, and the feature surface matched with it at the *k*th time can be represented by three points *X<sup>p</sup>* (*k*,*a*) , *X<sup>p</sup>* (*k*,*b*) and *<sup>X</sup><sup>p</sup>* (*k*,*c*) .

#### *4.5. GNSS Factor and Loop Factor*

When the carrier moves to a GNSS signal trusted environment, GNSS factors can be added to optimize with local sensors. The time interval of two frames of GNSS observations is Δ*t*, and given the GNSS measurements *pG<sup>g</sup> <sup>k</sup>* in the global frame and *<sup>p</sup>V<sup>g</sup> <sup>k</sup>* representing the observation of LVIO in the global frame, the GNSS factor can be expressed by the following observation residuals:

$$r\_G\left(\sharp\_{k+1'}^k \mathcal{X}\right) = p\_k^{V^S} - p\_k^{G^t} \tag{15}$$

Different from the assumption in [14] that GNSS factors are added to the system only when the GNSS measurement covariance is smaller than the LVIO measurement covariance, we noticed that the accuracy of outdoor GNSS positioning results is much higher than the LVIO local positioning results. The covariance threshold size for judging whether to add GNSS factors has little impact on the positioning accuracy. Therefore, we present that once the GNSS signal is detected by the system, the GNSS factor is added to the factor graph. In this way, even if the mobile carrier enters the GNSS rejection environment (such as the indoor parking lot or tunnel), it can also provide a more accurate initial observation value after GNSS correction. The fusion strategy of GNSS and LVIO is shown in Figure 8.

Further, considering the possible overlap of the mobile carrier travel area, i.e., the mobile carrier travels to the same position again after a period of time, we also added a loopback detection link to establish the loopback constraint that exists between non-adjacent frames. Unlike introducing another sensor (GNSS) for global correction of the local sensor (LVIO), the loopback factor establishes the correlation between the current observed frames and the historical data by the local sensor itself to obtain a globally consistent estimate. The conditions for adding the loopback factor are similar to those of GNSS. Once the carrier motion trajectory is detected to travel to the environment passed by the history, the loop factor is added to the factor graph. By registering with the point cloud of the prior map, the historical trajectory is corrected, and the global pose estimation result with higher accuracy is obtained.

**Figure 8.** Fusion strategy of GNSS and LVIO. The initial rotation *R<sup>L</sup> <sup>G</sup>* of LVIO in the local frame and the global frame is set to identity matrix. GNSS provides global constraints to LVIO to correct the global position of LVIO and update *R<sup>L</sup> <sup>G</sup>*, and the new *<sup>R</sup><sup>L</sup> <sup>G</sup>* is used for the next frame of LVIO.
