Edge SLAM dense map construction method based on Kirsch combined high and low thresholds

Abstract

The invention discloses an edge SLAM dense map construction method based on Kirsch combined high and low thresholds. The method comprises the following steps: inputting an RGB-D picture into an improved visual odometer, extracting Kirsch edge features of a color image, performing instance segmentation on the color image of a key frame by using a BlendMASK deep neural network, and fusing semantic information into the color image. And distance transformation is constructed according to the binary image of the Kirsch edge and is used for calculating a residual error of 3D-2D registration of the key frame and the current frame. And then, carrying out pose estimation on the current frame by using 3D-2D registration of the key frame and the current frame segmented by the fusion instance. A sliding window local BA optimization algorithm based on a foreground edge is used for carrying out local optimization on the pose, and on this basis, a closed-loop detection algorithm based on a random fern descriptor is used for carrying out global pose graph optimization. And finally, based on the camera projection model and the inverse projection function, constructing a dense point cloud map. According to the method, an edge detection algorithm combining Kirsch with high and low thresholds, a BlendMASK deep neural network, a sliding window local BA optimization algorithm based on foreground edges and a closed-loop detection algorithm based on random fern descriptors are combined, so that the mapping accuracy can be improved, and a remarkable effect is achieved in the aspect of realizing an accurate dense point cloud map.

Description

Technical Field

[0001] The present invention is applicable to the field of simultaneous localization and mapping (SLAM). Specifically, it relates to edge detection based on Kirsch combined high and low double thresholds, instance segmentation visual odometer integrated with BlendMASK deep neural network, sliding window local BA (Bundle Adjustment) optimization algorithm based on foreground edge, and closed-loop detection algorithm based on random fern descriptor. These technologies show superiority in processing RGB-D images, and can improve the quality of feature points, enhance the matching ability of feature points, and improve the robustness and accuracy of the system. In addition, the present invention combines local and global optimization methods to obtain more accurate dense point cloud map construction results. Through the application of these innovative technologies, the present invention has achieved remarkable results in achieving accurate dense point cloud maps. Background Art

[0002] SLAM is an important interdisciplinary field of robotics and computer vision. It is widely used in fields such as autonomous driving, drone driving and robot navigation, and has attracted much research attention in recent years.

[0003] Kirsch edge detection is a template-based edge detection algorithm, which includes multiple steps such as color component extraction, image denoising, image enhancement, pixel intensity calculation, threshold calculation, edge division and edge fusion. For each pixel, the Kirsch operator performs convolution operation with the pixels around it to calculate the intensity value of the edge response. In Kirsch edge detection, high threshold and low threshold are usually used to determine the intensity level of edge pixels, which are divided into strong edges, weak edges and edge pixels. Edge connection and edge removal are usually performed after double threshold. Edge connection forms a continuous edge by connecting weak edge pixels with strong edge pixels. Edge removal distinguishes weak edges from noise edges to obtain the final edge image. The Kirsch edge detection algorithm with high and low double thresholds combines the Kirsch operator and threshold processing, which can effectively detect edge information in the image and filter out some noise.

[0004] BlendMask is a one-stage dense instance segmentation method that combines the ideas of Top-down and Bottom-up. It adds a Bottom Module to the anchor-free detection model FCOS to extract low-level detail features and predict an attention at the instance-level; it draws on the fusion method of FCIS and YOLACT and uses the Blender module to better fuse the two features. This has significantly improved BlendMask in terms of accuracy and speed.

[0005] The sliding window BA optimization algorithm based on foreground edge is an optimization method based on ant colony algorithm, which is used to solve binary optimization problems. The algorithm combines sliding window and foreground edge information to improve the efficiency and accuracy of optimization.

[0006] The candidate closed-loop keyframe detection technology based on random fern descriptors is used to find candidate closed-loop keyframes similar to the current keyframe scene, and the candidate closed-loop keyframe set is sorted in ascending order according to the Hamming distance; then, the candidate closed-loop keyframe confirmation steps based on edge features are performed in sequence to ensure the accuracy of the closed-loop keyframes. Summary of the invention

[0007] The map constructed by the traditional ORB-SLAM2 system is a sparse map, which cannot realize dense map construction. The present invention proposes a method for constructing dense maps of edge SLAM based on Kirsch combined with high and low dual thresholds, which combines the edge detection of Kirsch combined with high and low dual thresholds and the visual odometer of the BlendMASK deep neural network, the sliding window local BA optimization algorithm of the foreground edge and the closed-loop detection method of the random fern descriptor. The edge detection of Kirsch combined with high and low dual thresholds has clearer edges, more complete details, better edge coherence and higher detection rate than other algorithms such as Sobel. At the same time, BlendMASK instance segmentation is added to achieve improved mask prediction by effectively combining instance-level information and low-level fine-grained semantic information, providing more accurate feature point matching and thus improving performance.

[0008] Specific content of the invention:

[0009] A method for constructing a dense map of edge SLAM based on Kirsch combined high and low double thresholds, characterized by comprising the following steps:

[0010] Step 1) An improved visual odometer based on Kirsch combined high and low double threshold edge detection is used to estimate the relative motion between frames and realize pose estimation, including the following steps:

[0011] Step 1.1) Improve the initialization of the visual odometry: Take the first RGB-D image as the current frame, extract the Kirsch edge features of the color image from it, and select this frame as the key frame; then use the BlendMASK deep neural network to perform instance segmentation on the color image of the key frame and integrate the semantic information; remove the previously extracted Kirsch high dynamic edge points and edge points without depth information, and initialize the weights of the low dynamic edge points and static edge points;

[0012] Step 1.2) construct the current frame from the second RGB-D image, take its Kirsch edge, and construct the distance transform (DT) based on the binary map of the Kirsch edge to calculate the residual of the key frame-current frame 3D-2D registration; then use the key frame-current frame 3D-2D registration of the fused instance segmentation to estimate the pose of the current frame; define ξ ji Under the transformation, the edge p i From F i Reproject to F j The reprojection edge distance error E pi for:

[0013] E pi =D j (τ(ξ ji ,p i ,Z i (p i )))

[0014] Step 1.3) When a new RGB-D image frame arrives, use it to construct the current frame and calculate the reprojection residual (according to step 1.2). Before performing the fusion instance segmentation keyframe-current frame 3D-2D registration pose estimation with the current keyframe, use the relative motion T between the two frames closest to the current frame. p To estimate the camera motion T of the current frame relative to the previous frame i , the specific motion estimation model is as follows:

[0015]

[0016] Where I represents the unit matrix, and the non-high dynamic edge points in the current key frame are respectively converted into T according to the above motion model. i Reproject to the current frame, and get the motion estimate T with the minimum reprojection residual i To estimate the initial pose transformation from the current key frame to the current frame during the 3D-2D registration of the key frame-current frame of the fusion instance segmentation, and then use the LM algorithm to optimize the pose calculation of the current frame; the LM method sets a trust region to constrain the size of △x. The steps of this method to solve △x are as follows:

[0017]

[0018] Among them, μ is the radius of the trust region, D is a non-negative diagonal matrix, which can usually be taken as the unit matrix. The Lagrange multiplier method is used to transform the above problem into an unconstrained optimization problem as follows:

[0019]

[0020] After expansion, we get:

[0021] (J(x) T J(x)+λD T D)△x=-J(x) T f(x)

[0022] Let D = I, which is equivalent to solving

[0023] (J(x) T J(x)+λI)△x=-J(x) T f(x)

[0024] By giving an estimated value of the initial state vector x and iteratively updating △x until convergence, the minimum value of the nonlinear function can be obtained;

[0025] Step 1.4) In order to suppress the influence of outliers, the Huber kernel function is used to suppress the residual values ​​corresponding to edge points with excessively large reprojection residuals;

[0026] Step 1.5) After the pose estimation of the current frame is completed, we need to check whether the frame meets the conditions for becoming a key frame; if the current frame does not meet the conditions for a key frame, then execute step 1.3, that is, select the next frame for pose estimation and registration process; however, if the current frame meets the conditions for becoming a key frame, we will select it as a key frame and perform the corresponding BlendMASK instance segmentation step; this step helps to further improve the quality and accuracy of scene reconstruction; after completing instance segmentation, we set the selected frame as the current key frame and continue to repeat step 1.3;

[0027] Step 2) Locally optimize the pose based on the sliding window local BA optimization algorithm of the foreground edge, including the following steps:

[0028] Step 2.1) Based on the construction and optimization of the cost function of the foreground edge, consider using a sliding window K with N key frames in K. In the window K, the depth of all active edges and the camera pose are jointly refined. All key frames F in the sliding window are i The downsampled foreground edge is reprojected to the current keyframe F n On the basis of the above, a new constrained residual term is constructed, which contains the optimized camera intrinsic C and inverse depth ρ i :

[0029] r in =D n (p′ i (ξ i ,ξ j ,ρ i ,C))

[0030] Among them, D n Function represents the current key frame F n The residual value calculated by bilinear interpolation in the corresponding distance transform, function p′ i Calculate the key frame F i A foreground edge point p on i At the current key frame F n The reprojection point on i and ε n Represent the key frames F i and F n The poses of these poses have been calculated and initially estimated in the pose tracking stage; ρ i Represents the foreground edge point p i When constructing the constrained residual term, only consider the case when th>0 and r in When the depth is less than the depth threshold th, where th is the mean number of elements in the sequence subset composed of pixels with depth plus 0.25;

[0031] Step 2.2) Add the above constraints to the cost function to be optimized, otherwise the key frame F i The corresponding foreground edge point is not captured by the current key frame F n Observe that the cost function E of local BA optimization is k for:

[0032]

[0033] Among them, h function represents Huber kernel function, r ij represents the original constraint between key frames in the sliding window, r in represents the key frame F in the sliding window i With the current key frame F n The constraints between

[0034] Step 2.3) Use a Gauss-Newton optimization algorithm to minimize the equation H = J T WJ b=J T Wr, iteratively solves the increment △x of the state variable x including the key frame pose and the inverse depth of the edge point:

[0035] H△x=-b

[0036] Where H is the approximate Hessian matrix, W∈R n×n is a diagonal weight matrix that can obtain the optimal estimate of the state variable x;

[0037] Step 2.4) Perform keyframe marginalization and keyframe removal based on foreground edge. Since the size of K is limited, delete and marginalize a keyframe before adding a new keyframe. If the active point of a keyframe is less than θ vis , then marginalize it, but if no keyframe satisfies this constraint, keep the two latest keyframes and calculate a distance score s i To ensure they are evenly distributed in 3D space:

[0038]

[0039] Among them, d i,j F i With F j The Euclidean distance between

[0040] Step 3) A closed loop detection algorithm based on random fern descriptors and global pose graph optimization includes the following steps:

[0041] Step 3.1) First, perform closed-loop keyframe detection and confirmation based on random fern descriptors in a downsampled 40×30 RGB image. f = 500 random sampling positions are used to calculate the fern descriptor, and then a position graph optimization (POG) problem is established:

[0042]

[0043] Where T ij is the relative transformation between keyframes i and j, and For the corrected world pose, use the Ceres solver to optimize this pose graph problem and parameterize the rotation into quaternions;

[0044] Step 3.2) Use an edge-based method to quickly verify the validity of the closed loop. Reproject a set of verification frames V and calculate the reprojections that overlap with the edge detection. To avoid calculating the overlapping reprojections multiple times, generate a separate count map for each keyframe C. 0--N-1 :

[0045]

[0046] where ε i is a set of valid edge pixels, and ξ ci Convert to F curr ;

[0047] Step 3.3) Through each C i The final mapping C is calculated by summing the elements of F. i ∈V only contributes to the overlap of one edge, then a histogram H of size N+1 is generated to count the number of edge overlaps. If the weighted sum of overlapping edges is less than the number of non-overlapping edges, the candidate edge is positively verified:

[0048]

[0049] where w i is the weight factor;

[0050] Step 4) Dense point cloud map construction, including the following steps:

[0051] Step 4.1) Based on the camera projection model and the inverse projection function, when the depth value of the pixel point is known, the spatial coordinates in the camera coordinate system are calculated and then converted to the world coordinate system, which is recorded as T w , then the coordinates of point P in the world coordinate system are w Expressed in homogeneous coordinate system:

[0052] P w =T w P

[0053] Through the above formula, we can unify all spatial points into the world coordinate system to construct a dense point cloud map.

[0054] The present invention has the following advantages and beneficial effects:

[0055] The original RGB-D image is processed by combining Kirsch's edge detection with high and low double thresholds. The traditional edge detection method is not used. A discrete first-order differential operator is used to detect the edges in 8 directions, and then the gradient intensity is reduced to 4 directions. Finally, the edge intensity value is obtained by taking the maximum two-norm method to obtain the edge intensity matrix. The double threshold algorithm is used to divide all pixels in the edge intensity matrix into two categories: edge and background, which helps to enhance the system's robustness, feature extraction capabilities, and attitude estimation accuracy, thereby improving the performance of positioning and map construction;

[0056] The purpose of BlendMASK instance segmentation is to segment different objects or targets in an image into independent regions. Through instance segmentation, the outline and boundary information of the object can be extracted to help the visual odometry system more accurately estimate the camera's posture and motion, and better track and match the position and posture changes of objects between consecutive frames. Combined with the results of instance segmentation, more accurate depth information can be provided to the visual odometry system. By inferring the distance of an object based on its size and shape, the accuracy of depth estimation can be improved. This depth information can be used to generate dense map point clouds, providing more detailed and accurate environmental structures. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 This is the overall flow chart of edge SLAM dense mapping of the present invention;

[0058] Figure 2 This is a flow chart of the visual odometer based on Kirsch's edge detection combined with high and low double thresholds and BlendMASK instance segmentation. DETAILED DESCRIPTION

[0059] The advantages and purposes of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the description herein is only for explaining the present invention and is not intended to limit the present invention.

[0060] The edge SLAM dense map construction method based on Kirsch combined high and low double thresholds proposed in the present invention first combines instance segmentation and Kirsch combined high and low double threshold edge detection to obtain camera pose information, then performs local optimization through a sliding window BA algorithm based on foreground edges and a closed loop detection algorithm based on a random fern descriptor and global pose graph optimization for global optimization, and finally constructs a dense point cloud map according to all spatial points in the world coordinate system. Figure 1 This is the overall flow chart of edge SLAM dense mapping in the present invention, Figure 2 The present invention is based on Kirsch combined high and low double threshold edge detection and BlendMASK instance segmentation visual odometer flow chart, including the following steps:

[0061] A method for constructing a dense map of edge SLAM based on Kirsch combined high and low double thresholds, characterized by comprising the following steps:

[0062] Step 1) An improved visual odometer based on Kirsch combined high and low double threshold edge detection is used to estimate the relative motion between frames and realize pose estimation, including the following steps:

[0063] Step 1.1) Improve the initialization of the visual odometry: Take the first RGB-D image as the current frame, extract the Kirsch edge features of the color image from it, and select this frame as the key frame; then use the BlendMASK deep neural network to perform instance segmentation on the color image of the key frame and integrate the semantic information; remove the previously extracted Kirsch high dynamic edge points and edge points without depth information, and initialize the weights of the low dynamic edge points and static edge points;

[0064] Step 1.2) construct the current frame from the second RGB-D image, take its Kirsch edge, and construct the distance transform (DT) based on the binary map of the Kirsch edge to calculate the residual of the key frame-current frame 3D-2D registration; then use the key frame-current frame 3D-2D registration of the fused instance segmentation to estimate the pose of the current frame; define ξ ji Under the transformation, the edge p i From F i Reproject to F j The reprojection edge distance error E pi for:

[0065] E pi =D j (τ(ξ ji ,p i ,Z i (p i )))

[0066] Step 1.3) When a new RGB-D image frame arrives, use it to construct the current frame and calculate the reprojection residual (according to step 1.2). Before performing the fusion instance segmentation keyframe-current frame 3D-2D registration pose estimation with the current keyframe, use the relative motion T between the two frames closest to the current frame. p To estimate the camera motion T of the current frame relative to the previous frame i , the specific motion estimation model is as follows:

[0067]

[0068] Where I represents the unit matrix, and the non-high dynamic edge points in the current key frame are respectively converted into T according to the above motion model. i Reproject to the current frame, and get the motion estimate T with the minimum reprojection residual i To estimate the initial pose transformation from the current key frame to the current frame during the 3D-2D registration of the key frame-current frame of the fusion instance segmentation, and then use the LM algorithm to optimize the pose calculation of the current frame; the LM method sets a trust region to constrain the size of △x. The steps of this method to solve △x are as follows:

[0069]

[0070] Among them, μ is the radius of the trust region, D is a non-negative diagonal matrix, which can usually be taken as the unit matrix. The Lagrange multiplier method is used to transform the above problem into an unconstrained optimization problem as follows:

[0071]

[0072] After expansion, we get:

[0073] (J(x) T J(x)+λD T D)△x=-J(x) T f(x)

[0074] Let D = I, which is equivalent to solving

[0075] (J(x) T J(x)+λI)△x=-J(x) T f(x)

[0076] By giving an estimated value of the initial state vector x and iteratively updating △x until convergence, the minimum value of the nonlinear function can be obtained;

[0077] Step 1.4) In order to suppress the influence of outliers, the Huber kernel function is used to suppress the residual values ​​corresponding to edge points with excessively large reprojection residuals;

[0078] Step 1.5) After the pose estimation of the current frame is completed, we need to check whether the frame meets the conditions for becoming a key frame; if the current frame does not meet the conditions for a key frame, then execute step 1.3, that is, select the next frame for pose estimation and registration process; however, if the current frame meets the conditions for becoming a key frame, we will select it as a key frame and perform the corresponding BlendMASK instance segmentation step; this step helps to further improve the quality and accuracy of scene reconstruction; after completing instance segmentation, we set the selected frame as the current key frame and continue to repeat step 1.3;

[0079] Step 2) Locally optimize the pose based on the sliding window local BA optimization algorithm of the foreground edge, including the following steps:

[0080] Step 2.1) Based on the construction and optimization of the cost function of the foreground edge, consider using a sliding window K with N key frames in K. In the window K, the depth of all active edges and the camera pose are jointly refined. All key frames F in the sliding window are i The downsampled foreground edge is reprojected to the current keyframe F n On the basis of the above, a new constrained residual term is constructed, which contains the optimized camera intrinsic C and inverse depth ρ i :

[0081] r in =D n (p′ i (ξ i ,ξ j ,ρ i ,C))

[0082] Among them, D n Function represents the current key frame F n The residual value calculated by bilinear interpolation in the corresponding distance transform, function p′ i Calculate the key frame F i A foreground edge point p on i At the current key frame F n The reprojection point on i and ε n Represent the key frames F i and F n The poses of these poses have been calculated and initially estimated in the pose tracking stage; ρ i Represents the foreground edge point p i When constructing the constrained residual term, only consider the case when th>0 and r in When the depth is less than the depth threshold th, where th is the mean number of elements in the sequence subset composed of pixels with depth plus 0.25;

[0083] Step 2.2) Add the above constraints to the cost function to be optimized, otherwise the key frame F i The corresponding foreground edge point is not captured by the current key frame F n Observe that the cost function E of local BA optimization is k for:

[0084]

[0085] Among them, h function represents Huber kernel function, r ij represents the original constraint between key frames in the sliding window, r in represents the key frame F in the sliding window i With the current key frame F n The constraints between

[0086] Step 2.3) Use a Gauss-Newton optimization algorithm to minimize the equation H = J T WJ b=J T Wr, iteratively solves the increment △x of the state variable x including the key frame pose and the inverse depth of the edge point:

[0087] H△x=-b

[0088] Where H is the approximate Hessian matrix, W∈R n×n is a diagonal weight matrix that can obtain the optimal estimate of the state variable x;

[0089] Step 2.4) Perform keyframe marginalization and keyframe removal based on foreground edge. Since the size of K is limited, delete and marginalize a keyframe before adding a new keyframe. If the active point of a keyframe is less than θ vis , then marginalize it, but if no keyframe satisfies this constraint, keep the two latest keyframes and calculate a distance score s i To ensure they are evenly distributed in 3D space:

[0090]

[0091] Among them, d i,j F i With F j The Euclidean distance between

[0092] Step 3) A closed loop detection algorithm based on random fern descriptors and global pose graph optimization includes the following steps:

[0093] Step 3.1) First, perform closed-loop keyframe detection and confirmation based on random fern descriptors in a downsampled 40×30 RGB image. f = 500 random sampling positions are used to calculate the fern descriptor, and then a position graph optimization (POG) problem is established:

[0094]

[0095] Where T ij is the relative transformation between keyframes i and j, and For the corrected world pose, use the Ceres solver to optimize this pose graph problem and parameterize the rotation into quaternions;

[0096] Step 3.2) Use an edge-based method to quickly verify the validity of the closed loop. Reproject a set of verification frames V and calculate the reprojections that overlap with the edge detection. To avoid calculating the overlapping reprojections multiple times, generate a separate count map for each keyframe C. 0--N-1 :

[0097]

[0098] where ε i is a set of valid edge pixels, and ξ ci Convert to F curr ;

[0099] Step 3.3) Through each C i The final mapping C is calculated by summing the elements of F. i ∈V only contributes to the overlap of one edge, then a histogram H of size N+1 is generated to count the number of edge overlaps. If the weighted sum of overlapping edges is less than the number of non-overlapping edges, the candidate edge is positively verified:

[0100]

[0101] where w i is the weight factor;

[0102] Step 4) Dense point cloud map construction, including the following steps:

[0103] Step 4.1) Based on the camera projection model and the inverse projection function, when the depth value of the pixel point is known, the spatial coordinates in the camera coordinate system are calculated and then converted to the world coordinate system, which is recorded as T w , then the coordinates of point P in the world coordinate system are w Expressed in homogeneous coordinate system:

[0104] P w =T w P

[0105] Through the above formula, we can unify all spatial points into the world coordinate system to construct a dense point cloud map.

Claims

1. A method for constructing dense maps of edge SLAM based on Kirsch combined high and low double thresholds, characterized in that The following steps are involved: Step 1) An improved visual odometer based on Kirsch combined high and low double threshold edge detection is used to estimate the relative motion between frames and realize pose estimation, including the following steps: Step 1.1) Improve the initialization of the visual odometry: Take the first RGB-D image as the current frame, extract the Kirsch edge features of the color image from it, and select this frame as the key frame; then use the BlendMASK deep neural network to perform instance segmentation on the color image of the key frame and integrate the semantic information; remove the previously extracted Kirsch high dynamic edge points and edge points without depth information, and initialize the weights of the low dynamic edge points and static edge points; Step 1.2) construct the current frame from the second RGB-D image, take its Kirsch edge, and construct the distance transform (DT) based on the binary map of the Kirsch edge to calculate the residual of the key frame-current frame 3D-2D registration; then use the key frame-current frame 3D-2D registration of the fused instance segmentation to estimate the pose of the current frame; define ξ ji Under the transformation, the edge p i From F i Reproject to F j The reprojection edge distance error E pi for: E pi =D j (t(x) ji ,p i ,Z i (p i ))) Step 1.3) When a new RGB-D image frame arrives, use it to construct the current frame and calculate the reprojection residual (according to step 1.2). Before performing the fusion instance segmentation keyframe-current frame 3D-2D registration pose estimation with the current keyframe, use the relative motion T between the two frames closest to the current frame. p To estimate the camera motion T of the current frame relative to the previous frame i , the specific motion estimation model is as follows: Where I represents the unit matrix, and the non-high dynamic edge points in the current key frame are respectively converted into T according to the above motion model. i Reproject to the current frame, and get the motion estimate T with the minimum reprojection residual i To estimate the initial pose transformation from the current key frame to the current frame during the 3D-2D registration of the key frame-current frame of the fusion instance segmentation, and then use the LM algorithm to optimize the pose calculation of the current frame; the LM method sets a trust region to constrain the size of △x. The steps of this method to solve △x are as follows: Among them, μ is the radius of the trust region, D is a non-negative diagonal matrix, which can usually be taken as the unit matrix. The Lagrange multiplier method is used to transform the above problem into an unconstrained optimization problem as follows: After expansion, we get: (J(x) T J(x)+λD T D)△x=-J(x) T f(x) Let D = I, which is equivalent to solving (J(x) T J(x)+λI)△x=-J(x) T f(x) By giving an estimated value of the initial state vector x and iteratively updating △x until convergence, the minimum value of the nonlinear function can be obtained; Step 1.4) In order to suppress the influence of outliers, the Huber kernel function is used to suppress the residual values ​​corresponding to edge points with excessively large reprojection residuals; Step 1.5) After the pose estimation of the current frame is completed, we need to check whether the frame meets the conditions for becoming a key frame; if the current frame does not meet the conditions for a key frame, then execute step 1.3, that is, select the next frame for pose estimation and registration process; however, if the current frame meets the conditions for becoming a key frame, we will select it as a key frame and perform the corresponding BlendMASK instance segmentation step; this step helps to further improve the quality and accuracy of scene reconstruction; after completing instance segmentation, we set the selected frame as the current key frame and continue to repeat step 1.3; Step 2) Locally optimize the pose based on the sliding window local BA optimization algorithm of the foreground edge, including the following steps: Step 2.1) Based on the construction and optimization of the cost function of the foreground edge, consider using a sliding window K with N key frames in K. In the window K, the depth of all active edges and the camera pose are jointly refined. All key frames F in the sliding window are i The downsampled foreground edge is reprojected to the current keyframe F n On the basis of the above, a new constrained residual term is constructed, which contains the optimized camera intrinsic C and inverse depth ρ i : r in =D n (p′ i (x) i ,x j ,r i ,C)) Among them, D n Function represents the current key frame F n The residual value calculated by bilinear interpolation in the corresponding distance transform, function p′ i Calculate the key frame F i A foreground edge point p on i At the current key frame F n The reprojection point on i and ε n Represent the key frames F i and F n The poses of these poses have been calculated and initially estimated in the pose tracking stage; ρ i Represents the foreground edge point p i When constructing the constrained residual term, only consider the case when th>0 and r in When the depth is less than the depth threshold th, where th is the mean number of elements in the sequence subset composed of pixels with depth plus 0.25; Step 2.2) Add the above constraints to the cost function to be optimized, otherwise the key frame F i The corresponding foreground edge point is not captured by the current key frame F n Observe that the cost function E of local BA optimization is k for: Among them, h function represents Huber kernel function, r ij represents the original constraint between key frames in the sliding window, r in represents the key frame F in the sliding window i With the current key frame F n The constraints between Step 2.3) Use a Gauss-Newton optimization algorithm to minimize the equation H = J T WJ b=J T Wr, iteratively solves the increment △x of the state variable x including the key frame pose and the inverse depth of the edge point: H△x=-b Where H is the approximate Hessian matrix, W∈R n×n is a diagonal weight matrix that can obtain the optimal estimate of the state variable x; Step 2.4) Perform keyframe marginalization and keyframe removal based on foreground edge. Since the size of K is limited, delete and marginalize a keyframe before adding a new keyframe. If the active point of a keyframe is less than θ vis , then marginalize it, but if no keyframe satisfies this constraint, keep the two latest keyframes and calculate a distance score s i To ensure they are evenly distributed in 3D space: Among them, d i,j F i With F j The Euclidean distance between Step 3) A closed loop detection algorithm based on random fern descriptors and global pose graph optimization includes the following steps: Step 3.1) First, perform closed-loop keyframe detection and confirmation based on random fern descriptors in a downsampled 40×30 RGB image. f = 500 random sampling positions are used to calculate the fern descriptor, and then a position graph optimization (POG) problem is established: Where T ij is the relative transformation between keyframes i and j, and For the corrected world pose, use the Ceres solver to optimize this pose graph problem and parameterize the rotation into quaternions; Step 3.2) Use an edge-based method to quickly verify the validity of the closed loop. Reproject a set of verification frames V and calculate the reprojections that overlap with the edge detection. To avoid calculating the overlapping reprojections multiple times, generate a separate count map for each keyframe C. 0--N-1 : where ε i is a set of valid edge pixels, and ξ ci Convert to F curr ; Step 3.3) Through each C i The final mapping C is calculated by summing the elements of F. i ∈V only contributes to the overlap of one edge, then a histogram H of size N+1 is generated to count the number of edge overlaps. If the weighted sum of overlapping edges is less than the number of non-overlapping edges, the candidate edge is positively verified: where w i is the weight factor; Step 4) Dense point cloud map construction, including the following steps: Step 4.1) Based on the camera projection model and the inverse projection function, when the depth value of the pixel point is known, the spatial coordinates in the camera coordinate system are calculated and then converted to the world coordinate system, which is recorded as T w , then the coordinates of point P in the world coordinate system are w Expressed in homogeneous coordinate system: P w =T w P Through the above formula, we can unify all spatial points into the world coordinate system to construct a dense point cloud map.

Images