Slam loopback detection method based on vision and laser line feature fusion
The SLAM loop closure detection method, which integrates visual and laser line features, solves the problems of insufficient positioning accuracy and unreliable map construction for underground track locomotives. It achieves high accuracy and robustness in the mining environment and resolves the issues of unreliable positioning and map construction for underground track locomotives.
Patent Information
- Application Number
- CN202511345049.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-09-19
- Publication Date
- 2025-12-16
AI Technical Summary
Traditional underground track locomotive positioning methods suffer from insufficient positioning accuracy, unreliable map construction, high loop closure false detection rate, and poor scale consistency in the mining environment, making it difficult to meet the requirements of high-precision automatic navigation and long-term stable operation.
A SLAM loop closure detection method based on the fusion of visual and laser line features is adopted. This method extracts three-dimensional line features from LiDAR point cloud data and matches them with two-dimensional line features in visual images. Combined with geometric error minimization and semantic consistency verification, loop closure detection and pose optimization are performed.
It improves positioning accuracy in mining environments, solves the accuracy and robustness issues of SLAM systems in mining environments, achieves high accuracy and long-term stability of SLAM systems in mining environments, adapts to different working conditions, and reduces false detection rates.
Smart Images
Figure CN121147313A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of mining engineering technology, and in particular to a SLAM loop closure detection method based on the fusion of visual and laser line features. Background Technology
[0002] With the development of intelligent mining, underground railcars, as core equipment for mine transportation, directly impact mine production efficiency and safety through their automation and intelligence levels. Traditional positioning methods for underground railcars primarily rely on manual driving and fixed beacons. However, due to the complex mine environment, insufficient lighting, severe dust interference, and signal multipath effects, traditional methods suffer from the following technical drawbacks:
[0003] Insufficient positioning accuracy:
[0004] Obstruction issues: In the confined space of underground mines, the presence of numerous equipment, rock walls, and supporting structures near the tracks easily leads to signal loss or interference with traditional positioning technologies (such as those based on beacons or single sensors). Insufficient lighting: Some sections of the mine have extremely poor lighting conditions, causing traditional visual positioning methods to experience significant performance degradation or even complete failure in low-light environments. Dust interference: Mining operations generate a large amount of dust, severely interfering with the performance of sensors such as lidar and cameras, reducing data acquisition quality. Signal multipath effect: The complex electromagnetic environment in mines easily leads to the reflection and interference of wireless signals, further reducing the reliability of traditional positioning systems. These problems result in insufficient positioning accuracy for underground track-mounted locomotives, making it difficult to meet the requirements for high-precision automatic navigation and stable operation.
[0005] Map building is unreliable:
[0006] Existing technologies are limited in their performance under low-texture environments, such as mines. **Location Drift:** In low-texture environments (e.g., smooth rock walls or monochromatic areas), traditional mapping and localization methods struggle to extract effective feature points, leading to increased cumulative errors and ultimately, location drift. **Poor Long-Term Stability:** Traditional mapping and localization algorithms are easily affected by the complexity of the mine environment during long-term operation, resulting in uncontrollable cumulative errors and failing to meet the requirements of long-term operations.
[0007] While existing SLAM technology can partially alleviate the above problems, it still has shortcomings such as high loop closure false detection rate and poor scale consistency in degraded mine environments, and a more robust solution is urgently needed. Summary of the Invention
[0008] This invention aims to address the shortcomings of existing technologies by providing a SLAM loop closure detection method based on the fusion of visual and laser line features.
[0009] To achieve the above objectives, this invention employs the following technical solution: a SLAM loop closure detection method based on the fusion of visual and laser line features, comprising the following steps:
[0010] S1. Extract 3D line features from lidar point cloud data, including calculating the covariance matrix of the local neighborhood, eigenvalue decomposition, constructing linearity index and screening line feature points;
[0011] S2. Extract two-dimensional line features from visual images and construct an image line feature dictionary;
[0012] S3. Project the three-dimensional laser line features extracted in S1 onto the image and match them with the line segments in the image line feature dictionary based on angle difference and Hausdorff distance to generate fused line features.
[0013] S4. In the loop closure detection stage, the loop closure between the current frame and the historical keyframes is determined by minimizing geometric error and verifying semantic consistency; at the same time, the fusion line features are added to the pose graph in real time to optimize the current pose.
[0014] Specifically, the steps in S1 are as follows:
[0015] S11. Extracting a local window: For each point p in the LiDAR point cloud data... ij Find the k nearest neighbors to form a neighborhood N. i :N i ={p i1 ,p i2 ,...,p ik};
[0016] S12. Calculate the covariance matrix: Calculate the mean of the neighborhood points, i.e., the centroid.
[0017] The covariance matrix is then:
[0018] S13. Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix C to obtain eigenvalues λ1, λ2, λ3 and corresponding eigenvectors e1, e2, e3, where λ1≥λ2≥λ3;
[0019] S14. Determine if it is a linear feature: If λ1≥λ2≈λ3, it is determined to be a linear feature;
[0020] Construct a linearity metric:
[0021] If L→1, it indicates that the linear feature is obvious; if L→0, it indicates that the feature is planar or point-like.
[0022] S15, Filter line feature points: Set the threshold to L minOnly retain those satisfying L > L min The points are used as feature points of the line.
[0023] Specifically, the steps in S2 are as follows:
[0024] S21. Image preprocessing: Convert the image to grayscale and perform smoothing and noise reduction processing;
[0025] S22. Edge Detection: Use the Canny edge detection algorithm to detect edges in the image;
[0026] S23. Hough Transform: Extracting straight lines using polar coordinates;
[0027] S24. Endpoint Extraction: Extract the endpoints of line segments based on the intersection points of the line and the image boundary;
[0028] S25. Post-processing: Filtering and merging similar line segments;
[0029] Finally, the line segment features on the image are obtained. Constructing an image line feature dictionary Each line segment is represented as: Among them, (u j1 v j1 ) and (u j2 v j2 ) are the coordinates of the two endpoints of the line segment, u j1 and u j2 It is the horizontal pixel coordinate of the endpoint on the image, v j1 and v j2 It is the horizontal pixel coordinate of the endpoint on the image.
[0030] Specifically, the steps in S3 are as follows:
[0031] S31. Obtain the extrinsic parameter matrix T through joint calibration of lidar and camera. lidar→cam ∈SE(3), the intrinsic parameter matrix K∈R is obtained through separate camera calibration. 3×3 ;where SE(3) represents the rigid body transformation group in three-dimensional space, including rotation and translation transformations; R 3×3 Represents a 3×3 matrix in three-dimensional space;
[0032] S32. Perform coordinate transformation on the two endpoints of the lidar line segment:
[0033] Transform the endpoints of the laser line segment from the radar coordinate system to the camera coordinate system: p cam =R*p lidar +t;
[0034] Where, p camThis represents the coordinate representation of points in the radar point cloud transformed into the image coordinate system, where R represents the rotation matrix from the radar coordinate system to the camera coordinate system, and p... lidar The coordinates of a point in the radar point cloud are represented in the radar coordinate system, and t represents the translation matrix from the radar coordinate system to the camera coordinate system.
[0035] Project the endpoints of the laser line segment in the camera coordinate system onto the image coordinate system through the pinhole model:
[0036] Obtain the corresponding projected line segment:
[0037] Where u represents the horizontal pixel coordinate on the image, v represents the vertical pixel coordinate on the image, K is the camera intrinsic parameter matrix, and Z is the vertical distance from a point in the radar point cloud to the camera imaging plane, i.e., the depth value.
[0038] S33. Calculate the angular difference θ between the projected line segment and the image line segment, and the Hausdorff distance d. H :
[0039] Calculate the angle difference: Where, v = p end -p start ;
[0040] Calculate distance overlap:
[0041] Where S1 and S2 are the sets of endpoints d of the two line segments respectively. H (S1,S2), where p1 and p2 represent the coordinates of the endpoints;
[0042] S34. Using the comprehensive similarity scoring function S=α*θ+β*d H Determine the optimal match, generate fusion line features, and use α and β as weight coefficients, which are adjusted according to different radars, cameras, and scenes; these are empirical values.
[0043] S35. Assign the three-dimensional information of the laser line features to the successfully matched image line segment features to obtain a set of matched laser line features and image line features. At the same time, use the trained YOLO network to add semantic labels to these line features.
[0044] Specifically, the steps in S4 are as follows:
[0045] S41. During loop closure detection, add matching of line features between the current frame and the loop closure history frames. That is, if the current frame is frame n and the loop closure history frame is frame 0', then the set of line features for each frame is: The set of matching line pairs is: Where, l=(p s ,p e), p s and p e These represent the starting and ending points of the line segment, respectively, and 'l' represents a line feature.
[0046] S42, The transformation model between two frames is: T cam = {R, t}∈SE(3), where R∈SO(3) is a rotation matrix, and t∈R 3 It is a translation vector, and SO(3) represents the special orthogonal group formed by all three-dimensional rotation matrices;
[0047] S43. In three-dimensional space, transform the two endpoints of a line segment: Transform the characteristics of the line segment: l n′ =T cam *l n ;
[0048] S44. Minimize the geometric error of the matching line segment using the formula Fitting and calculating endpoint distance error:
[0049] S45. Based on the above calculations, determine the loop closure:
[0050] Based on the fitting residuals of the matched line segments and the number of matches, the mean residual is defined as follows: Define a threshold N for the number of matches. min Residual threshold γ max ;
[0051] The condition for determining whether a loop occurs is:
[0052] After the geometric verification is passed, semantic verification is performed: semantic consistency verification is performed on the line features that passed the geometric verification; when the number of features that successfully match semantically is greater than the minimum threshold for semantic matching, it is determined to be a valid loop closure; after the loop closure is confirmed, the pose graph is optimized by minimizing the graph optimization error function, and the pose and map point information of the associated keyframes are updated.
[0053] The beneficial effects of this invention are:
[0054] Improve positioning accuracy: By fusing LiDAR and visual camera, the limitations of a single sensor in occluded, low-light, or dusty environments are overcome; by utilizing the geometric stability of LiDAR line features and the semantic information of visual line features, effective features can still be extracted in low-texture environments, reducing positioning drift.
[0055] Optimize map building reliability: Integrate fused line features into the pose graph in real time to constrain the pose of the current frame and the local map, and solve short-term cumulative errors; Through geometric and semantic dual verification after loop closure detection, the false detection rate is significantly reduced and long-term map drift is corrected; Line features are not sensitive to changes in lighting and dust interference, and have higher stability in the mining environment compared with traditional point features.
[0056] Enhance system adaptability: Adaptively balance the contributions of laser and visual line features through a comprehensive similarity scoring function to adapt to different working conditions; introduce YOLO network to perform semantic annotation of line features to further eliminate false matches and improve the reliability of loop closure detection.
[0057] This invention significantly improves the accuracy, robustness, and long-term stability of SLAM systems in mining environments through line feature fusion and hierarchical optimization mechanisms, providing reliable technical support for unmanned transportation in mines. Attached Figure Description
[0058] Figure 1 This is a flowchart of the present invention;
[0059] The following will describe in detail, with reference to the accompanying drawings, embodiments of the present invention. Detailed Implementation
[0060] The present invention will be further described below with reference to embodiments:
[0061] like Figure 1 As shown, a SLAM loop closure detection method based on the fusion of visual and laser line features includes the following steps:
[0062] S1. Extract 3D line features from LiDAR point cloud data, including calculating the covariance matrix of the local neighborhood, eigenvalue decomposition, constructing linearity indices, and screening line feature points; the specific steps are as follows:
[0063] S11. Extracting a local window: For each point p in the LiDAR point cloud data... ij Find the k nearest neighbors to form a neighborhood N. i :N i ={p i1 ,p i2 ,...,p ik};
[0064] S12. Calculate the covariance matrix: Calculate the mean of the neighborhood points, i.e., the centroid.
[0065] The covariance matrix is then:
[0066] S13. Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix C to obtain eigenvalues λ1, λ2, λ3 and corresponding eigenvectors e1, e2, e3, where λ1≥λ2≥λ3;
[0067] S14. Determine if it is a linear feature: If λ1≥λ2≈λ3, it is determined to be a linear feature; (If λ1≥λ2≈λ3, it means that the point changes significantly in one dimension (the direction pointed to by the feature vector e1), while the changes in the other two directions are very small, so its neighborhood is considered to present a "linear structure");
[0068] Construct a linearity metric:
[0069] If L→1, it indicates that the linear feature is obvious; if L→0, it indicates that the feature is planar or point-like.
[0070] S15, Filter line feature points: Set the threshold to L min Only retain those satisfying L > L min The points are used as feature points of the line.
[0071] By extracting 3D line features from LiDAR point cloud data, the limitations of traditional single sensors in complex environments, such as occlusion, low light, or dust, can be overcome. Due to their geometric stability, LiDAR line features can still be effectively extracted in low-texture environments, reducing positioning drift.
[0072] S2. Extract two-dimensional line features from the visual image and construct an image line feature dictionary; the specific steps are as follows:
[0073] S21. Image preprocessing: Convert the image to grayscale and perform smoothing and noise reduction processing;
[0074] S22. Edge Detection: Use the Canny edge detection algorithm to detect edges in the image;
[0075] S23. Hough Transform: Extracting straight lines using polar coordinates;
[0076] S24. Endpoint Extraction: Extract the endpoints of line segments based on the intersection points of the line and the image boundary;
[0077] S25. Post-processing: Filtering and merging similar line segments;
[0078] Finally, the line segment features on the image are obtained. Constructing an image line feature dictionary Each line segment is represented as: Among them, (u j1 v j1 ) and (u j2 v j2 ) are the coordinates of the two endpoints of the line segment, uj1 and u j2 It is the horizontal pixel coordinate of the endpoint on the image, v j1 and v j2 It is the horizontal pixel coordinate of the endpoint on the image.
[0079] Visual line features provide rich semantic information, which helps in more accurate localization and map building in complex environments. Through visual features, the system can better adapt to changes in lighting and dust interference, improving its stability and reliability in mining environments.
[0080] S3. Project the 3D laser line features extracted in S1 onto the image, and match them with line segments in the image line feature dictionary based on angle difference and Hausdorff distance to generate fused line features; the specific steps are as follows:
[0081] S31. Obtain the extrinsic parameter matrix T through joint calibration of lidar and camera. lidar→cam ∈SE(3), the intrinsic parameter matrix K∈R is obtained through separate camera calibration. 3×3 ;where SE(3) represents the rigid body transformation group in three-dimensional space, including rotation and translation transformations; R 3×3 Represents a 3×3 matrix in three-dimensional space;
[0082] S32. Perform coordinate transformation on the two endpoints of the lidar line segment:
[0083] Transform the endpoints of the laser line segment from the radar coordinate system to the camera coordinate system: p cam =R*p lidar +t;
[0084] Where, p cam This represents the coordinate representation of points in the radar point cloud transformed into the image coordinate system, where R represents the rotation matrix from the radar coordinate system to the camera coordinate system, and p... lidar The coordinates of a point in the radar point cloud are represented in the radar coordinate system, and t represents the translation matrix from the radar coordinate system to the camera coordinate system.
[0085] Project the endpoints of the laser line segment in the camera coordinate system onto the image coordinate system through the pinhole model:
[0086] Obtain the corresponding projected line segment:
[0087] Where u represents the horizontal pixel coordinate on the image, v represents the vertical pixel coordinate on the image, K is the camera intrinsic parameter matrix, and Z is the vertical distance from a point in the radar point cloud to the camera imaging plane, i.e., the depth value.
[0088] S33. Calculate the angular difference θ between the projected line segment and the image line segment, and the Hausdorff distance d.H :
[0089] Perform similarity matching between the projected line segments and the image line segment dictionary, and perform similarity matching on each projected line segment. Each line l in the image line segment dictionary j ∈D img Perform matching. Use both angle difference and distance overlap as measures of similarity.
[0090] Calculate the angle difference: Where, v = p end -p start ;
[0091] Calculate distance overlap: The Hausdorff distance represents the maximum distance between the farthest pairs of endpoints of two line segments;
[0092] Where S1 and S2 are the sets of endpoints d of the two line segments respectively. H (S1,S2), where p1 and p2 represent the coordinates of the endpoints;
[0093] S34. Using the comprehensive similarity scoring function S=α*θ+β*d H The optimal match is determined, and the fusion line features are generated. α and β are used as weight coefficients, which are adjusted according to different radars, cameras, and scenes. These are empirical values, and are dynamically adjusted to the optimal value during application.
[0094] In summary, a smaller S value means that the two line segments have little difference in direction and a high degree of overlap, indicating a good match, while a larger S value indicates a poor match.
[0095] S35. Assign the three-dimensional information of the laser line features to the successfully matched image line segment features to obtain a set of matched laser line features and image line features. At the same time, use the trained YOLO network to add semantic labels to these line features.
[0096] By matching 3D laser line features with 2D image line features, fused line features are generated, which helps improve the robustness and accuracy of the features. These fused line features combine the advantages of laser and vision, enabling more effective loop closure detection and reducing false positive rates.
[0097] S4. In the loop closure detection stage, the loop closure between the current frame and historical keyframes is determined by minimizing geometric error and verifying semantic consistency; simultaneously, the fused line features are added to the pose graph in real time to optimize the current pose; the specific steps are as follows:
[0098] S41. During loop closure detection, add matching of line features between the current frame and the loop closure history frames. That is, if the current frame is frame n and the loop closure history frame is frame n', then the set of line features for each frame is: The set of matching line pairs is: Where, l=(p s ,p e ), p s and p e These represent the starting and ending points of the line segment, respectively, and 'l' represents a line feature.
[0099] S42, The transformation model between two frames is: T cam = {R, t}∈SE(3), where R∈SO(3) is a rotation matrix, and t∈R 3 It is a translation vector, and SO(3) represents the special orthogonal group formed by all three-dimensional rotation matrices;
[0100] S43. In three-dimensional space, transform the two endpoints of a line segment: Transform the characteristics of the line segment: l n′ =T cam *l n ;
[0101] S44. Minimize the geometric error of the matching line segment using the formula Fitting and calculating endpoint distance error:
[0102] S45. Based on the above calculations, determine the loop closure:
[0103] Based on the fitting residuals of the matched line segments and the number of matches, the mean residual is defined as follows: Define a threshold N for the number of matches. min Residual threshold γ max ;
[0104] The condition for determining whether a loop occurs is:
[0105] After the geometric verification is passed, semantic verification is performed: semantic consistency verification is performed on the line features that passed the geometric verification; when the number of features that successfully match semantically is greater than the minimum threshold for semantic matching, it is determined to be a valid loop closure; after the loop closure is confirmed, the pose graph is optimized by minimizing the graph optimization error function, and the pose and map point information of the associated keyframes are updated.
[0106] By minimizing geometric errors and verifying semantic consistency, loop closures can be determined more accurately, thereby improving the accuracy and robustness of the SLAM system. By incorporating fused line features into the pose map in real time, the pose of the current frame and the local map can be constrained, addressing short-term accumulated errors, significantly reducing false detection rates, and correcting long-term mapping drift.
[0107] These steps work together to significantly improve the accuracy, robustness, and long-term stability of SLAM systems in mining environments, providing reliable technical support for unmanned transportation in mines.
[0108] The present invention has been described above by way of example. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any improvements made by adopting the inventive concept and technical solution of the present invention, or direct application to other occasions without modification, are all within the protection scope of the present invention.
Claims
1. A SLAM loop closure detection method based on the fusion of visual and laser line features, characterized in that, Includes the following steps: S1. Extract 3D line features from lidar point cloud data, including calculating the covariance matrix of the local neighborhood, eigenvalue decomposition, constructing linearity index and screening line feature points; S2. Extract two-dimensional line features from visual images and construct an image line feature dictionary; S3. Project the three-dimensional laser line features extracted in S1 onto the image and match them with the line segments in the image line feature dictionary based on angle difference and Hausdorff distance to generate fused line features. S4. In the loop closure detection stage, the loop closure between the current frame and the historical keyframes is determined by minimizing geometric error and verifying semantic consistency; at the same time, the fusion line features are added to the pose graph in real time to optimize the current pose.
2. The SLAM loop closure detection method based on visual and laser line feature fusion according to claim 1, characterized in that, The specific steps in S1 are as follows: S11. Extracting a local window: For each point p in the LiDAR point cloud data... ij Find the k nearest neighbors to form a neighborhood N. i :N i ={p i1 ,p i2 ,...,p ik }; S12. Calculate the covariance matrix: Calculate the mean of the neighborhood points, i.e., the centroid. The covariance matrix is then: S13. Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix C to obtain eigenvalues λ1, λ2, λ3 and corresponding eigenvectors e1, e2, e3, where λ1≥λ2≥λ3; S14. Determine if it is a linear feature: If λ1≥λ2≈λ3, it is determined to be a linear feature; Construct a linearity metric: If L→1, it indicates that the linear feature is obvious; if L→0, it indicates that the feature is planar or point-like. S15, Filter line feature points: Set the threshold to L min Only retain those satisfying L > L min The points are used as feature points of the line.
3. The SLAM loop closure detection method based on visual and laser line feature fusion according to claim 1, characterized in that, The specific steps in S2 are as follows: S21. Image preprocessing: Convert the image to grayscale and perform smoothing and noise reduction processing; S22. Edge Detection: Use the Canny edge detection algorithm to detect edges in the image; S23. Hough Transform: Extracting straight lines using polar coordinates; S24. Endpoint Extraction: Extract the endpoints of line segments based on the intersection points of the line and the image boundary; S25. Post-processing: Filtering and merging similar line segments; Finally, the line segment features on the image are obtained. Constructing an image line feature dictionary Each line segment is represented as: Among them, (u j1 v j1 ) and (u j2 v j2 ) are the coordinates of the two endpoints of the line segment, u j1 and u j2 It is the horizontal pixel coordinate of the endpoint on the image, v j1 and v j2 It is the horizontal pixel coordinate of the endpoint on the image.
4. The SLAM loop closure detection method based on visual and laser line feature fusion according to claim 3, characterized in that, The specific steps in S3 are as follows: S31. Obtain the extrinsic parameter matrix T through joint calibration of lidar and camera. lidar→cam ∈SE(3), the intrinsic parameter matrix K∈R is obtained through separate camera calibration. 3×3 ;where SE(3) represents the rigid body transformation group in three-dimensional space, including rotation and translation transformations; R 3 ×3 Represents a 3×3 matrix in three-dimensional space; S32. Perform coordinate transformation on the two endpoints of the lidar line segment: Transform the endpoints of the laser line segment from the radar coordinate system to the camera coordinate system: p cam =R*p lidar +t; Where, p cam This represents the coordinate representation of points in the radar point cloud transformed into the image coordinate system, where R represents the rotation matrix from the radar coordinate system to the camera coordinate system, and p... lidar The coordinates of a point in the radar point cloud are represented in the radar coordinate system, and t represents the translation matrix from the radar coordinate system to the camera coordinate system. Project the endpoints of the laser line segment in the camera coordinate system onto the image coordinate system through the pinhole model: Obtain the corresponding projected line segment: Where u represents the horizontal pixel coordinate on the image, v represents the vertical pixel coordinate on the image, K is the camera intrinsic parameter matrix, and Z is the vertical distance from a point in the radar point cloud to the camera imaging plane, i.e., the depth value. S33. Calculate the angular difference θ between the projected line segment and the image line segment, and the Hausdorff distance d. H : Calculate the angle difference: Where, v = p end -p start ; Calculate distance overlap: Where S1 and S2 are the sets of endpoints d of the two line segments respectively. H (S1,S2), where p1 and p2 represent the coordinates of the endpoints; S34. Using the comprehensive similarity scoring function S=α*θ+β*d H Determine the optimal match, generate fusion line features, and use α and β as weight coefficients, which are adjusted according to different radars, cameras, and scenes; these are empirical values. S35. Assign the three-dimensional information of the laser line features to the successfully matched image line segment features to obtain a set of matched laser line features and image line features. At the same time, use the trained YOLO network to add semantic labels to these line features.
5. The SLAM loop closure detection method based on visual and laser line feature fusion according to claim 4, characterized in that, The specific steps in S4 are as follows: S41. During loop closure detection, add matching of line features between the current frame and the loop closure history frames. That is, if the current frame is frame n and the loop closure history frame is frame n', then the set of line features for each frame is: The set of matching line pairs is: Where, l=(p s ,p e ), p s and p e These represent the starting and ending points of the line segment, respectively, and 'l' represents a line feature. S42, The transformation model between two frames is: T cam = {R, t}∈SE(3), where R∈SO(3) is a rotation matrix, and t∈R 3 It is a translation vector, and SO(3) represents the special orthogonal group formed by all three-dimensional rotation matrices; S43. In three-dimensional space, transform the two endpoints of a line segment: Transform the characteristics of the line segment: l n′ =T cam *l n ; S44. Minimize the geometric error of the matching line segment using the formula Fitting and calculating endpoint distance error: S45. Based on the above calculations, determine the loop closure: Based on the fitting residuals of the matched line segments and the number of matches, the mean residual is defined as follows: Define a threshold N for the number of matches. min Residual threshold γ max ; The condition for determining whether a loop occurs is: After the geometric verification is passed, semantic verification is performed: semantic consistency verification is performed on the line features that passed the geometric verification; when the number of features that successfully match semantically is greater than the minimum threshold for semantic matching, it is determined to be a valid loop closure; after the loop closure is confirmed, the pose graph is optimized by minimizing the graph optimization error function, and the pose and map point information of the associated keyframes are updated.