Monocular vision inertia synchronous positioning and mapping method of explicit fusion depth estimation basic model
By explicitly fusing the basic depth estimation model, adjusting the feature point extraction threshold, and constructing a bundle adjustment optimization problem, the problem of fusing depth information with the visual SLAM module was solved, improving SLAM positioning accuracy and dense mapping capability, and realizing the effective utilization of depth information.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIHANG UNIV
- Filing Date
- 2026-02-27
- Publication Date
- 2026-04-21
AI Technical Summary
Existing SLAM methods that integrate depth estimation have failed to effectively integrate depth information with other computational modules of visual SLAM. Furthermore, the scale-free disparity relationships output by the depth estimation network are difficult to directly integrate into RGB-D mapping algorithms, which limits the applicability and accuracy of the system.
By explicitly fusing the basic depth estimation model, adjusting the feature point extraction threshold and establishing feature point tracking based on optical flow, a bundle adjustment optimization problem is constructed, depth constraint residuals are added, and depth mapping parameters are obtained through bundle adjustment optimization to restore the metric depth map for dense mapping.
It improves SLAM positioning accuracy and dense mapping capabilities, realizes cross-modal propagation and implicit regularization of depth information in parameter space, and enhances the depth perception capability of monocular vision inertial SLAM system.
Smart Images
Figure CN121898397A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the fields of robotics, computer vision, multi-sensor data fusion, and deep learning, and specifically to a method for simultaneous localization and mapping of monocular visual inertial data based on an explicit fusion of a depth estimation model. Background Technology
[0002] Simultaneous Localization and Mapping (SLAM) technology, first proposed by Smith in 1986, has undergone nearly 40 years of development and is now widely used in applications such as robot environmental perception, autonomous navigation, autonomous driving, and augmented reality. It has evolved from early single image sensors to more advanced sensors, such as LiDAR sensors that directly acquire absolute depth, millimeter-wave radar sensors suitable for harsh environments like rain and fog, inertial measurement units that are not limited by the type of external environment, and event cameras with higher information density, or combinations of these sensors. These sensors complement each other, achieving high localization and mapping accuracy in complex scenes.
[0003] While LiDAR-based SLAM methods can directly acquire scale information and possess high accuracy, their limitations in sensor weight, energy consumption, and cost make them difficult to deploy in resource-constrained devices such as lightweight robots or drones. In contrast, visual sensors and IMUs, due to their high maturity, low cost, light weight, and ease of calibration, show promising application prospects for SLAM combining monocular vision and IMUs in indoor scenes with limited depth range.
[0004] Visual sensors can capture rich texture information and have been widely used in downstream tasks such as image classification, object detection, image segmentation, semantic segmentation, and depth estimation. Among these, depth estimation techniques, based on neural networks, can directly infer pixel-level depth from image input, thus providing depth information for monocular visual SLAM. Compared to the triangulation methods commonly used in traditional monocular visual SLAM, this technique eliminates the need for complex indirect calculations, enabling more direct acquisition of depth information. By generating a depth map and combining it with the original RGB image, a pseudo-RGB-D input can be formed, which can be directly fed into RGB-D mapping algorithms to perform dense mapping. However, this method still faces two challenges:
[0005] Challenge (1): Depth estimation networks need to output metric-scale results, which places strict requirements on training data. The training dataset must use absolute depth values rather than relative depth relation representations and contain accurate metadata such as camera intrinsics. This strong constraint means that the network can only be trained on a small number of standard datasets, making it difficult to adapt to diverse scenes and incompatible with inputs from different camera intrinsics, which significantly limits the applicability of the system.
[0006] Challenge (2): Existing SLAM methods that fuse depth estimation have failed to achieve effective fusion of depth information with other computational modules of visual SLAM. Mainstream methods usually adopt a loosely coupled approach, using only depth information as the front-end input, without forming joint constraints with perception data such as IMU measurements and pose priors in the back-end optimization.
[0007] In recent years, the academic community has used large-scale unlabeled depth estimation datasets to train depth estimation networks with strong zero-shot generalization capabilities, known as "foundational depth estimation models." Typical algorithms include MiDaS proposed by René Ranftl et al. and Depth Anything proposed by Lihe Yang et al. These models normalize the disparities of different datasets to a specific disparity space during training, thus outputting only scale-free disparity relationships during the inference phase. Ideally, the inference depth should be similar to the metric depth. The following relationship exists between them:
[0008]
[0009] In the formula, for image input : For image No. Line number The estimated distance (i.e., the metric depth) from the position corresponding to the column pixel to the camera coordinate system reference plane; For images The first depth estimation neural network outputs the first depth. Line number The value corresponding to the column; This is a proportional parameter; This is the offset parameter. And the parameter... and Only with the image Changes with pixel position Irrelevant.
[0010] While the above methods address challenge (1) to some extent by improving cross-scene generalization through a unified depth representation, the output of the depth estimation base model remains a scale-free normalized depth, which cannot be directly integrated into RGB-D mapping algorithms. Therefore, how to effectively utilize the scale-free depth data output by the depth estimation base model to serve the localization and dense mapping of visual inertial SLAM systems remains a research problem that urgently needs to be solved. Summary of the Invention
[0011] In view of the limitations of existing technologies, this invention proposes a simultaneous localization and mapping method for monocular visual inertial navigation that explicitly fuses a depth estimation base model. The basic process involves the SLAM method front end adjusting the feature point extraction threshold based on the disparity gradient map generated by the depth estimation base model and establishing optical flow-based feature point tracking; the SLAM method back end adding depth constraint residuals and obtaining depth mapping parameters through bundle adjustment; and then recovering the metric depth map based on the depth mapping parameters and performing dense mapping. Finally, depth constraints and multi-source information such as geometry and inertia are unified into a joint optimization framework, enabling cross-modal propagation and implicit regularization of depth information in the gradient flow of the parameter space.
[0012] A method for simultaneous localization and mapping of monocular vision-inertial systems based on an explicit fusion depth estimation model includes the following steps:
[0013] Step S1: Acquire color images, linear acceleration, and angular velocity sequence data from a calibrated camera and inertial measurement unit (IMU); select the original image as the current frame in chronological order and perform distortion correction processing on it; input the distortion-corrected image into the depth estimation base model to obtain a disparity map to be calibrated containing scaling and offset biases; perform min-max normalization processing on the disparity map to be calibrated to obtain a normalized disparity map to be calibrated; calculate the spatial domain gradient map of the normalized disparity map to be calibrated, and adjust the feature point extraction threshold and extract feature points according to the magnitude of the gradient in the gradient map; complete feature point tracking based on optical flow method.
[0014] Step S2: Construct the bundle adjustment optimization problem, including visual reprojection residuals, IMU pre-integration residuals, and depth constraint residuals; the depth constraint residuals establish landmark point constraints based on common-view relationships for each frame image within the bundle adjustment optimization window; using the normalized disparity map to be calibrated described in Step S1, backproject the corresponding feature points of the landmark points in the pixel coordinate system to the camera coordinate system; project them to different frame camera coordinate systems through Euclidean transformation to construct reprojection residuals and calculate the Jacobian matrix; use the Levenberg-Marquardt algorithm to simultaneously optimize the pose, the mapping parameters of the normalized disparity map to be calibrated to the metric depth map within the bundle adjustment optimization window, until each optimization parameter reaches the convergence condition; in the context of this invention, "metric depth map" refers to an image aligned with the distortion-free image, where the value stored in each pixel represents the projection distance from that point in the scene to the camera optical center on the Z-axis, in meters;
[0015] Step S3: Using the normalized disparity map to metric depth map mapping parameters described in Step S2, obtain the estimated value of the metric depth map; input the estimated value of the metric depth map and the corresponding distorted image to form a pseudo-RGB-D image; input the pseudo-RGB-D image and the pose described in Step S2 into the reconstruction algorithm based on the Truncated Signed Distance Function (TSDF) method, and achieve dense reconstruction of the scene through voxel fusion, finally obtaining the triangular mesh model of the scene.
[0016] Furthermore, the adjustment of the feature point extraction threshold in step S1 is based on the following principles:
[0017] In regions of disparity abrupt change with a large gradient magnitude, a higher feature point extraction threshold is set to remove unstable feature points at the edges; in regions of smooth disparity with a small gradient magnitude, the feature point extraction threshold is lower to retain more effective features; the feature point extraction threshold is calculated as follows:
[0018]
[0019] In the formula, This serves as the baseline threshold for feature point extraction, and its range is determined based on the feature point extraction algorithm used. For the ORB feature point extraction algorithm, This represents the FAST corner extraction threshold, which can be set to... For the Shi-Tomas corner detection algorithm, This represents the quality coefficient, which can be set to... ; For the adjusted first Frame pixel coordinates Threshold for feature point extraction at the location; This is an adjustment factor used to balance the contribution of the gradient to the feature point extraction threshold; it can be set to... ; For the first Frame normalized disparity map to be calibrated Location First gradient, Either 1 or 2 can be chosen.
[0020] Furthermore, step S2 also includes:
[0021] First, for each frame of the bundle adjustment optimization window, a landmark constraint based on common-view relation is established; then, each frame within the bundle adjustment optimization window is traversed; for the traversed frame... Frame, further traversal can be performed on the first frame. Each landmark observed in the frame; for each landmark that has been traversed. Search for the excluding the first Other out-of-frame capabilities for road signs The frame from which the observation is generated is denoted as the first frame. Frame; To ensure that the description of this shared view relationship is not repeated during the above multi-level traversal process, it must satisfy... ;
[0022] Secondly, the metric depth of the feature points is estimated using the normalized disparity map to be calibrated and the mapping parameters from the normalized disparity map to the metric depth map. The corresponding feature points of the landmark points in the pixel coordinate system are then back-projected to the camera coordinate system. The mapping parameters from the normalized disparity map to the metric depth map include the first... Frame scaling parameters and offset parameters Use both to obtain road signs In the The estimated metric depth at the pixel coordinates of the frame. The calculation is as follows:
[0023]
[0024] In the formula, For road signs In the The normalized parallax to be calibrated at the projection point location in the pixel coordinate system of the frame; using the estimated metric depth, the parallax located at the landmark point... In the The projection points in the frame pixel coordinate system are back-projected to the first... In the frame camera coordinate system, the calculation is as follows:
[0025]
[0026] In the formula, For road signs In the Normalized coordinates in the frame camera coordinate system;
[0027] Finally, the reprojection residuals are projected onto different frame camera coordinate systems using Euclidean transformation to construct the reprojection residuals and calculate the Jacobian matrix; Projected to the In the frame camera coordinate system, the projected coordinates are obtained. The calculation is as follows:
[0028]
[0029] In the formula, and Represent the rotation matrix and translation vector between the corresponding coordinate systems, respectively; labeling , , , , They represent camera, body, and the first, respectively. Frame body, the first Frame body and world coordinate systems; based on projected coordinates and coordinates Calculate residuals The calculation is as follows:
[0030]
[0031] Calculate the residuals using the perturbation method. The partial derivatives with respect to other optimization variables are used to construct the Jacobian matrix of the depth-constrained residuals.
[0032] The beneficial effects of this invention are as follows: Based on the challenges faced by existing technologies, this invention proposes a simultaneous localization and mapping method for monocular visual inertial navigation that explicitly fuses a depth estimation base model. This method can introduce the prior depth information contained in the scale-free disparity map generated by the depth estimation network into the SLAM process. Without modifying the optimization factors in the original method, adding a depth constraint factor can introduce more prior information into the SLAM module, strengthen the constraints on the variables to be optimized, and improve SLAM localization accuracy. Simultaneously, by using inverse normalization mapping, the relative depth map is converted into a metric depth map, facilitating precise localization and dense mapping in visual SLAM. Attached Figure Description
[0033] Figure 1 This is a diagram illustrating the architecture of the SLAM method in an embodiment of the present invention.
[0034] Figure 2This is a schematic diagram of the constraints for bundle adjustment optimization in the SLAM method. Detailed Implementation
[0035] To more clearly illustrate the intended objectives, technical means, and effects of the present invention, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.
[0036] It should be noted that, unless otherwise specified, all technical and scientific terms involved in this invention have their generally accepted meanings in the relevant technical field. Experimental methods in the following embodiments that do not specify specific conditions were performed according to conventional techniques or ordinary conditions in the art. It should be understood that the described embodiments are merely some examples of the present invention and not exhaustive. All other embodiments obtained by those skilled in the art based on the disclosed embodiments of this invention without inventive effort should be included within the scope of protection of this invention.
[0037] Example 1
[0038] To address the problem that existing SLAM methods based on fusion depth estimation fail to effectively integrate depth information with other computational modules in visual SLAM, and to provide an effective technical solution for high-precision positioning and dense map construction, this invention proposes a simultaneous localization and mapping method for monocular visual inertial SLAM that explicitly fuses a depth estimation base model. This method explicitly embeds the powerful spatial perception capabilities of the depth estimation base model into the SLAM framework, thereby enhancing the depth perception capabilities of the monocular visual inertial SLAM system. The basic process involves acquiring relative depth data based on the depth estimation base model, constructing relative depth constraint factors based on the relative depth data and performing bundle adjustment optimization, recovering the metric depth map using the optimized parameters, and incrementally updating the map. Figure 1 As shown, the method for simultaneous localization and mapping of monocular vision-inertial based on the explicit fusion depth estimation model includes the following steps:
[0039] Step S1: Acquire color images, linear acceleration, and angular velocity sequence data from the calibrated camera and IMU; select the original image as the current frame in chronological order and perform distortion correction processing on it; input the distortion-corrected image into the depth estimation base model to obtain a disparity map to be calibrated containing scaling and offset bias; perform min-max normalization processing on the disparity map to be calibrated to obtain a normalized disparity map to be calibrated; calculate the spatial domain gradient map of the normalized disparity map to be calibrated, and adjust the feature point extraction threshold and extract feature points according to the magnitude of the gradient in the gradient map; complete feature point tracking based on optical flow method.
[0040] Step S1.1: Acquire color images, linear acceleration, and angular velocity sequence data from the calibrated camera and IMU.
[0041] Typical visual-inertial SLAM datasets can be selected, such as KITTI Odometry, EuRoC, M2DGR, and other public datasets. These datasets have already been calibrated for both the camera and IMU, and contain metadata such as camera intrinsics, IMU intrinsics, and camera and IMU extrinsic parameters.
[0042] Step S1.2: Select the original image as the current frame in chronological order and perform distortion correction processing on it.
[0043] Depending on the camera intrinsics and camera lens model, the original image The pixels in the image are remapped to obtain the distortion-free image. .
[0044] Step S1.3: Input the distortion-reduced image into the depth estimation base model to obtain the disparity map to be calibrated, which includes scaling and offset bias.
[0045] The depth estimation base model uses only RGB images as input. Before input, a preprocessing process is involved to adapt the image to the numerical space selected during training. This process includes data type conversion, data transfer, channel transpose, and color value normalization. The final result is the disparity map to be calibrated. .
[0046] Step S1.4: Perform minimum-maximum normalization on the disparity map to be calibrated to obtain a normalized disparity map to be calibrated.
[0047] Iterate through the image to obtain the maximum value of the disparity map to be calibrated. and minimum value Minimum-maximum normalization is performed to obtain a normalized disparity map to be calibrated. The calculation is as follows:
[0048]
[0049] Step S1.5: Calculate the spatial domain gradient map of the normalized disparity map to be calibrated, and adjust the feature point extraction threshold and extract feature points according to the magnitude of the gradient in the gradient map.
[0050] Differential operators such as the Sobel and Laplace operators are used to normalize the disparity map to be calibrated. Perform spatial domain convolution to calculate pixel-level results. Step gradient plot In regions of abrupt disparity change with a large gradient magnitude, the feature point extraction threshold is higher to suppress potentially unstable feature point tracking at foreground-background boundaries and object edges. In regions of gentle disparity with a small gradient magnitude, the feature point extraction threshold is lower to extract more feature points, thereby enhancing the tracking of stable feature points. The feature point extraction threshold is calculated as follows:
[0051]
[0052] In the formula, This serves as the base threshold for feature point extraction, and its range is determined based on the feature point extraction algorithm used; for the ORB corner detection algorithm, This represents the FAST corner threshold, which can be set to... For the Shi-Tomas corner detection algorithm, This represents the quality coefficient, which can be set to... ; For the adjusted first Frame pixel coordinates Threshold for feature point extraction at the location; This is an adjustment factor used to balance the contribution of the gradient to the feature point extraction threshold; it can be set to... ; For the first Frame normalized disparity map to be calibrated Location First gradient, Either 1 or 2 can be chosen.
[0053] Step S1.6: Complete feature point tracking based on optical flow method.
[0054] Using the Lucas-Kanade optical flow method, optical flow tracking is performed based on the feature points extracted from the current frame and the previous frame, and the matching relationship of the feature points is recorded.
[0055] Step S2: Construct the bundle adjustment optimization problem, including visual reprojection residuals, IMU pre-integration residuals, and depth-constrained residuals, such as... Figure 2As shown; the depth constraint residual establishes landmark point constraints based on common-view relationships for each frame of the image within the bundle adjustment optimization window; using the normalized disparity map to be calibrated described in step S1, the corresponding feature points of the landmark points in the pixel coordinate system are back-projected to the camera coordinate system; the Euclidean transformation is used to project them to different frame camera coordinate systems to construct reprojection residuals and calculate the Jacobian matrix; the Levenberg-Marquardt algorithm is used to simultaneously optimize the pose, the normalized disparity map to be calibrated, and the mapping parameters to the metric depth map within the bundle adjustment optimization window until each optimization parameter reaches the convergence condition; in the context of this invention, the "metric depth map" refers to the image aligned with the distortion-free image, where the value stored in each pixel represents the projection distance from that point in the scene to the camera optical center on the Z-axis, in meters.
[0056] Step S2.1: For each frame of image within the bundle adjustment optimization window, establish landmark constraints based on common-view relationships.
[0057] Iterate through each frame within the bundle adjustment optimization window; for the frame that is traversed... Frame, further traversal can be performed on the first frame. Each landmark observed in the frame; for each landmark that has been traversed. Search for the excluding the first Other out-of-frame capabilities for road signs The frame from which the observation is generated is denoted as the first frame. Frame; To ensure that the description of this shared view relationship is not repeated during the above multi-level traversal process, it must satisfy... .
[0058] Step S2.2: Using the normalized disparity map to be calibrated described in step S1, back-project the corresponding feature points of the landmark points in the pixel coordinate system to the camera coordinate system.
[0059] The mapping parameters from the normalized disparity map to the metric depth map include the first... Frame scaling parameters and the Frame offset parameter Use both to obtain road signs In the The estimated metric depth at the pixel coordinates of the frame. The calculation is as follows:
[0060]
[0061] In the formula, For road signs In the The normalized parallax to be calibrated at the projection point position in the pixel coordinate system of the frame; in bundle adjustment optimization, the scaling parameter Initialized to 1, offset parameter It was initialized to 0;
[0062] Using metric depth estimates, the locations of landmarks In the The projection points in the frame pixel coordinate system are back-projected to the first... In the frame camera coordinate system, the calculation is as follows:
[0063]
[0064] In the formula, For road signs In the Normalized coordinates in the frame camera coordinate system;
[0065] Step S2.3: Project the image onto the camera coordinate system of different frames using Euclidean transformation to construct the reprojection residual and calculate the Jacobian matrix.
[0066] Through Euclidean transformation, Projected to the In the frame camera coordinate system, the projected coordinates are obtained. The calculation is as follows:
[0067]
[0068] In the formula, and Represent the rotation matrix and translation vector between the corresponding coordinate systems, respectively; labeling They represent camera and the first Frame body and world coordinate systems; based on projected coordinates and coordinates Calculate residuals The calculation is as follows:
[0069]
[0070] For the Frame and the Frame pose representation and In bundle adjustment optimization, for the initialization phase, the rotation matrix... and Initialized as an identity matrix, translation vector and Initialized to a zero vector; for the non-initialization phase, the rotation matrix and translation vector are initialized to the rotation matrix and translation vector of the second newest keyframe after the last optimization.
[0071] Calculate the residuals using the perturbation method. The partial derivatives with respect to other optimization variables are used to construct the Jacobian matrix of the depth-constrained residuals.
[0072] Step S2.4: Use the Levenberg-Marquardt algorithm to simultaneously optimize the pose and the normalized mapping parameters from the disparity map to the metric depth map within the bundle adjustment optimization window until each optimization parameter reaches the convergence condition.
[0073] Incorporating all residuals constructed within the bundle adjustment optimization window into the optimization problem, along the gradient direction indicated by the Jacobian matrix, for all residuals... , , , , , The optimization parameters, including those included, are iteratively adjusted; the convergence condition is set as the change between two iterations being less than a threshold, or reaching the preset maximum number of iterations.
[0074] Step S3: Using the normalized disparity map to metric depth map mapping parameters described in Step S2, obtain the estimated value of the metric depth map; input the estimated value of the metric depth map and the corresponding distorted image to form a pseudo-RGB-D image; input the pseudo-RGB-D image and the pose described in Step S2 into the reconstruction algorithm based on the TSDF method, and achieve dense reconstruction of the scene through voxel fusion, finally obtaining the triangular mesh model of the scene.
[0075] Step S3.1: Using the normalized mapping parameters from the disparity map to the metric depth map described in step S2, obtain the estimated value of the metric depth map.
[0076] Use depth mapping scaling parameters and offset parameters The estimation of mapping the normalized depth map to the metric depth map is calculated as follows:
[0077]
[0078] In the formula, This is an element-wise division of a matrix by a scalar.
[0079] Step S3.2: Input the estimated value of the metric depth map and the corresponding distorted image to form a pseudo-RGB-D image.
[0080] Estimation of metric depth maps with the same resolution Corresponding distortion-free image Encapsulate it as a sensor_msgs / Image message of the Robot Operating System (ROS) and add the same timestamp.
[0081] Step S3.3: Input the pseudo-RGB-D image and the pose described in step S2 into the reconstruction algorithm based on the TSDF method, and achieve dense reconstruction of the scene through voxel fusion to finally obtain the triangular mesh model of the scene.
[0082] The pose described in step S2 is encapsulated into a ROS tf / tfMessage message and given the same timestamp as the pseudo RGB-D image message described in step S3.2; each message is published and the visualization results of the constructed map are viewed in Rviz.
Claims
1. A method for simultaneous localization and mapping of monocular visual inertial systems based on an explicit fusion depth estimation fundamental model, characterized in that, Includes the following steps: Step S1: Acquire color images, linear acceleration, and angular velocity sequence data from the calibrated camera and inertial measurement unit (IMU); select the original image as the current frame in chronological order and perform distortion correction processing on it; input the distortion-corrected image into the depth estimation base model to obtain a disparity map to be calibrated containing scaling and offset biases; The disparity map to be calibrated is subjected to minimum-maximum normalization to obtain a normalized disparity map to be calibrated. Calculate the spatial domain gradient map of the normalized disparity map to be calibrated, and adjust the feature point extraction threshold and extract feature points according to the magnitude of the gradient in the gradient map; Feature point tracking based on optical flow method; Step S2: Construct the bundle adjustment optimization problem, including visual reprojection residuals, IMU pre-integration residuals, and depth constraint residuals; the depth constraint residuals establish landmark point constraints based on common-view relationships for each frame image within the bundle adjustment optimization window; using the normalized disparity map to be calibrated described in Step S1, backproject the corresponding feature points of the landmark points in the pixel coordinate system to the camera coordinate system; project them to different frame camera coordinate systems through Euclidean transformation to construct reprojection residuals and calculate the Jacobian matrix; use the Levenberg-Marquardt algorithm to simultaneously optimize the pose, the mapping parameters of the normalized disparity map to be calibrated to the metric depth map within the bundle adjustment optimization window, until each optimization parameter reaches the convergence condition; where the metric depth map refers to the image aligned with the distortion-free image, and the value stored in each pixel represents the projection distance from the corresponding point in the scene to the camera optical center on the Z-axis, in meters; Step S3: Using the normalized disparity map to metric depth map mapping parameters described in Step S2, obtain the estimated value of the metric depth map; input the estimated value of the metric depth map and the corresponding distorted image to form a pseudo-RGB-D image; input the pseudo-RGB-D image and the pose described in Step S2 into the reconstruction algorithm based on the Truncated Signed Distance Function (TSDF) method, and achieve dense reconstruction of the scene through voxel fusion, finally obtaining the triangular mesh model of the scene.
2. The method for simultaneous localization and mapping of monocular visual inertial systems based on an explicit fusion depth estimation fundamental model according to claim 1, characterized in that: The adjustment of the feature point extraction threshold in step S1 follows these principles: In regions with abrupt disparity changes and large gradient magnitudes, a higher feature point extraction threshold is set to eliminate unstable edge feature points; in regions with gentle disparity changes and small gradient magnitudes, a lower feature point extraction threshold is used to retain more effective features. The feature point extraction threshold is calculated as follows: In the formula, This serves as the base threshold for feature point extraction, and its range is determined based on the feature point extraction algorithm used; for the ORB corner detection algorithm, This represents the FAST corner threshold, which can be set to... For the Shi-Tomas corner detection algorithm, This represents the quality coefficient, which can be set to... ; For the adjusted first Frame pixel coordinates Threshold for feature point extraction at the location; This is an adjustment factor used to balance the contribution of the gradient to the feature point extraction threshold; it can be set to... ; For the first Frame normalized disparity map to be calibrated Location First gradient, Either 1 or 2 can be chosen.
3. The method for simultaneous localization and mapping of monocular visual inertial systems based on an explicit fusion depth estimation fundamental model according to claim 1, characterized in that: Step S2 also includes: First, for each frame of the bundle adjustment optimization window, a landmark constraint based on common-view relation is established; then, each frame within the bundle adjustment optimization window is traversed; for the traversed frame... Frame, further traversal can be performed on the first frame. Each landmark observed in the frame; for each landmark that has been traversed. Search for the excluding the first Other out-of-frame capabilities for road signs The frame from which the observation is generated is denoted as the first frame. Frame; To ensure that the description of this shared view relationship is not repeated during the above multi-level traversal process, it must satisfy... ; Secondly, the metric depth of the feature points is estimated using the normalized disparity map to be calibrated and the mapping parameters from the normalized disparity map to the metric depth map. The corresponding feature points of the landmark points in the pixel coordinate system are then back-projected to the camera coordinate system. The mapping parameters from the normalized disparity map to the metric depth map include the first... Frame scaling parameters and offset parameters Use both to obtain waypoints In the The estimated metric depth at the pixel coordinates of the frame. The calculation is as follows: In the formula, For road signs In the The normalized parallax to be calibrated at the projection point location in the pixel coordinate system of the frame; using the estimated metric depth, the parallax located at the landmark point... In the The projection point in the frame pixel coordinate system is back-projected to the first... In the frame camera coordinate system, the calculation is as follows: In the formula, For road signs In the Normalized coordinates in the frame camera coordinate system; Finally, the reprojection residuals are projected onto different frame camera coordinate systems using Euclidean transformation to construct the reprojection residuals and calculate the Jacobian matrix; Projected to the In the frame camera coordinate system, the projected coordinates are obtained. The calculation is as follows: In the formula, and Represent the rotation matrix and translation vector between the corresponding coordinate systems, respectively; labeling , , , , They represent camera, body, and the first, respectively. Frame body, the first Frame body and world coordinate systems; based on projected coordinates and coordinates Calculate residuals The calculation is as follows: Calculate the residuals using the perturbation method. The partial derivatives with respect to other optimization variables are used to construct the Jacobian matrix of the depth-constrained residuals.