A visual slam method fusing point features and coplanar anchors
By using a matching method of coplanar anchor point features and planar geometric constraints, the problem of tracking failure in low-texture environments of traditional visual SLAM is solved, achieving high-precision and robust localization and mapping, reducing computational complexity, and ensuring the real-time performance and accuracy of the method.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- YICHUN VOCATIONAL TECH COLLEGE
- Filing Date
- 2026-04-30
- Publication Date
- 2026-07-24
AI Technical Summary
Traditional visual SLAM systems fail to track due to sparse point features in low-texture environments, resulting in poor robustness. Existing methods that introduce line or surface features have high computational complexity, making it difficult to balance accuracy and efficiency in practical systems.
We employ coplanar anchor point features and perform fast and robust matching based on planar geometric constraints. We combine point features and coplanar anchor point features for localization and mapping, and use two intersecting line segments to determine the plane for feature matching, thereby reducing computational complexity.
It provides strong geometric constraints in low-texture, structured environments, improves positioning accuracy and robustness, reduces computational complexity, avoids ambiguity issues in line feature matching, and ensures the real-time performance and accuracy of the method.
Smart Images
Figure CN122454086A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of computer vision technology, and more specifically to a visual SLAM method that fuses point features and coplanar anchor points. Background Technology
[0002] Visual SLAM is a core technology for achieving autonomous localization and mapping in fields such as robotics and augmented reality. Traditional visual SLAM systems, such as ORB-SLAM2, mainly rely on point features (such as corners and spots) in images for tracking and localization. However, in low-texture environments (such as white walls and empty corridors), image point features are sparse or even missing, causing the system to easily fail in tracking and exhibit poor robustness.
[0003] To overcome the shortcomings of point features, existing technologies attempt to introduce line or surface features. Line features (such as PL-SLAM) utilize object edges, but in real-world images, line segments are prone to breakage, endpoint localization is inaccurate, and matching ambiguity is significant. While planar features offer stable geometric constraints, the number of planes in the environment is limited, and extracting planes requires high computational complexity and suffers from poor real-time performance. Furthermore, methods that fuse multiple features such as points, lines, and surfaces (such as Point-Plane SLAM) improve accuracy but introduce higher computational complexity and framework design difficulties, making it challenging to balance accuracy and efficiency in practical systems.
[0004] Therefore, how to design a feature and SLAM framework that can provide strong geometric constraints without excessively increasing the computational burden in low-texture, structured environments has become a technical problem that urgently needs to be solved in this field. Summary of the Invention
[0005] To address the shortcomings of existing technologies, this invention utilizes coplanar anchor point features and planar geometric constraints to achieve fast and robust matching, thereby enabling high-precision and robust positioning and mapping of low-texture, structured environments.
[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution: In a first aspect, the present invention provides a visual SLAM method for fusing point features and coplanar anchor points, the method comprising: Step S1: Extract point features and line segment features in parallel for the current frame image; Step S2: Process the line segment features extracted from the current frame image, filter and combine them to generate coplanar anchor point features; wherein, the coplanar anchor point features are composed of two intersecting line segments in the image that are coplanar in three-dimensional space. Step S3: Match the point features and coplanar anchor point features extracted from the current frame image with the features in the established visual map to obtain a set of point feature matching pairs and a set of coplanar anchor point feature matching pairs. Step S4: Based on the set of point feature matching pairs and the set of coplanar anchor point feature matching pairs, construct a fusion reprojection error model to optimize the camera pose of the current frame image and obtain the optimized camera pose. Step S5: Update the visual map based on the optimized camera pose, and determine whether to add the current frame image as a new keyframe.
[0007] Furthermore, an initialization step is included before step S1 to establish the established visual map. The initialization step specifically includes: Acquire the first frame of the RGB-D image; For the first frame of RGB-D image, point features and line segment features are extracted in parallel, and coplanar anchor point features are generated based on the line segment features; Based on the depth information of the first frame RGB-D image, the extracted point features and coplanar anchor point features are back-projected into three-dimensional space to construct an initial visual map, and the first frame RGB-D image is set as the first keyframe. The initial visual map serves as the established visual map.
[0008] Further, the processing of the line segment features extracted from the current frame image in step S2, and the filtering and combination to generate coplanar anchor point features, specifically includes: Step S21: Use the LSD algorithm to extract the original line segment set from the current frame image. By calculating the endpoint spacing, direction angle and projection overlap of adjacent line segments, merge broken line segments that meet the conditions of proximity, direction consistency and collinearity, and filter out line segments that are too short to obtain the optimized line segment set. Step S22: Select a line segment from the optimized line segment set as a reference line segment; Step S23: Establish a rectangular spatial influence area centered on the reference line segment; Step S24: From the optimized line segment set, find candidate line segments that satisfy all of the following conditions to combine with the reference line segment to form a coplanar anchor point feature: At least one endpoint of the candidate line segment is located within the spatial influence area; The intersection of the candidate line segment and the reference line segment, or the midpoint of the line connecting the shortest distance between the candidate line segment and the reference line segment, is located within the spatial influence area; The candidate line segment and the reference line segment satisfy the coplanar constraint in three-dimensional space.
[0009] Further, in step S24, the midpoint of the line connecting the candidate line segment and the reference line segment with the shortest distance; the method for calculating the shortest distance is as follows: Let the two endpoints of the candidate line segment be the first endpoints of the candidate line segment. The second endpoint of the candidate line segment The direction vector of the candidate line segment is ; Let the two endpoints of the reference line segment be the first endpoints of the reference line segment. The second endpoint of the reference line segment The direction vector of the reference line segment is ; The points on the candidate line segment are represented as follows: , ; The points on the reference line segment are represented as follows: , ; By solving constrained optimization problems The constraints are as follows: , , obtain the first parameter Second parameter ; First parameter Second parameter All in Within the interval, the shortest distance is... and The distance between them; First parameter Or the second parameter Not here Within the interval, calculate the first endpoint of each candidate line segment. The shortest distance to the reference line segment, the second endpoint of the candidate line segment The shortest distance to the reference line segment, the first endpoint of the reference line segment The shortest distance to the candidate line segment and the second endpoint of the reference line segment The shortest distance to the candidate line segment is determined by taking the minimum of the four shortest distances mentioned above as the shortest distance between the candidate line segment and the reference line segment.
[0010] Further, in step S3, the point features and coplanar anchor point features extracted from the current frame image are matched with the features in the established visual map, respectively; wherein, the matching of coplanar anchor point features with the features in the established visual map involves the following steps: For a coplanar anchor point feature, a spatial plane is determined using the two line segments of the coplanar anchor point feature; The planar parameters of the spatial plane in the world coordinate system are expressed as follows: ,in, Let be the normal vector of the space plane. This is the planar distance from the origin to the spatial plane; The planar parameters of the spatial plane in the world coordinate system determined by the coplanar anchor point features extracted from the current frame image are compared with the planar parameters of the spatial plane in the world coordinate system determined by the coplanar anchor point features extracted from the established visual map. If the angle between their normal vectors is less than a threshold and the difference in their planar distance is less than a threshold, then the coplanar anchor point features extracted from the current frame image are determined to match the coplanar anchor point features extracted from the established visual map.
[0011] Furthermore, the fusion reprojection error model described in step S4 is a weighted sum of the reprojection error of point features and the reprojection error of coplanar anchor point features; wherein, the reprojection error of coplanar anchor point features is calculated as follows: ; in, For the reprojection error of coplanar anchor point features, , These are the projection errors of the first and second endpoints of the first line segment, respectively. , These are the projection errors of the first and second endpoints of the second line segment, respectively. This represents the projection error at the intersection of the two line segments; The reprojection error of the coplanar anchor point features The sum of errors between the projected positions and measured positions of the four endpoints and the intersection of the two line segments, which are coplanar anchor points, on the current frame image.
[0012] Furthermore, the overall objective function of the fused reprojection error model is as follows: ; in, For the optimized camera pose, The camera pose of the current frame image. The set of keyframes corresponding to the current frame image. For the set of point feature matching pairs, For robust kernel functions, For the first In the first keyframe Transpose of the reprojection error of a point feature The information matrix of point features, For the first In the first keyframe Reprojection error of individual feature points For the set of coplanar anchor point feature matching pairs, The weighting coefficients for the coplanar anchor point features. The transpose of the reprojection error of the coplanar anchor point features. The information matrix of coplanar anchor point features. For the first In the first keyframe The reprojection error of the features of each coplanar anchor point.
[0013] Furthermore, in step S4, the optimization solution for the camera pose of the current frame image is obtained by using the Levonburg-Marquardt LM algorithm.
[0014] In a second aspect, the present invention provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the aforementioned visual SLAM method for fusing point features and coplanar anchor points.
[0015] Thirdly, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the aforementioned visual SLAM method for fusing point features and coplanar anchor points.
[0016] Compared with the prior art, the present invention has the following beneficial effects: The coplanar anchor point feature proposed in this invention is based on the inherent geometric structure of the scene, rather than image texture. Therefore, even in low-texture environments with smooth walls and lack of texture, the coplanar anchor point feature remains stable, effectively solving the tracking failure problem caused by feature sparsity in traditional point feature SLAM.
[0017] A coplanar anchor point feature provides geometric constraints equivalent to multiple point features, which can provide stronger and more stable constraints for backend pose optimization, thereby significantly improving trajectory estimation accuracy in low-texture and structured scenes.
[0018] This invention utilizes the geometric principle that two intersecting lines define a plane for feature matching, simplifying the high-dimensional feature descriptor matching problem into a low-dimensional plane parameter comparison, thus significantly reducing computational complexity. Furthermore, this feature extraction and optimization process is easily integrated into existing SLAM frameworks, ensuring the overall operational efficiency of the method.
[0019] Matching methods based on planar geometric constraints are insensitive to problems such as broken line segments and inaccurate endpoints, avoiding the ambiguity issues in traditional line feature matching and achieving higher matching accuracy. Attached Figure Description
[0020] Figure 1 This is a flowchart of the present invention. Detailed Implementation
[0021] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions in the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.
[0022] This embodiment provides a visual SLAM method that integrates point features and coplanar anchor points, such as Figure 1 As shown, the process generally includes a one-time initialization phase and a tracking and mapping loop phase that is continuously executed for each frame of the input image.
[0023] I. Initialization Phase Before processing the continuous video stream, an initialization operation is performed to establish an initial visual map. The specific steps are as follows: 1. Acquire the first RGB-D image: Capture the first image using an RGB-D camera. This image contains both color (RGB) information and depth (D, i.e., distance) information for each pixel.
[0024] 2. Feature Extraction: For the color portion of this first RGB-D image, perform the following two operations simultaneously: Point feature extraction: Using the ORB (Oriented Fast and Rotated BRIEF) feature extractor, find some significant corner points (such as texture points, corners) in the image, and calculate an ORB descriptor for each corner point. Record all extracted point features as a point feature set.
[0025] Extracting line segment features: Using the LSD (Line Segment Detector) algorithm, find all obvious straight line segments in the image (such as the edges of walls or the edges of tables). Record all extracted original line segments as the original line segment set.
[0026] 3. Generate coplanar anchor point features: For the original set of line segments extracted in step 2, perform the same processing flow as step S2 in the subsequent tracking and mapping loop, and filter and combine them to generate a set of coplanar anchor point features. Each coplanar anchor point feature consists of two intersecting line segments in the image that are coplanar in three-dimensional space.
[0027] Constructing the initial visual map: Using the depth information inherent in the first frame image and the camera's internal parameters, the following calculations are performed: For each two-dimensional point in the point feature set, calculate its three-dimensional coordinates in three-dimensional space based on its pixel position and corresponding depth value.
[0028] Similarly, calculate the three-dimensional coordinates of the four endpoints of the two line segments contained in each feature of the coplanar anchor point feature set, as well as their intersection (or approximate intersection).
[0029] All the calculated 3D points and their hierarchical relationships (e.g., which five points belong to the same coplanar anchor point feature) are saved to form the initial visual map.
[0030] Set the first keyframe: Pack the first RGB-D image, the camera pose at this time (usually set to the initial position, i.e., without rotation or movement), and the map points corresponding to all feature points in the image into a single keyframe and store it for later use.
[0031] At this point, an initial visual map and reference keyframes containing 3D point features and 3D coplanar anchor point features have been obtained. The initial visual map is the established visual map that subsequent processing relies on.
[0032] II. Tracking and Mapping Cycle Phase After initialization, for each subsequent input frame, the following five steps are executed sequentially: Step S1: Feature Extraction For the current input image frame, i.e., the current frame image: Point feature extraction: Using the ORB feature extractor, a set of point features and their descriptors for the current frame image are obtained. The ORB feature extractor first quickly locates key points (corners) in the image using the FAST corner detection algorithm, and then calculates a rotation-invariant binary BRIEF descriptor for each key point, which serves as a unique identifier for that point feature.
[0033] Extracting line segment features: Using the LSD algorithm, the original set of line segments in the current frame image is obtained.
[0034] Step S2: Coplanar anchor point feature generation The purpose of this step is to construct stable coplanar anchor point features from the original line segments of the current frame image. The specific sub-steps are as follows: Step S21, Line Segment Preprocessing and Optimization: First, the original set of line segments extracted from the current frame image is optimized to improve the quality of the line segments, laying the foundation for the subsequent generation of stable coplanar anchor point features. The optimization process includes two sub-steps: line segment fusion and line segment selection.
[0035] Segment merging: This function aims to reconnect broken segments of the same long line due to image noise or occlusion. It iterates through any two segments in the original segment set; if they simultaneously satisfy the following three geometric constraints, they are considered broken portions of the same long line segment and merged into a single, longer segment:
[0036] Directional consistency constraint: The angle between the direction vectors of two line segments is less than a preset angle threshold. (e.g., 5°) to ensure that their orientation is basically consistent.
[0037] Proximity constraint: Two line segments must be sufficiently close in space. The proximity distance is determined by calculating the perpendicular distance from the midpoint of one line segment to the other, and taking the smaller of the two values as the proximity distance. If this proximity distance is less than a preset distance threshold... (For example, 10 pixels), then the proximity property is satisfied.
[0038] Collinearity constraint: Two line segments lie on the same straight line. The coverage intervals of the two line segments on the one-dimensional projection axis are calculated by projecting them onto their average direction. If the two intervals are separate (i.e., do not overlap) and the gap between the intervals is less than a preset gap threshold, then the collinearity constraint is applied. If the conditions are met, then the model can be merged.
[0039] Line segment filtering: After the fusion operation, line segments that are too short (e.g., less than 1 / 30 of the image height) are filtered out to remove noise interference and reduce unnecessary computation. After fusion and filtering, an optimized set of line segments is obtained.
[0040] Step S22, Select a reference line segment: Select one line segment from the optimized line segment set as the reference line segment.
[0041] Step S23, Establish the spatial influence region: Using the center point of the reference line segment as a reference, define a rectangular spatial influence region. The geometric parameters of this spatial influence region are defined as follows: extending in the length direction... ( For reference line segment length, (For the preset neighborhood threshold), expand in the width direction. This forms a spatial constraint range with the perpendicular bisector of the reference line segment as the axis of symmetry.
[0042] Step S24: Find and pair candidate line segments: Traverse the optimized line segment set, excluding the reference line segment, and use them as candidate line segments. Only candidate line segments that meet all three of the following conditions can be paired with the reference line segment to form a coplanar anchor point feature: Endpoint proximity condition: At least one endpoint of the candidate line segment falls within the aforementioned spatial influence area.
[0043] Geometric intersection proximity condition: Calculate the intersection points of the candidate line segment and the reference line segment on the image. If they do not intersect, calculate the midpoint of the shortest line connecting them as the approximate intersection point. This intersection point or approximate intersection point must fall within the spatial influence area. The specific calculation methods for the shortest distance and midpoint are as follows: Let the two endpoints of the candidate line segment be the first endpoints of the candidate line segment. The second endpoint of the candidate line segment The direction vector of the candidate line segment is ; Let the two endpoints of the reference line segment be the first endpoints of the reference line segment. The second endpoint of the reference line segment The direction vector of the reference line segment is ; The points on the candidate line segment are represented as follows: , ; The points on the reference line segment are represented as follows: , ; By solving constrained optimization problems The constraints are as follows: , , obtain the first parameter Second parameter ; First parameter Second parameter All in Within the interval, the shortest distance is... and The distance between them; First parameter Or the second parameter Not here Within the interval, calculate the first endpoint of each candidate line segment. The shortest distance to the reference line segment, the second endpoint of the candidate line segment The shortest distance to the reference line segment, the first endpoint of the reference line segment The shortest distance to the candidate line segment and the second endpoint of the reference line segment The shortest distance to the candidate line segment is determined by taking the minimum of the four shortest distances mentioned above as the shortest distance between the candidate line segment and the reference line segment. The midpoint of the point pair corresponding to the minimum of these four distances is then calculated.
[0044] 3D coplanarity condition: Using depth information, confirm that the candidate line segment and the reference line segment are located on the same plane in the real 3D space.
[0045] Step S3: Feature Matching This step matches the point features and coplanar anchor point features extracted from the current frame image with the features in the established visual map, respectively.
[0046] Point feature matching: For each point feature extracted from the current frame image, its binary descriptor is compared with the binary descriptors associated with each map point in the established visual map. Similarity is measured by calculating the Hamming distance between the descriptors. For each point feature in the current frame image, the map point with the smallest Hamming distance less than a preset threshold in the established visual map is found, thus forming a point feature matching pair. All point feature matching pairs constitute a point feature matching pair set.
[0047] Coplanar anchor point feature matching: For a coplanar anchor point feature generated in the current frame image, it can determine a spatial plane in three-dimensional space. This spatial plane can be described by the plane parameters of the spatial plane in the world coordinate system, namely the normal vector of the spatial plane and the plane distance from the origin to the spatial plane.
[0048] For a coplanar anchor point feature in an established visual map, its planar parameters are also obtained.
[0049] Compare the two planes: calculate the angle between the normal vectors of the two planes and the absolute difference between the distance parameters of the two planes.
[0050] Match determination: If the included angle is less than a preset angle threshold and the absolute difference is less than a preset distance threshold, then the two coplanar anchor points are determined to match, forming a coplanar anchor point feature matching pair. All coplanar anchor point feature matching pairs constitute a coplanar anchor point feature matching pair set.
[0051] Step S4: Camera pose optimization Based on the set of point feature matching pairs and the set of coplanar anchor point feature matching pairs obtained in step S3, a fusion reprojection error model is constructed. The fusion reprojection error model is a weighted sum of the reprojection errors of the point features and the reprojection errors of the coplanar anchor point features.
[0052] Reprojection error of point features: For a point feature matching pair, its reprojection error is the difference between the two-dimensional coordinates of the 3D point in the map projected onto the current frame image plane and the two-dimensional coordinates actually observed in the current frame image.
[0053] Reprojection error of coplanar anchor point features: For a coplanar anchor point feature matching pair, its reprojection error is defined as the sum of the errors between the projected positions of the four endpoints and one intersection of the two line segments constituting the feature on the current frame image and their actual positions; the formula is as follows: ;in, For the reprojection error of coplanar anchor point features, , These are the projection errors of the first and second endpoints of the first line segment, respectively. , These are the projection errors of the first and second endpoints of the second line segment, respectively. The projection error of the intersection point of the two line segments is the projection error of the intersection point of the first line segment and the second line segment.
[0054] The overall objective function for constructing the fusion reprojection error model is the reprojection error of fused point features and coplanar anchor point features.
[0055] The overall objective function of the fused reprojection error model is given by the following formula: ; in, For the optimized camera pose, The camera pose of the current frame image. The set of keyframes corresponding to the current frame image. For the set of point feature matching pairs, For robust kernel functions, For the first In the first keyframe Transpose of the reprojection error of a point feature The information matrix of point features, For the first In the first keyframe Reprojection error of individual feature points For the set of coplanar anchor point feature matching pairs, The weighting coefficients for the coplanar anchor point features. The transpose of the reprojection error of the coplanar anchor point features. The information matrix of coplanar anchor point features. For the first In the first keyframe The reprojection error of the features of each coplanar anchor point.
[0056] Optimization solution: The Levenberg-Marquardt (LM) algorithm is used for iterative solution to obtain the optimized camera pose.
[0057] Step S5: Map Update and Keyframe Management Based on the optimized camera pose, the visual map is updated, and the following strategy is used to determine whether to add the current frame image as a new keyframe: Map update: Using the optimized camera pose, new point features and new coplanar anchor point features that were successfully tracked in the current frame but did not match the map are identified by triangulation or depth back projection to determine their 3D coordinates and added to the established visual map to expand the map.
[0058] Keyframe determination: The current frame is determined as a new keyframe when all of the following conditions are met: Viewpoint change condition: The number of map features (including point features and coplanar anchor point features) successfully tracked in the current frame is significantly less than the number tracked in the previous reference keyframe. This indicates that the camera has entered a new area or a significant viewpoint change has occurred, and the current frame carries new scene information.
[0059] Minimum interval condition: The time interval from the previous keyframe or the relative pose change of the camera movement exceeds a preset minimum threshold. This ensures that the keyframes are sparsely distributed in time or space, avoiding redundancy.
[0060] System load conditions: The background threads responsible for processing keyframe images and performing local map optimization are idle or under low load. This ensures that the system can process new keyframes in a timely manner and maintain real-time performance.
[0061] If the current frame image is determined to be a new keyframe, its image, optimized camera pose, and all associated map feature information are stored in the keyframe database for subsequent loop closure detection, relocalization, and long-term map optimization.
[0062] III. Experimental Verification and Results To verify the effectiveness of this invention, experiments were conducted on publicly available datasets. ORB-SLAM2 and PL-SLAM were selected as comparison methods and tested on the TUM and ICL-NUIM datasets. The evaluation metric included absolute trajectory error (ATE).
[0063] This invention uses five sequences (fr3_office, fr3_s_nt_far, fr3_s_nt_near, fr3_s_t_far, fr3_s_t_near) from the TUM dataset and eight sequences from the ICL-NUIM dataset for testing. In the TUM dataset, fr3_office records a camera moving along a great circle trajectory through home and office scenes, containing rich textures and structures, with overlapping trajectories forming significant closed loops. fr3_s_nt_far describes a camera moving along a zigzag path at a height of one meter, photographing a wooden board structure covered with white plastic foil and with almost no texture. fr3_s_nt_near is similar, but the camera height is reduced to half a meter. fr3_s_t_far records a camera moving along a zigzag path at a height of one meter, photographing a wooden board covered with colored plastic foil and with strong textures, with multiple high-texture posters covering the ground. fr3_s_t_near is similar, but at a height of half a meter. The ICL-NUIM dataset contains two scenes: a living room and an office. It provides data for four different camera trajectories for each scene. See Table 1 for details.
[0064] Table 1 Introduction to the ICL-NUIM Dataset
[0065] This invention tested ORB-SLAM2, ORB-SLAM2+line, and the invention itself (ours), and the absolute trajectory differences of the three algorithms are shown in Table 2. The values in Table 2 are the median of the root mean square error (RMSE) after five experiments.
[0066] Table 2 Comparison of Absolute Trajectory Difference Data for Each Algorithm
[0067] These datasets validated the algorithm's performance across diverse scenarios, covering rich textures, low textures, and complex structural environments, ensuring the comprehensiveness and reliability of the evaluation. In low-texture sequences, ORB-SLAM2 often experiences tracking failures or a sharp increase in error due to feature sparsity. When ORB-SLAM2 incorporates traditional line features (ORBSLAM2+line), although some tracking capability is restored, the absolute trajectory error remains high. This invention further improves accuracy by introducing coplanar anchor features. Specifically, in five sequences (fr3_office, fr3_s_nt_far, fr3_s_nt_near, fr3_s_t_far, fr3_s_t_near) in the TUM dataset, the average absolute trajectory error of this invention (ours) is 53.2% higher than that of the ORBSLAM2+line algorithm. This result demonstrates that coplanar anchor features (CAF), by providing geometric constraints equivalent to multiple point features, effectively compensate for the degradation problem caused by insufficient point features in low-texture scenes.
[0068] To verify the superiority of this invention, it was compared with other excellent algorithms in recent years, such as planar-SLAM, SP-SLAM, MSC-VO, and Structure PLP-SLAM. The results are shown in Table 3.
[0069] Table 3 Comparison of absolute trajectory difference data with other algorithms
[0070] As shown in Table 3, the present invention exhibits the lowest absolute trajectory error in most sequences. For example, in the fr3_office scene, the absolute trajectory error of the present invention is 0.009573 m, which is significantly lower than MSC-VO (0.037872 m), SP-SLAM (0.020219 m), Planar-SLAM (0.142735 m), and SPLP-SLAM (0.3192022 m). In low-texture scenes such as fr3_s_nt_far and fr3_s_nt_near, the present invention also maintains a low error level of around 0.02 m, significantly better than Planar-SLAM and SPLP-SLAM, and comparable to or even better than SP-SLAM. Except for a few special scenarios, such as lr_kt0 and or_kt0, where there are too many environmental structure line segments, the extraction of two line segments may cause mismatches, thus failing to achieve a significant improvement effect; however, in sequences with complex geometric structures such as lr_kt1, lr_kt2, lr_kt3, or_kt1, or_kt2, and or_kt3, this invention maintains the lowest error, while other algorithms show significant degradation (e.g., SP-SLAM reaches 0.633m in or_kt1, and SPLP-SLAM exceeds 0.9m).
[0071] In summary, the coplanar anchor point feature method proposed in this paper exhibits stronger accuracy and robustness in low-texture scenes and complex geometric environments. Compared with existing methods, it effectively reduces absolute trajectory error while ensuring real-time performance, demonstrating better consistency and stability, and providing strong support for the application of SLAM in complex environments.
[0072] This invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements a visual SLAM method for fusing point features and coplanar anchor points as described in any of the above embodiments.
[0073] This invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a visual SLAM method for fusing point features and coplanar anchor points as described in any of the above embodiments.
[0074] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
Claims
1. A visual SLAM method that fuses point features and coplanar anchor points, characterized in that, The methods include: Step S1: Extract point features and line segment features in parallel for the current frame image; Step S2: Process the line segment features extracted from the current frame image, filter and combine them to generate coplanar anchor point features; wherein, the coplanar anchor point features are composed of two intersecting line segments in the image that are coplanar in three-dimensional space. Step S3: Match the point features and coplanar anchor point features extracted from the current frame image with the features in the established visual map to obtain a set of point feature matching pairs and a set of coplanar anchor point feature matching pairs. Step S4: Based on the set of point feature matching pairs and the set of coplanar anchor point feature matching pairs, construct a fusion reprojection error model to optimize the camera pose of the current frame image and obtain the optimized camera pose. Step S5: Update the visual map based on the optimized camera pose, and determine whether to add the current frame image as a new keyframe.
2. The visual SLAM method for fusing point features and coplanar anchor points according to claim 1, characterized in that, The process begins with an initialization step before step S1, used to establish the established visual map. This initialization step specifically includes: Acquire the first frame of the RGB-D image; For the first frame of RGB-D image, point features and line segment features are extracted in parallel, and coplanar anchor point features are generated based on the line segment features; Based on the depth information of the first frame RGB-D image, the extracted point features and coplanar anchor point features are back-projected into three-dimensional space to construct an initial visual map, and the first frame RGB-D image is set as the first keyframe. The initial visual map serves as the established visual map.
3. The visual SLAM method for fusing point features and coplanar anchor points according to claim 2, characterized in that, Step S2, which involves processing the line segment features extracted from the current frame image, filtering and combining them to generate coplanar anchor point features, specifically includes: Step S21: Use the LSD algorithm to extract the original line segment set from the current frame image. By calculating the endpoint spacing, direction angle and projection overlap of adjacent line segments, merge broken line segments that meet the conditions of proximity, direction consistency and collinearity, and filter out line segments that are too short to obtain the optimized line segment set. Step S22: Select a line segment from the optimized line segment set as a reference line segment; Step S23: Establish a rectangular spatial influence area centered on the reference line segment; Step S24: From the optimized line segment set, find candidate line segments that satisfy all of the following conditions to combine with the reference line segment to form a coplanar anchor point feature: At least one endpoint of the candidate line segment is located within the spatial influence area; The intersection of the candidate line segment and the reference line segment, or the midpoint of the line connecting the shortest distance between the candidate line segment and the reference line segment, is located within the spatial influence area; The candidate line segment and the reference line segment satisfy the coplanar constraint in three-dimensional space.
4. The visual SLAM method for fusing point features and coplanar anchor points according to claim 3, characterized in that, In step S24, the midpoint of the line connecting the candidate line segment and the reference line segment with the shortest distance is determined; the method for calculating the shortest distance is as follows: Let the two endpoints of the candidate line segment be the first endpoints of the candidate line segment. The second endpoint of the candidate line segment The direction vector of the candidate line segment is ; Let the two endpoints of the reference line segment be the first endpoints of the reference line segment. The second endpoint of the reference line segment The direction vector of the reference line segment is ; The points on the candidate line segment are represented as follows: , ; The points on the reference line segment are represented as follows: , ; By solving constrained optimization problems The constraints are as follows: , , obtain the first parameter Second parameter ; First parameter Second parameter All in Within the interval, the shortest distance is... and The distance between them; First parameter Or the second parameter Not here Within the interval, calculate the first endpoint of each candidate line segment. The shortest distance to the reference line segment, the second endpoint of the candidate line segment The shortest distance to the reference line segment, the first endpoint of the reference line segment The shortest distance to the candidate line segment and the second endpoint of the reference line segment The shortest distance to the candidate line segment is determined by taking the minimum of the four shortest distances mentioned above as the shortest distance between the candidate line segment and the reference line segment.
5. The visual SLAM method for fusing point features and coplanar anchor points according to claim 4, characterized in that, In step S3, the point features and coplanar anchor point features extracted from the current frame image are matched with the features in the established visual map, respectively; specifically, the matching of coplanar anchor point features with the features in the established visual map involves the following steps: For a coplanar anchor point feature, a spatial plane is determined using the two line segments of the coplanar anchor point feature; The planar parameters of the spatial plane in the world coordinate system are expressed as follows: ,in, Let be the normal vector of the space plane. This is the planar distance from the origin to the spatial plane; The planar parameters of the spatial plane in the world coordinate system determined by the coplanar anchor point features extracted from the current frame image are compared with the planar parameters of the spatial plane in the world coordinate system determined by the coplanar anchor point features extracted from the established visual map. If the angle between their normal vectors is less than a threshold and the difference in their planar distance is less than a threshold, then the coplanar anchor point features extracted from the current frame image are determined to match the coplanar anchor point features extracted from the established visual map.
6. The visual SLAM method for fusing point features and coplanar anchor points according to claim 1, characterized in that, The fusion reprojection error model described in step S4 is a weighted sum of the reprojection error of point features and the reprojection error of coplanar anchor point features; wherein, the reprojection error of coplanar anchor point features is calculated as follows: ； in, For the reprojection error of coplanar anchor point features, , These are the projection errors of the first and second endpoints of the first line segment, respectively. , These are the projection errors of the first and second endpoints of the second line segment, respectively. This represents the projection error at the intersection of the two line segments; The reprojection error of the coplanar anchor point features The sum of errors between the projected positions and measured positions of the four endpoints and the intersection of the two line segments with coplanar anchor point features on the current frame image.
7. The visual SLAM method for fusing point features and coplanar anchor points according to claim 1, characterized in that, The overall objective function of the fused reprojection error model is given by the following formula: ； in, For the optimized camera pose, The camera pose of the current frame image. The set of keyframes corresponding to the current frame image. For the set of point feature matching pairs, For robust kernel functions, For the first In the first keyframe Transpose of the reprojection error of a feature point The information matrix of point features, For the first In the first keyframe Reprojection error of individual feature points For the set of coplanar anchor point feature matching pairs, The weighting coefficients for the coplanar anchor point features. The transpose of the reprojection error of the coplanar anchor point features. The information matrix of coplanar anchor point features. For the first In the first keyframe The reprojection error of the features of each coplanar anchor point.
8. The visual SLAM method for fusing point features and coplanar anchor points according to claim 1, characterized in that, In step S4, the camera pose of the current frame image is optimized and solved using the Levenberg-Marquardt LM algorithm.
9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements a visual SLAM method for fusing point features and coplanar anchor points as described in any one of claims 1 to 8.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements a visual SLAM method for fusing point features and coplanar anchor points as described in any one of claims 1 to 8.