GDVI-Fusion: Enhancing Accuracy with Optimal Geometry Matching and Deep Nearest Neighbor Optimization

Abstract

The visual–inertial odometry (VIO) system is not robust enough in long time operation. Especially, the visual–inertial and Global Navigation Satellite System (GNSS) coupled system is prone to dispersion of system position information in case of failure of visual information or GNSS information. To address the above problems, this paper proposes a tightly coupled nonlinear optimized localization system of RGBD visual, inertial measurement unit (IMU), and global position (GDVI-Fusion) to solve the problems of insufficient robustness of carrier position estimation and inaccurate localization information in environments where visual information or GNSS information fails. The preprocessing of depth information in the initialization process is proposed to solve the influence of an RGBD camera by lighting and physical structure and to improve the accuracy of the depth information of image feature points so as to improve the robustness of the localization system. Based on the K-Nearest-Neighbors (KNN) algorithm, to process the feature points, the matching points construct the best geometric constraints and eliminate the feature matching points with an abnormal length and slope of the matching line, which improves the rapidity and accuracy of the feature point matching, resulting in the improvement of the system’s localization accuracy. The lightweight monocular GDVI-Fusion system proposed in this paper achieves a 54.2% improvement in operational efficiency and a 37.1% improvement in positioning accuracy compared with the GVINS system. We have verified the system’s operational efficiency and positioning accuracy using a public dataset and on a prototype.

1. Introduction

State estimation algorithms have garnered significant attention owing to the emergence of applications such as unmanned driving, autonomous navigation, and 3D reconstruction. The popularity of multi-sensor fusion localization methods has been on the rise, as they harness the complementary advantages of various sensors to bolster the robustness and accuracy of state estimation [1,2,3]. To meet the demand for accurate and fast position estimation in autonomous navigation and positioning, numerous open-source algorithms for visual–inertial navigation system (VINS) fusion have been developed [4,5,6]. These algorithms, combining visual and inertial sensors, can achieve high accuracy. However, the VINS combined navigation system has four unobservable directions [7], and the accuracy and robustness are influenced by the quantity of visual observations. The stability of monocular visual–inertial odometry (VIO) [5,8,9] relies on the initialization effect, and in stationary scenarios, scale information degrades to an unobservable state, resulting in localization information prone to dispersion. The performance of stereo VIO [10,11] is constrained by the computational resources of the embedded device. These fusion algorithms have some drift in the localization information when the visual information is subjected to external interference. In challenging environments where camera images may be ambiguous or affected by lighting conditions, Global Navigation Satellite Systems (GNSS) can provide global position information and restrain state estimation divergence. GNSS receivers offer unique coordinates such as latitude, longitude, and altitude, independent of environmental conditions. However, GNSS alone is insufficient for positioning in environments like tunnels or indoors. RGBD vision cameras, capable of capturing depth information, provide scale information for the real environment, mitigating the impact of inaccurate initial scale estimation in monocular VIO systems. The effective fusion of RGBD visual–inertial and GNSS measurement information enhances the robustness and accuracy of the positioning system. This fusion approach ensures that the localization information remains stable even in environments where visual or GNSS measurements are disturbed.

However, there are many challenges in the fusion of the three major sensor systems. First, RGBD cameras face limitations in perception range and structure, making it challenging to acquire accurate depth information in environments with depth discontinuity. Additionally, open-source algorithms [10,12] struggle to operate in scenarios characterized by poor image texture, rapid rotation of the device, and incomplete image depth information. Therefore, the system initialization stage needs to optimize the depth information with noise to obtain effective depth information. Second, the computational time complexity and space complexity required for visual feature point matching are high, and there are false matches. Finally, image information becomes unreliable in bright light environments, resulting in the failure of both visual and depth information. In scenarios like traveling through tunnels and indoor–outdoor transitions, this situation can lead to a brief loss of GNSS information. Therefore, it poses a challenge to maintain system localization information without dispersion when one sensor fails, relying on other sensor data.

To solve the above problems, this paper proposes a RGBD visual–inertial and GNSS fusion positioning algorithm. The GDVI-Fusion algorithm tightly integrates GNSS raw measurements with RGBD visual and inertial data through nonlinear optimization. It utilizes the visual residual factor, GNSS pseudorange factor, Doppler factor, and inertial measurement unit (IMU) factor to jointly constrain the optimized system state, depth bias, and scale information, effectively reducing accumulated drift errors. To improve feature matching accuracy and efficiency, the algorithm incorporates KNN feature matching and optimal geometric constraints. The algorithm, which relies on KNN feature matching and optimal geometric constraints, is employed to enhance the correctness and efficiency of feature matching. This reduces the reprojection error and improves the positioning accuracy of the system through the initial matching and matching optimization phases. Furthermore, the Nearest Neighbor Resampling (NNR) algorithm assigns effective depth values with minimal error to feature points lacking depth information, fusing the aligned monocular image information and depth information. This ensures accurate depth information for each feature point, enhancing the robustness of the localization system. To achieve stable and accurate localization even in the event of a sensor data failure, a fusion localization method using three sensors is proposed. Compared with the stereo VIO system [11], the monocular GDVI-Fusion localization system can run on the on-board computer TX2, making it a lightweight and robust solution. The main innovations and contributions of this article are as follows:

• Fusion of depth information from RGBD cameras and raw measurement information from GNSS enables fast and accurate real-time estimation of the system state in case of failure of one of the sensors, such as image information or GNSS information.
• The Nearest Neighbor Resampling (NNR) algorithm processes RGBD camera depth by assigning depth values with smaller errors to feature points lacking depth information. This increases the count of effective feature points. Additionally, depth bias and a scale factor are introduced to optimize the depth information of the feature points, enhancing the robustness of the positioning system.
• The optimization of the best geometric constraints based on the KNN matching algorithm improves the accuracy and speed of feature matching and reduces the reprojection error, thereby enhancing the system’s positioning accuracy.

2. Related Work

Presently, various works on multi-sensor fusion state estimation algorithms show that multi-sensor localization systems are accurate and robust in localization. Most of these fusion-based localization algorithms rely on visual simultaneous localization and mapping (SLAM) techniques, which are classified into two main approaches: direct method [13,14,15] and indirect method [16,17]. Purely visual SLAM algorithms [18,19] are susceptible to challenges such as image ambiguity and strong exposure, which can result in the divergence of positional information. Additionally, monocular visual SLAM lacks the ability to observe scale information, leading to significant errors in localization outcomes due to cumulative long-term drift. Hence, to overcome these limitations and enhance localization performance, fusion algorithms integrate vision with other sensor modalities [20,21,22]. These fusion algorithms can be further classified into tightly coupled [23,24] and loosely coupled [11,25] frameworks. In the loosely coupled framework, estimates from different sensors are fused to make predictions. However, the accuracy of these estimates significantly influences the fusion modeling process and can lead to imprecise final results. The tightly coupled fusion [26,27] algorithm fuses raw measurements from different sensors with higher accuracy compared with the loosely coupled algorithm.

Algorithms for multi-sensor coupling can be divided into filter-based methods and optimization-based methods. The filter-based approach [28] to visual–inertial fusion state estimation system can quickly and efficiently optimize the system state using residual constraints between camera keyframes. The nonlinear BA optimization-based visual–inertial fusion state estimation system [5] uses sliding window optimization to constrain the system state by multiple keyframes, with better state estimation performance. However, the fusion of camera and inertial measurement unit (IMU) data only provides constraints on the local state between consecutive frames, failing to eliminate accumulated errors over long-term system operation. To mitigate this issue, the RGBD-VINS [29,30] system incorporates depth information from RGBD cameras to reduce accumulated errors. Additionally, the fusion of GNSS raw measurement information with the visual–inertial system offers a means to decrease the accumulated drift error in the VINS system, as GNSS position measurements are unique in the Earth-Centered, Earth-Fixed frame. State estimation systems such as [31,32,33] adopt a loosely coupled approach, fusing GNSS signals, vision, and inertial sensors within the Extended Kalman Filter (EKF) framework. VINS-Fusion [11] applies a nonlinear optimization approach to loosely couple VIO state estimates with GNSS measurements. In contrast, the GNSS visual–inertial navigation system (GVINS) [1] utilizes a tightly coupled fusion method to combine GNSS raw measurement information with visual–inertial sensors. Furthermore, GVINS proposes a coarse-to-fine initial alignment method to ensure non-divergence of the system state in the presence of GNSS signal degradation. When GNSS signals deteriorate, GVINS relies solely on monocular visual and inertial data, making the positioning system prone to drift when the platform is stationary or moving slowly. Moreover, in environments with low texture or unstable lighting conditions, the system is also susceptible to drift or cumulative errors.

In this paper, GNSS raw measurement information, visual image information, depth information, and inertial sensor information are tightly coupled. The system’s performance is evaluated in a complex urban environment, covering a total distance of 36.37 km. The experimental results show that the distance error between the initial and final positions of the loopback start and end points is only 1.8 m. By employing the tightly coupled positioning method, the paper effectively addresses the issue of divergence in positioning information when visual or GNSS signals degrade in the positioning system. Moreover, the system autonomously performs coordinate system conversions, allowing it to operate in both internal and external tunnel environments while maintaining global consistency.

3. System Description

The coordinates involved in our GDVI-Fusion system include the following:

$o_i{x}_i{y}_i{z}_i$ (i-frame): The Earth-Centered Inertial (ECI) frame is the output reference frame of the inertial sensor, with the center of the Earth as its origin and the $o_i{x}_i$ axis pointing to the vernal equinox. The $o_i{z}_i$ axis is the rotation axis of the Earth, pointing to the North Pole, and $o_i{y}_i$ is in the plane of the Earth’s equator. The i-frame does not rotate with the Earth.

$o_e{x}_e{y}_e{z}_e$ (e-frame): The Earth-Centered, Earth-Fixed (ECEF) frame has its origin at the center of the Earth. The $o_e{x}_e$ axis points to the prime meridian, the $o_e{z}_e$ axis is aligned with the Earth’s rotation axis, pointing to the North Pole, and the $o_e{y}_e$ axis lies in the equatorial plane of the Earth. The angular rate of rotation of the Earth ${\omega }_{ie}$ represents the angular motion between the ECEF frame and the Earth-Centered Inertial (ECI) frame. GNSS measurement data are based on the Earth-Centered, Earth-Fixed (ECEF) frame.

$o_n{x}_n{y}_n{z}_n$ (n-frame): The navigation frame is represented by the “East–North–Up” geographic coordinate system, which is the coordinate system that the carrier system equipment refers to when solving the navigation and positioning information, with the center of the carrier system equipment as the origin and the $x_n$, $y_n$, and $z_n$ axes pointing to the geographic east, north, and up directions, respectively.

$o_b{x}_b{y}_b{z}_b$ (b-frame): The body frame takes the center of gravity of the carrier as the origin; $x_b$, $y_b$, and $z_b$ point to the carrier rightward, forward, and upward, respectively. We define the b-frame to be the same as the IMU frame.

$o_c{x}_c{y}_c{z}_c$ (c-frame): The camera frame takes the center of gravity of the camera as the origin; $x_c$, $y_c$, and $z_c$ point to the camera rightward, forward, and upward, respectively. The c-frame is fixedly connected to the IMU. $c_k$ is the kth frame image in the camera frame.

The implementation of the positioning system involves three main phases: data preprocessing, initialization, and optimization. In the preprocessing stage, the pseudorange information obtained from a GNSS signal is processed by the SPP algorithm [34], while the acceleration and angular velocity information obtained from IMU is pre-integrated, and the depth information obtained from RGBD camera is optimized [29], where the image information is subjected to feature point detection, extraction, KNN-optimized matching, and optical flow tracking. During the initialization phase, the visual construction structure from motion (SFM) [35] is aligned with the pre-integrated IMU information. If the initialization of visual–inertial odometry (VIO) fails, GNSS residual data are utilized. If VIO initialization succeeds, it undergoes alignment with GNSS for yaw alignment and anchor positioning alignment. If data alignment succeeds, the data are optimized using VIO-GNSS residual data; otherwise, only VIO residual data are utilized. In the optimization phase, the residual data go through a sliding window optimization process. The overall system implementation is depicted in Figure 1.

3.1. Measurement Preprocessing

As shown in Figure 1, the initialization module includes IMU pre-integration, optimal geometric constraints based on KNN matching, preprocessing of visual feature point depth, and processing of raw GNSS information.

IMU Pre-integration: Inertial measurement devices are susceptible to various sources of error, including acceleration bias, accelerometer noise, gyroscope bias, and gyroscope noise. Additionally, earth surface measurements captured by the IMU contain a gravity component. However, for the purpose of this discussion, we will ignore errors related to scale factor, misalignment angle, and non-orthogonality in low-cost IMU measurements. Thus, inertial measurement model is

$$\begin{array}{cc}\hfill & {\tilde{\mathrm{a}}}_k={\mathrm{a}}_k+b_a+{\mathrm{R}}_n^{b_k}{\mathrm{g}}^n+n_a\hfill \\ \hfill & {\tilde{\omega }}_k={\omega }_k+b_g+n_g\hfill \end{array}$$

(1)

where ${\tilde{\mathrm{a}}}_k$ and ${\tilde{\omega }}_k$ are the actual measured values of linear and angular acceleration of the IMU at time k. ${\mathrm{a}}_k$, ${\omega }_k$ are the real values of linear and angular acceleration of the IMU at time k. Assume that the accelerometer noise $n_a$ and gyroscope noise $n_g$ obey Gaussian distribution $n_a\sim N\left(0,{\sigma }_a^2\right)$, $n_g\sim N\left(0,{\sigma }_g^2\right)$. $b_a$ and $b_g$ are the accelerometer bias and gyroscope bias, respectively. ${\mathrm{R}}_n^{b_k}$ is the rotation matrix from the n-frame to the b-frame at the moment k. ${\mathrm{g}}^n$ is the acceleration of gravity under the n-frame.

With the IMU output frequency surpassing the camera acquisition frequency per frame, the gyroscope and accelerometer measurements are pre-integrated [5] to derive information about the camera’s position, velocity, and rotation for two consecutive frames in $\left[t_k, t_{k+1}\right]$ time, as follows:

$$\begin{array}{cc}\hfill & {\alpha }_{b_k}^{b_{k+1}}={\int \int }_{t\in [t_k,t_{k+1}]}{\mathrm{R}}_t^{b_k}({\tilde{\mathrm{a}}}_t-b_a)d{t}^2\hfill \\ \hfill & {\beta }_{b_k}^{b_{k+1}}={\int \int }_{t\in [t_k,t_{k+1}]}{\mathrm{R}}_t^{b_k}({\tilde{\mathrm{a}}}_t-b_a)dt\hfill \\ \hfill & {\gamma }_{b_k}^{b_{k+1}}={\int \int }_{t\in [t_k,t_{k+1}]}{\displaystyle \frac{1}{2}}\mathsf{\Omega }({\tilde{\omega }}_t-b_g){\gamma }_t^{b_k}dt\hfill \end{array}$$

(2)

where the pre-integration equations ${\alpha }_{b_k}^{b_{k+1}}$, ${\beta }_{b_k}^{b_{k+1}}$, and ${\gamma }_{b_k}^{b_{k+1}}$ are the changes in position, velocity, and rotation of two consecutive frames of the camera in $\left[t_k,t_{k+1}\right]$ time, respectively. ${\mathrm{R}}_t^{b_k}$ is the rotation matrix of the $b_k$ frame at time t. $\mathsf{\Omega }(·)$ are defined as [5]

$$\begin{array}{c}\mathsf{\Omega }\left(\omega \right)=\left[\begin{array}{cc}-{\omega }^{\wedge }& \omega \\ -{\omega }^T& 0\end{array}\right]\hfill \\ {\omega }^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]\hfill \end{array}$$

Best geometric constraint algorithm based on KNN matching: We use the KNN algorithm for the initial matching of the extracted feature points. The sets of feature points in image $P_1$ and its matching image $P_2$ are denoted as $A=\left\{p_{11},p_{12},\cdots ,p_{1n}\right\}$ and $B=\left\{p_{21},p_{22},\cdots ,p_{2m}\right\}$, respectively.

Calculate the Euclidean distance from the feature vector of each feature point in image $P_1$ to the feature vector of each feature point in image $P_2$ and store the distances in a matrix ${\left[\begin{array}{ccc}{d}_{11}& \cdots & d_{1n}\\ ⋮& & ⋮\\ d_{n1}& \cdots & d_{nm}\end{array}\right]}_{n\times m}$, where $d_{ij}$ is the Euclidean distance of the eigenvectors of the feature points $p_{1i}$ and $p_{2i}$. The $i^{th}$ row vector $\left[\begin{array}{ccc}{d}_{i1}& \cdots & d_{im}\end{array}\right]$ represents the Euclidean distances between point $p_{1i}$ and all its corresponding points on image $P_2$. Identify the K (in this paper, $K=2$) smallest values in this m-dimensional vector. The points on image $P_2$ corresponding to these K distance minima are considered the K matching points.

If the distance ratio between the nearest neighbor point and the next nearest neighbor point is less than the threshold $D_{th}$ (the value of $D_{th}$ is set to 0.7), the nearest neighbor point will be used as the matching point. After completing the initial matching, the matching results will be subsequently optimized to reduce the false matching rate of the algorithm.

Feature matching feature point mismatch will occur between neighboring frames; when the mismatch is too large, it will affect the system accuracy. Therefore, the feature point matching algorithm needs to be optimized on the basis of the initial matching, and the false matching is eliminated by the geometric features of the feature point matching line so as to filter out the matching point pairs that satisfy the constraints of the geometric features. As shown in Figure 2. By comparing the length and slope of the matching lines between adjacent point pairs, we can detect abnormal matching point pairs. This screening process considers the distance $d_{ij}$ and slope $k_{ij}$ between the matching points (denoted as points $p_i(u_i, v_i)$ and $p_j(u_j,v_j)$).

$$\begin{array}{cc}\hfill & d_{ij}=\sqrt{{(u_j-u_i)}^2+{(v_j-v_i)}^2}\hfill \\ \hfill & k_{ij}={\displaystyle \frac{v_j-v_i}{u_j-u_i}}\hfill \end{array}$$

(3)

where $i,j\in [1,n]$, n are positive integers.

The KNN-based best geometric constraint matching algorithm filtering process is shown in Figure 3. The filtering abnormal matching point pairs method is as follows:

(a)

Construct the set of feature point pairs, let the set of matching point pair line lengths be $x\in \mathbb{R}$, and the set of slopes as $k\in \mathbb{R}$.

(b)

The mean value D of the sets $x\in \mathbb{R}$ and $k\in \mathbb{R}$, as well as the root mean square error S; set D as the base value and S as the error range.

(c)

Filter pairs of feature matching points that satisfy the condition of the value domain $\left[D-S, D+S\right]$.

In this paper, we utilize a KNN-based matching algorithm with optimal geometric constraints to not only enhance matching efficiency but also improve the accuracy of feature point matching, ultimately enhancing the overall system localization accuracy.

Visual feature depth Nearest Neighbor Resampling preprocessing: Discontinuous and incomplete depth information is measured by RGBD cameras. Invalid depth information has an impact on the robustness of the system. In this paper, we use the method of depth information preprocessing to obtain the initial value of valid depth information of feature points, which makes it easier to achieve robust and fast visual–inertial VIO initialization.

The images and depth information captured by the camera are aligned, and feature points of keyframes are extracted. This paper constructs a set $p\in {\mathbb{R}}^3$ of feature points with valid depth. Since the local variation of depth is gradual, a nearest neighborhood is defined around key points with invalid depth, and during feature tracking, resampling is performed on the depth around these key points and their vicinity. The feature points within the nearest neighborhood of this point with valid depth are iterated using the RANSAC algorithm [36] to find a sufficient number of inliers of depth values, thereby recovering the approximate depth of the key point $p_r\in {\mathbb{R}}^3$. In some scenes, the recovered depth has errors; therefore, the depth information needs to be optimized again.

The key point depth preprocessing method can roughly estimate the visual scale information, set the initial value of the scale, and further optimize it in the VIO initialization process to achieve fast initialization and improve the robustness of the system. Although resampling keypoints can be confounded by depth Nearest Neighbor Resampling (NNR) errors, most resampling keypoints provide depth values that are very close to the corresponding expected values. Deep NNR focuses on extending feature points with valid depth values, and we use resampling keypoints in tracking feature points to improve robustness when the depth map is severely incomplete.

GNSS raw information measurement: We incorporate GNSS measurements, including pseudorange measurements and Doppler shift measurements, in our system. These measurements are used to estimate the receiver position using the Single Point Positioning (SPP) algorithm [34]. Throughout the process of GNSS measurements, various errors can arise, such as orbital errors, clock deviations, and ionospheric and tropospheric errors. Therefore, we formulate the pseudorange measurement model to account for these errors:

$${\tilde{P}}_{r_t}^s=∥p_{s_t}^i-p_{r_t}^i∥+c\left({\delta }_{r_t}-{\delta }_{s_t}\right)+I_{r_t}^s+T_{r_t}^s+{\epsilon }_{r_t}^s$$

(4)

where, at time t, $p_{s_t}^i$ and $p_{r_t}^i$ denote the coordinates of satellite s and receiver r in the i-frame, respectively. ${\delta }_{r_t}$ denotes the clock deviation of the receiver. ${\delta }_{s_t}$ denotes the clock deviation of the satellite. c denotes the speed of light. $I_{r_t}^s$ and $T_{r_t}^s$ denote the ionospheric and tropospheric errors, respectively. ${\epsilon }_{r_t}^s$ denotes the measurement noise. $∥·∥$ is the Mahalanobis norm.

The Doppler effect introduces a frequency difference between the signals transmitted by the GNSS satellites and received by the receiver, known as the Doppler shift. This Doppler shift is utilized to represent the relative motion between the GNSS receiver and the satellite. It is influenced by factors such as clock frequency deviations and measurement noise. Thus, the Doppler shift model is expressed as

$$\Delta {\tilde{f}}_{r_t}^s=-{\displaystyle \frac{1}{\lambda }}[{{\kappa }_{r_t}^s}^T(v_{s_t}^i-v_{r_t}^i)+c(\delta \dot{t}-\Delta {\dot{t}}^s)+{\eta }_r^s]$$

(5)

where, in the i-frame at time t, ${\kappa }_r^{sT}$ denotes the unit vector of the receiver pointing to the satellite. $\lambda$ denotes the wavelength of the carrier signal. $v_{s_t}^i$ and $v_{r_t}^i$ denote the velocities of the receiver and the satellite, respectively. c denotes the speed of light. $\delta \dot{t}$ denotes the clock offset rate of the receiver. $\Delta {\dot{t}}^s$ denotes the clock offset rate of the satellite. ${\eta }_r^s$ denotes the Doppler measurement noise.

3.2. Estimator Initialization

The state estimation of a multi-sensor fusion system is inherently nonlinear. As a result, the accuracy and robustness of the system heavily rely on the initialization values. In this paper, we propose a method where the scale initial values of depth information, IMU pre-integration, and visual information are loosely coupled and processed to obtain the initial values of gyroscope bias, acceleration bias, and gravity vector through online optimization.

With known scale initial values, the fusion of visual and IMU information requires knowing the initial values of gravity vector, velocity, and IMU deviation. We solve the problem in two steps.

Gyroscope bias: In consecutive frames k and $k+1$ of the sliding window, according to the constraints on the gyroscope measurements and the camera keyframe orientation, the equation for the residuals of the rotation term is defined as follows:

$$r_k^g\left(b_g\right)=Log\left({(\Delta {\mathrm{R}}_k^{k+1}{\mathrm{Exp}(\mathrm{J}}_{\Delta \mathrm{R}}^g{b}_g\left)\right)}^{\mathrm{T}}{\mathrm{R}}_k^{\mathrm{T}}{\mathrm{R}}_{k+1}\right)$$

(6)

where $\Delta {\mathrm{R}}_k^{k+1}$ denotes the change in rotation from frame k to frame k+1. ${\mathrm{J}}_{\Delta \mathrm{R}}^g$ denotes the Jacobi matrix of pre-integrated rotations to gyroscope deviations, and its analytic expression can be found in reference [37]. ${\mathrm{R}}_k^{\mathrm{T}}$ is the transpose of the rotation at times k. ${\mathrm{R}}_{k+1}$ is the rotation at times $k+1$.

Gyroscope deviation is optimized by iterative optimization using the LM algorithm, and the optimization equation is

$${\chi }_g=\underset{b_g}{argmin}{∥r_p^g\left(b_g\right)∥}_{{\sum }_p^g}^2+\sum _k{∥r_k^g\left(b_g\right)∥}_{{\sum }_k^g}^2$$

(7)

where $r_p^g\left(b_g\right)$ is the a priori residual, iterating from zero deviation, which can be ignored, and $r_k^g\left(b_g\right)$ denotes the residual term of the gyroscope deviation.

Accelerometer bias, gravity vector, scale, velocity initialization: After initializing the gyroscope bias, calculate the accelerometer bias, gravity vector, and velocity for two consecutive frames k to $k+1$ moments [38], as follows:

$${\chi }_I=[v_{b_0},v_{b_1},\dots ,v_{b_n},{\mathrm{g}}^{c_0},s,b_a]$$

(8)

where $v_{b_k}$ is the velocity of the frame k image in the body frame. $g^{c_0}$ is the acceleration of gravity in the camera $c_0$ coordinate system, while $b_a$ is the accelerometer bias. s is the scaling parameter.

The position ${\alpha }_{b_k}^{b_{k+1}}$ and velocity ${\beta }_{b_k}^{b_{k+1}}$ of the IMU are computed in the body frame for two consecutive frames k to $k+1$ moments, as follows:

$$\begin{array}{cc}\hfill & {\alpha }_{b_k}^{b_{k+1}}={\mathrm{R}}_{c_0}^{b_k}(s({\overline{p}}_{c_{k+1}}-{\overline{p}}_{c_k})-{\displaystyle \frac{1}{2}}{\mathrm{g}}^{c_0}\Delta t_k^2-{\mathrm{R}}_{b_k}^{c_0}{v}_{b_k}\Delta {\mathrm{t}}_k+{\mathrm{J}}_{\Delta {\mathrm{p}}_k^{k+1}}^{b_a}{b}_a)\hfill \\ \hfill & {\beta }_{b_k}^{b_{k+1}}={\mathrm{R}}_{c_0}^{b_k}({\mathrm{R}}_{b_{k+1}}^{c_0}{v}_{b_{k+1}}-{\mathrm{g}}^{c_0}\Delta t_k-{\mathrm{R}}_{b_k}^{c_0}{v}_{b_k}+{\mathrm{J}}_{\Delta v_k^{k+1}}^{b_a}{b}_a)\hfill \end{array}$$

(9)

where ${\overline{p}}_{c_k}$ is the position at moment k in $c_0$ frame. $\Delta {\mathrm{t}}_k$ is the time difference between k and $k+1$ moments. ${\mathrm{R}}_{c_0}^{b_k}$ is the rotation matrix from $c_0$ frame to $b_k$ frame. ${\mathrm{J}}_{\Delta p_k^{k+1}}^a$ and ${\mathrm{J}}_{\Delta v_k^{k+1}}^a$ are the Jacobi matrices of pre-integrated position and velocity to acceleration bias, respectively. Reference [37] explains analytic expressions for the pre-integrated Jacobi matrix and uncertainty propagation. The least squares optimization model is established, as follows:

$$\underset{{\chi }_I}{min}\sum _{k\in N}{∥{\widehat{\mathrm{z}}}_{b_k}^{b_k+1}-{\mathrm{H}}_{b_k}^{b_k+1}{\chi }_I∥}^2$$

(10)

From (9), we obtain the following linear measurement model:

$$\begin{array}{c}\hfill {\widehat{\mathrm{z}}}_{b_k}^{b_{k+1}}=\left[\begin{array}{c}{\widehat{\alpha }}_{b_k}^{b_{k+1}}-p_c^b+{\mathrm{R}}_{c_0}^{b_k}{\mathrm{R}}_{b_{k+1}}^{c_0}{p}_c^b\\ {\widehat{\beta }}_{b_k}^{b_{k+1}}\end{array}\right]={\mathrm{H}}_{b_k}^{b_{k+1}}{\chi }_I+n_{b_k}^{b_{k+1}}\end{array}$$

`

where

$$\begin{array}{cc}& {\mathrm{H}}_{b_k}^{b_{k+1}}=\hfill \\ & \left[\begin{array}{ccccc}-I\Delta t_k& 0& -{\displaystyle \frac{1}{2}}{\mathrm{R}}_{c_0}^{b_k}\Delta t_k^2& {\mathrm{R}}_{c_0}^{b_k}({\overline{p}}_{c_{k+1}}-{\overline{p}}_{c_k})& {\mathrm{J}}_{\Delta p_k^{k+1}}^a{b}_a\\ -I& {\mathrm{R}}_{c_0}^{b_k}{\mathrm{R}}_{b_{k+1}}^{c_0}& {\mathrm{R}}_{c_0}^{b_k}\Delta t_k& 0& {\mathrm{J}}_{\Delta v_k^{k+1}}^a{b}_a\end{array}\right]\hfill \end{array}$$

The linear least squares model solution procedure is as follows:

$$\begin{array}{c}{\mathrm{H}}_{b_k}^{b_{k+1}}{\chi }_I={\widehat{\mathrm{z}}}_{b_k}^{b_{k+1}}\\ {\left({\mathrm{H}}_{b_k}^{b_{k+1}}\right)}^T{\mathrm{H}}_{b_k}^{b_{k+1}}{\chi }_I={\left({\mathrm{H}}_{b_k}^{b_{k+1}}\right)}^T{\widehat{\mathrm{z}}}_{b_k}^{b_{k+1}}\\ {\chi }_I={\left({\left({\mathrm{H}}_{b_k}^{b_{k+1}}\right)}^T{\mathrm{H}}_{b_k}^{b_{k+1}}\right)}^{-1}{\left({\mathrm{H}}_{b_k}^{b_{k+1}}\right)}^T{\widehat{\mathrm{z}}}_{b_k}^{b_{k+1}}\end{array}$$

(12)

According to the least squares optimization model, the velocity of each frame in the body frame, the acceleration of gravity in the camera frame, and accelerometer bias are obtained.

3.3. Factor Graph Model

Formulation: Our system uses a nonlinear optimization approach, and the system state can be summarized as

$$\begin{array}{cc}\hfill & \chi =[x_i,x_{i+1},x_{i+2},\cdots x_j]\hfill \\ \hfill & x_k=\left[q_{b_k}^n,v_{b_k}^n,p_{b_k}^n,s_k,b_{a_k},b_{g_k},\delta t_k,\delta {\dot{t}}_k\right],k\in \left[i,j\right]\hfill \end{array}$$

(13)

where, within the sliding window $[i,j]$ frame, the k-frame states include attitude $q_{b_k}^n$, velocity $v_{b_k}^n$, position $p_{b_k}^n$, scale $s_k$, accelerometer bias $b_{a_k}$, gyro bias $b_{g_k}$, receiver clock bias $\delta t_k$, and receiver clock drifting rate $\delta {\dot{t}}_k$.

We analyze the factor graph model, with residual factors constructed for each sensor, for back-end nonlinear optimization. The factor graph is shown in Figure 4. Build the maximum posterior probability model, as follows:

$$\begin{array}{cc}\hfill & {\chi }^{*}=\underset{\chi }{argmax}p\left(\chi \right|\mathrm{z})\hfill \\ \hfill & =\underset{\chi }{argmax}p\left(\chi \right)p\left(\mathrm{z}\right|\chi )\hfill \\ \hfill & =\underset{\chi }{argmin}\{{∥r_p∥}_{{\sum }_p}^2+\sum _k{∥{\mathrm{r}}_B({\tilde{z}}_{b_{t_k}}^{b_{t_{k+1}}},\chi )∥}_{{\sum }_k}^2+\sum _k{∥{\mathrm{r}}_c({\tilde{\mathrm{z}}}_l,\chi )∥}_{{\sum }_k}^2\hfill \\ \hfill & +\sum _k{∥r_p({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )∥}_{{\sum }_k}^2+\sum _k{∥r_D({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )∥}_{{\sum }_k}^2\}\hfill \end{array}$$

(14)

$r_p$ denotes the a priori information about the system state. ${\mathrm{r}}_B({\tilde{z}}_{b_{t_k}}^{b_{t_{k+1}}},\chi )$, ${\mathrm{r}}_c({\tilde{\mathrm{z}}}_l,\chi )$, $r_p({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )$, and $r_D({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )$ denote the IMU residuals, visual residuals, GNSS pseudorange residuals, and Doppler shift residuals, respectively. We decompose the optimization problem into individual factors related to states and measurements. Figure 4 shows the factorization diagram of our system. In addition to the factors from the measurements, there is an a priori factor used to constrain the sliding window system state. We discuss each factor in detail below.

Inertial measurement unit (IMU) residual factor: From the IMU measurement model in Equations (3) and (11), the residual of the IMU in the sliding window time is defined as

$$\begin{array}{c}{\mathrm{r}}_B({\tilde{z}}_{b_k}^{b_{k+1}},\chi )=\left[\begin{array}{c}\delta {\alpha }_{b_k}^{b_{k+1}}\\ \delta {\beta }_{b_k}^{b_{k+1}}\\ \delta {\theta }_{b_k}^{b_{k+1}}\\ \delta b_a\\ \delta b_g\end{array}\right]\hfill \\ =\left[\begin{array}{c}{\mathrm{R}}_n^{b_k}(p_{b_{k+1}}^n-p_{b_k}^n-{\displaystyle \frac{1}{2}}{\mathrm{g}}^n\Delta t_k^2-v_{b_k}^n\Delta t_k)-{\widehat{\alpha }}_{b_k}^{b_{k+1}}\\ {\mathrm{R}}_n^{b_k}(v_{b_{k+1}}^n+{\mathrm{g}}^n\Delta t_k-v_{b_k}^n)-{\widehat{\beta }}_{b_k}^{b_{k+1}}\\ 2{[{\left(q_{b_{k+1}}^n\right)}^{-1}\otimes q_{b_k}^n\otimes {\left({\widehat{\gamma }}_{b_k}^{b_{k+1}}\right)}^{-1}]}_{xyz}\\ b_{a_{k+1}}-b_{a_k}\\ b_{g_{k+1}}-b_{g_k}\end{array}\right]\hfill \end{array}$$

(15)

where the vector component of the quaternion q is extracted by ${[·]}_{xyz}$ for representing the error state. $\delta {\alpha }_{b_{t_{k+1}}}^{b_{t_k}},\delta {\beta }_{b_{t_{k+1}}}^{b_{t_k}},\delta {\theta }_{b_{t_{k+1}}}^{b_{t_k}},\delta b_a,\delta b_g$ denote the observed terms of position, velocity, attitude, acceleration bias, and gyro bias between two consecutive frames, respectively. ⊗ represents the multiplication operation between two quaternions.

Depth-scaled BA based on NNR: Using NNR can extract enough effective depth information from the discontinuous and incomplete depth images, but there are errors in the depth data obtained in this way, leading to the failure of the SLAM system. In this paper, we introduce depth bias $\delta \lambda$ for optimization, assuming that the depth is constant in a short time. After extracting the feature points of consecutive keyframes in the image and applying distortion correction to the feature points, the visual projection is modeled as

$$\tilde{\mathrm{P}}={\pi }_c({\mathrm{R}}_b^c({\mathrm{R}}_n^c{x}^n+p_n^b)+p_b^c)+n_c$$

(16)

where $\{{\mathrm{R}}_b^c,p_b^c\}$ is the rotation and translation between the IMU coordinate system and the camera coordinate system. $\{{\mathrm{R}}_n^c,p_n^b\}$ is the rotation and translation between the camera and navigation frame. $n_c$ is the measurement error. $x^n$ is the 3D feature points on the navigation frame. ${\pi }_c(·)$ is the camera projection function. Therefore, at the moment k, the reprojection residual of feature point l projected from frame i to frame j can be expressed as

$${\mathrm{r}}_c({\tilde{\mathrm{z}}}_l,\chi )={\tilde{\mathrm{P}}}_l^{c_{k_j}}-{\pi }_c({\widehat{\mathrm{x}}}_l^{c_{k_j}})$$

(17)

where

$${\widehat{\mathrm{x}}}_l^{c_{k_j}}={\mathrm{R}}_b^c({\mathrm{R}}_n^{b_{t_j}}({\mathrm{R}}_{b_{t_i}}^n({\mathrm{R}}_c^b{\delta \lambda }_l{\pi }_c^{-1}({\tilde{\mathrm{P}}}_l^{c_{k_i}})+p_c^b)+p_{b_{k_i}}^n)+p_n^{b_{k_i}})+p_b^c,$$

${\delta \lambda }_l$ is the depth bias of 3D feature points l. The depth value ${\lambda }_l^{new}$ of feature point l is updated as

$${\lambda }_l^{new}={\displaystyle \frac{1}{1+\delta {\lambda }_l}}{\lambda }_l^{old}$$

(18)

GNSS signal residual factor: We are able to compute the ECEF frame of the receiver at any moment for a given system state. Since the time of the GNSS measurement signal is marked by the receiver, we define the ECI frame at the moment of signal reception as coinciding with the ECEF frame. When the signal arrives at the receiver, we know ${\mathrm{p}}_r^i=p_r^e$. By broadcasting ephemeris and pseudorange measurements, the position of the satellite in the ECEF frame at the moment of signal transmission can be obtained as $p_s^{e^{\prime }}$. However, there exists the effect of the angular velocity of the Earth’s rotation ${\omega }_{ie}$, which changes the ECEF frame at the time the signal leaves the satellite from the time the signal arrives at the receiver. Therefore, it is necessary to convert the satellite position to the ECI coordinate system, as follows:

$$p_{s_j}^i=R_e^i(I+T({\omega }_{ie}\times ))p_{s_j}^{e^{\prime }}$$

(19)

The pseudorange is shown in (4). Therefore, the single pseudorange residual of the $j^{th}$ satellites received by the GNSS receiver r at moment k can be expressed as

$$r_p({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )=∥p_{s_j}^i-({\mathrm{R}}_n^i{p}_{r_k}^n+p_b^i)∥+c(\delta t_{r_k}-\delta t_{s_j})+T_{r_k}^{s_j}+I_{r_k}^{s_j}-{\tilde{P}}_{r_k}^s$$

(20)

The Doppler shift describes a phenomenon in which the frequency of a wave received by a receiver changes from the frequency actually transmitted by the satellite when there is relative motion between the satellite and the receiver, as expressed by (5). The Doppler shift is defined as a frame of the ECEF frame in which the satellite is transmitting a signal. By defining an ECI frame as an ECEF frame at reception, we know ${\mathrm{v}}_r^i={\mathrm{v}}_r^e$. The satellite’s velocity $v_s^{e^{\prime }}$ in the signaling ECEF frame is then transformed into an ECI frame, as follows:

$${\mathrm{v}}_{s_j}^i=R_e^i(I+T({\omega }_{ie}\times ))v_{s_j}^{e^{\prime }}$$

(21)

Therefore, the Doppler shift residual of the $j^{th}$ satellite s received by the GNSS receiver r at moment k can be expressed as

$$r_D({\tilde{\mathrm{z}}}_{r_k}^{s_j},\chi )={\displaystyle \frac{1}{\lambda }}{{\kappa }_{r_k}^{s_j}}^T(v_{s_j}^i-{\mathrm{R}}_n^i{v}_{r_k}^n)+{\displaystyle \frac{c}{\lambda }}(\dot{\delta }{t}_{r_k}-\Delta t_{s_j})+\Delta {\tilde{f}}_{r_k}^{s_j}$$

(22)

where $\{{\mathrm{R}}_n^i,p_b^i\}$ denotes the rotation and translation between body frame and ECI frame.

4. Experimental Results

We conduct experiments using the TUM RGBD dataset, as well as real UAV and vehicle tests, to evaluate the proposed GDVI-Fusion system. The TUM RGBD dataset provides time-stamped color and depth images, which we use to analyze the performance of the depth NNR algorithm. In the real UAV and vehicle experiments, this system is compared with renowned open-source systems such as VINS-Mono [5], VINS-Fusion [11], ORB-SLAM3 [10], and the GVINS [1] system. These comparative experiments focus on operational efficiency, scale error, and root mean square error, aiming to validate the localization accuracy and robustness of the GDVI-Fusion system. The system uses the processor Jetson TX2 with 256-core NVIDIA Pascal GPU, ARMv8 64-bit Cortex-A57 CPU, 8 GB memory, and D455 model camera, with the hardware shown in Figure 5 and Figure 6. The hardware parameters are shown in

Table 1. Hardware parameters.

Sensor Type	Item	Value
Camera	Sensor	RealSense D455
	Shutter	Global shutter
	RGB Resolution	$1280\times 800$
	RGB Fov	${90}^{\circ }\times {65}^{\circ }$
	Depth Resolution	$1280\times 720$
	Depth Fov	${87}^{\circ }\times {58}^{\circ }$
	Depth Accuracy	Less than 2% at 4 m
	Frame Rate	20 fps
IMU	Sensor	HY-INS-21B
	Frequency	200 Hz
	Gyro bias	0.003°/h
	Gyro random noise	0.0003°/h
	Accelerometer bias	10 $\mathsf{\mu }$g
	Accelerometer random noise	10 $\mathsf{\mu }$g
GNSS	Receiver	MarinePak7
	Antenna	Beitian BT-200
	Frequency	10 Hz
Hardware	Processors	Jetson TX2
	CPU	6 Core Denver and A57
	GPU	256 Core Pascal
	Memory	8GB 128bit LPDDR4
	Power	7.5 W (or 15 W)

.

4.1. Dataset Comparison

We utilize the TUM RGBD dataset to validate the performance of the depth NNR method proposed in this paper. Depth NNR, with a radius of 3 pixels, is applied to each feature point that has valid depth information. We analyze the accuracy of depth NNR and its impact on the robustness of the system’s localization.

Table 2. MAD and MRD.

Dataset	MAD			MRD
	5 mm	15 mm	25 mm	0.5%	1.5%	2.5%
fr1*	37.56	69.35	88.65	40.12	79.96	93.52
fr2*	25.64	36.65	68.33	30.56	50.65	75.44
fr3*	36.33	72.23	91.45	42.21	81.49	94.33

fr1* includes fr1_360, fr1_floor, fr1_desk, fr1_desk2, and fr1_room; fr2* includes fr2/360_hemisphere, fr2/360_kidnap, fr2/desk, fr2/large_no_loop, and fr2/large_with_loop; and fr3* includes fr3_office and fr3_nst.

displays the mean absolute deviation(MAD) and mean relative deviation(MRD) of the resampled depth values compared with the actual values. In the TUM/freiburg1 and TUM/freiburg3 sequences, almost 90% of the resampled depth values demonstrate high accuracy (MAD < 25 mm, MRD < 2.5%). In the TUM/freiburg2 sequences, about 70% of the resampled depth values achieve this level of accuracy. The depth NNR sampling method enhances the stability of visual scale information, mitigates the dispersion of system localization information due to scale information degradation, and effectively improves the robustness of the localization system.

4.2. UAV Experiments

In this experiment, the UAV performed indoor environment, outdoor environment, and outdoor flight through the courtyard building environment to evaluate the average scale error of the GDVI-Fusion system. Based on the UAV flying a section of the trajectory, the system calculated the scale, trajectory, real scale, and ground trajectory alignment, solving for the average scale error. As shown in Figure 7, Figure 8 and Figure 9 for the UAV test, only the open-loop VIO was used to test the system for the purpose of scale estimation comparisons. From the experiments, it can be seen that the inertial measurement unit of RealSense D455 has a general performance, and the estimated scale information is not accurate, which leads to a large drift of the monocular VINS.

Table 3. Mean scale error.

Mean Scale Error (%)	Outdoor	Indoor	Indoor–Outdoor
GDVI-Fusion	1.07	0.62	1.13
VINS-Mono [5]	1.6	3.03	3.91
VINS-Fusion [11]	1.37	1.48	1.83
GVINS [1]	1.19	0.84	1.37
ORB-SLAM3 [10]	1.25	0.85	1.77

displays the mean scale errors estimated by different systems. The depth NNR and depth optimization algorithm, as proposed by GDVI-Fusion, achieves an average scale error of 0.57% in outdoor environment tests, 0.12% in indoor environment tests, and 0.63% in outdoor crossing courtyard building environment tests. These results indicate that GDVI-Fusion can enhance the accuracy of scale estimation by up to 20% compared with other algorithms.

Scale error significantly impacts the accuracy of system localization, with excessive scale error potentially leading to dispersion of localization information. This study compares the actual localization trajectories generated by different algorithms against the real ground trajectories and calculates the root mean square errors (RMSE) of these trajectories to evaluate the effects of scale errors on the positioning accuracy of the system.

Table 4. Root mean square error.

Mean Scale Error (m)	Outdoor	Indoor	Indoor–Outdoor
GDVI-Fusion	0.125	0.116	0.133
VINS-Mono	0.808	0.298	1.287
VINS-Fusion	0.256	0.249	0.224
GVINS	0.155	0.132	0.162
ORB-SLAM3	0.85	0.35	0.63

presents the accuracy of UAV flight positioning trajectories generated by various algorithms in indoor, outdoor, and outdoor crossing courtyard building environments.

4.3. Vehicle-Based Experiments

The device is installed on the test vehicle to verify the tracking time per frame for feature matching, system operation time, and average reprojection error of different localization algorithms during vehicle operation. As shown in

Table 5. Tracking time consumption for different system features.

	Feature Matching Tracking (ms)	System Runtime (ms)	Average Reprojection Error
VINS [11]	91.8	499.0	2.314144
GVINS [1]	81.8	560.2	1.259165
ORB-SLAM3 [10]	32.5	428.2	1.548526
GDVI-Fusion	30.2	256.3	0.895625

, GDVI-Fusion enhances feature matching efficiency by almost 66%, improves system operation efficiency by approximately 50%, and reduces the average reprojection error to only 0.89 compared with other algorithms. The experiment demonstrates that our KNN-based initial matching and optimization algorithms effectively increase the efficiency and accuracy of feature matching. Our GDVI-Fusion system requires minimal time for feature detection and tracking and maintains a low reprojection error, contributing to improved operational efficiency, positioning accuracy, and robustness of the localization system. The feature tracking match is shown in Figure 10.

To evaluate the localization accuracy and robustness of various algorithms in urban complex environments characterized by high-rise buildings, numerous obstructions, significant light variations, and tunnels that impact sensor performance, we conduct a vehicle test in the Jimei District of Xiamen City. This test spans a total distance of 36.37 km over 1 h. The vehicle’s trajectory, depicted in Figure 11, includes areas with tall buildings, dim and bright lighting conditions, rapid vehicle movement, and high dynamic scenarios that challenge the accuracy of single sensors.

In these testing conditions, VINS-Mono experiences substantial visual disruptions upon entering brightly lit areas, leading to significant localization offsets and noticeable drift. VINS-Fusion encounters large fluctuations in localization data as the vehicle accelerates or passes through tunnels, struggling to accurately maintain vehicle localization. Conversely, both the GVINS and the GDVI-Fusion algorithms, as proposed in this paper, successfully complete the urban complex environment test, demonstrating resilience against single-sensor positioning degradation. The test results, presented in

Table 6. Errors in vehicle tests.

	VINS-Mono	VINS-Fusion	GVINS	GDVI-Fusion
Horizontal positioning error (m)	-	-	6.782	4.667
Root mean square error of on-board test (m)	-	-	8.729	5.485

, indicate that, over a 1 h period and a total distance of 36.37 km, the GVINS system incurs a horizontal positioning error of 6.782 m (95% CI: [5.43, 8.14] m) and a root mean square error of 8.729 m (95% CI: [6.98, 10.48] m). In contrast, the GDVI-Fusion system achieves a horizontal positioning error of 4.667 m (95% CI: [3.73, 5.60] m) and a root mean square error of 5.729 m (95% CI: [4.58, 6.87] m). We conducted a test in a GPS-denied environment (tunnel) to validate the positioning accuracy of different positioning algorithms. The horizontal positioning error and root mean square error (RMSE) for the VINS-Mono system are 45.352 m (95% CI: [44.352, 46.352] m) and 46.252 m (95% CI: [45.252, 47.252] m), respectively. For the VINS-Fusion system, the horizontal positioning error and RMSE are 57.755 m (95% CI: [56.755, 58.755] m) and 59.364 m (95% CI: [58.364, 60.364] m), respectively. For the GVINS system, the horizontal positioning error and RMSE are 26.224 m (95% CI: [25.224, 27.224] m) and 28.014 m (95% CI: [27.014, 29.014] m), respectively. For the GDVI-Fusion system, the horizontal positioning error is 4.017 m (95% CI: [3.017, 5.017] m) and the RMSE is 5.889 m (95% CI: [4.889, 6.889] m).

The horizontal positioning accuracy of the proposed algorithm improves by 87.2% compared with that of VINS-Mono, 90.1% compared with that of VINS-Fusion, and 37.1% compared with that of GVINS, as shown in

Table 7. Tunnel positioning error.

	VINS-Mono	VINS-Fusion	GVINS	GDVI-Fusion
Horizontal positioning error (m)	45.352	57.755	26.224	4.017
Root mean square error of on-board test (m)	46.252	59.364	28.014	5.889

. Therefore, these results validate the GDVI-Fusion system’s robustness and accuracy in navigating complex urban environments.

5. Conclusions

The RGBD visual–inertial and GNSS tightly coupled, nonlinear optimization positioning system (GDVI-Fusion) we proposed addresses the issue of positioning drift in navigation systems, particularly in complex environments or GNSS-failure scenarios. First, during initialization, the depth information is preprocessed, mitigating the effects of lighting and physical structure on the RGBD camera, thus improving the accuracy of the image feature point depth information. Second, the KNN algorithm is used to process the feature points, and optimal geometric constraints are applied to matching point pairs. This process eliminates feature matches with abnormal matching line lengths and slopes, thereby enhancing feature matching efficiency and accuracy. Finally, the image information and depth data from the RGBD camera are tightly coupled with the raw GNSS measurement data using a factor graph optimization state method. In urban environment vehicle tests, our algorithm improves operational efficiency by 54.2% over GVINS and 40.1% over ORB-SLAM3, and our positioning accuracy is improved by 37.1% compared with that of GVINS.