A semi-direct visual positioning method fusing point and line features

Abstract

The application discloses a semi-direct visual positioning method based on point-line feature fusion, and belongs to the field of three-dimensional vision. A monocular camera is used to continuously acquire images, point features and line features are extracted from each image frame respectively, feature descriptors are used to match the features of the current frame and the previous frame, and the pose transformation between adjacent frames is estimated based on the epipolar geometry constraint and a three-dimensional map is initialized. A sparse feature pixel alignment method is used for pose estimation of the current frame. If the pose estimation fails, the point features and line features of the current frame are extracted, the camera pose is re-estimated by minimizing the re-projection error of the point-line features. Key frames are extracted based on the motion constraint of the camera, point features and line features are extracted from the key frames, new map points and map lines are generated, and the three-dimensional map is updated. Loop detection is performed, the camera pose and the spatial point position are further optimized based on the point-line fusion BA, and the cumulative error is reduced. The application solves the tracking failure of the monocular visual positioning method in a low-texture scene due to the lack of point features.

Description

Technical Field

[0001] This invention belongs to the field of 3D vision and relates to a semi-direct visual localization method based on a monocular vision sensor and fusing point and line features. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a technique that uses sensors on a robot to build a model of its environment while it is moving, without prior information about the environment, and simultaneously estimates its own motion. Visual SLAM technology uses a camera as the primary sensor to estimate the camera's motion and a model of the environment from images. Visual localization technology is widely used in mobile robotics, virtual reality, autonomous driving, and other fields.

[0003] Mainstream visual SLAM methods can be divided into feature-point methods and direct methods. Feature-point methods typically extract image feature points and use the geometric relationship between camera pose and feature point positions to solve for camera trajectory, such as ORB-SLAM. However, feature extraction and matching are time-consuming and lack adaptability under conditions such as low texture or motion blur. Direct methods do not require calculating image feature points and rely on the assumption of gray-level invariance. They quickly solve for camera pose and the 3D coordinates of feature points through photometric matching of image pixels, such as DSO and LSD-SLAM. However, scenarios with large camera baseline motion or changes in illumination can cause camera pose tracking based on direct methods to fail.

[0004] To address the issue of insufficient adaptability of the feature point method in low-texture scenes, such as Figure 1 As shown, some systems use point and line fusion to solve the SLAM problem, such as the PL-SLAM method. This method uses the LSD method to extract straight line segments from the image and adds feature line processing to ORB-SLAM2, which improves the accuracy of system localization and mapping, as well as its robustness in low-texture environments. However, the extraction of point and line features requires a significant amount of runtime, challenging the real-time performance of visual localization. This invention fully utilizes the advantages of direct methods and feature point methods. In the pose tracking process of ordinary image frames, a direct method based on point and line fusion is applied for pose estimation, improving system speed. If the direct method fails, a feature-based method is used to re-estimate the camera pose, extracting point and line features from keyframes. A reprojection error model based on point and line feature fusion is used to optimize the camera pose and 3D map, thereby ensuring the accuracy of system localization and 3D mapping. Summary of the Invention

[0005] To effectively improve the accuracy and robustness of visual positioning systems in low-texture scenes, this invention proposes a semi-direct visual positioning method that fuses point and line features. This method estimates camera pose using a sparse feature pixel alignment method at non-keyframes. If the direct method fails, a feature-based method is used to re-estimate the camera pose. At keyframes, a feature descriptor matching method and a reprojection error model based on point and line feature fusion are used to optimize camera pose and the 3D map, achieving real-time robust camera trajectory tracking in low-texture scenes.

[0006] This invention proposes a semi-direct visual localization method that integrates point and line features, comprising:

[0007] Step 1: System initialization. This step uses a monocular camera to continuously acquire image frames, extracts point and line features of each image frame, and matches the point and line features of the current frame and the previous frame based on the feature descriptor method. If a sufficient number of point and line features are matched, the pose transformation of the current frame and the previous frame is estimated based on epipolar geometric constraints, and the 3D map is initialized.

[0008] Step 1.1: Continuously acquire images through the camera, perform distortion correction on the images using the camera's own distortion parameters, and convert them into grayscale images; extract ORB point features and LSD line features from the image frames, and calculate the BRIEF and LBD descriptors for the point and line features respectively.

[0009] Step 1.2: Calculate the descriptor similarity between the point features and line features of the current frame and the previous frame. Count the number of point features and line features matched between the current frame and the previous frame. If more than 100 point features are matched and more than 15 line features are matched, to ensure that there are enough matching features to maintain the robustness of the system, then set the current frame and the previous frame as the initial keyframes for initializing the visual positioning system.

[0010] Step 1.3: Calculate the pose transformation between two initial keyframes based on epipolar geometry constraints. Establish a 3D world coordinate system with the first keyframe as the origin. Use triangulation to map the point and line features of the two initial keyframes to 3D space, initialize the 3D map, and finally optimize the pose of the initial keyframes and the 3D map points using a point-line feature fusion-based Basis Argumentation (BA) method. The solver for the BA optimization problem is defined using the g2o library, and the iteration strategy uses the LM algorithm with 20 iterations to effectively remove outliers and reduce false matches.

[0011] Step 2: After initialization, the pose estimation of the current frame is performed based on the sparse feature pixel alignment method. If the pose estimation fails, the point features and line features of the current image frame are extracted and matched with the points and lines of the 3D map to construct a reprojection error model of point and line features to estimate the camera pose.

[0012] Step 2.1: Use the point and line features observed in the previous frame to back-project to the three-dimensional space, and then project the three-dimensional coordinates corresponding to the features back to the pixel coordinate system of the current frame;

[0013] Step 2.2: Track the projection positions of point features and line features in the current frame using the sparse feature pixel alignment method, construct a photometric error model, estimate the camera pose by minimizing the photometric error, set the number of iterations to 30, remove outliers, and repeat the outlier removal operation 4 times to effectively remove outliers.

[0014] Step 2.3: If the number of inliers obtained from pose estimation is less than the threshold (here, the threshold is set to 50 to ensure the effectiveness of optimization), then the direct method camera pose estimation is judged to have failed. The point features and line features of the current frame are extracted, and the corresponding descriptors are calculated respectively.

[0015] Step 2.4: If the number of inliers in Step 2.3 is less than the threshold, then based on the point and line feature descriptors, match the map points and map lines in the current frame and the 3D map, construct a reprojection error model based on the fusion of point and line features, and re-estimate the camera pose of the current frame.

[0016] Step 3: Using the camera's motion constraints and keyframe judgment constraints in ORB-SLAM, determine whether the current frame is a keyframe. If it is a keyframe, extract point features and line features from the current frame, generate new map points and map lines, and remove redundant keyframes and duplicate map points and map lines.

[0017] Step 3.1: Determine whether the current frame is a keyframe based on the camera's motion constraints. If it is a keyframe and features have not yet been extracted from the current frame, extract the ORB point features and LSD line features of the current frame, calculate the corresponding BRIEF and LBD descriptors, as well as the BOW vector of the ORB point features, and insert the current frame into the keyframe sequence.

[0018] Step 3.2: Based on point and line feature descriptors, match the point features and line features extracted in the current frame with the map points and map lines in the 3D map, and generate new map points and map lines based on the triangulation method;

[0019] Step 3.3: Traverse all keyframes within the 3D map area and remove redundant keyframes and duplicate map points and lines based on the common redundancy of features.

[0020] Step 4: Perform loop closure detection on the bag-of-words model based on ORB features for keyframes, optimize keyframe pose and 3D map, and reduce visual localization cumulative error.

[0021] Step 4.1: Use the bag-of-words model based on ORB feature points to perform loop closure detection on the current frame and keyframe sequence, extract reference keyframes with co-visible regions with the current frame, and use the random sampling consensus algorithm to calculate the similarity transformation between the current frame and the matched reference keyframes to perform pose correction of the current keyframe and adjacent keyframes.

[0022] Step 4.2: The BA method based on point and line feature fusion is used to optimize the positions of all keyframes and 3D map points and lines, reduce the cumulative error of the visual positioning system, and output the camera pose of the keyframes. Attached Figure Description

[0023] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute an undue limitation of this application.

[0024] Figure 1 This refers to the low-texture scene involved in this invention;

[0025] Figure 2 This is a schematic diagram of the overall framework of the present invention;

[0026] Figure 3 This invention relates to the matching relationship between point features and line features on the initial keyframe;

[0027] Figure 4 This invention relates to feature matching and motion trajectory of keyframes for visual positioning.

[0028] Figure 5 This is the result of the camera motion trajectory estimation in a real long corridor environment according to the present invention. Detailed Implementation

[0029] The following provides a general overview of one or more aspects to address a basic understanding of these aspects. This overview does not elaborate on all the contemplated aspects; its sole purpose is to present some concepts of one or more aspects in a simplified form to precede the more detailed description that follows. Note that the aspects described below in conjunction with the accompanying drawings and specific embodiments are exemplary only and should not be construed as limiting the scope of the invention in any way.

[0030] Figure 2 The system framework of this invention is shown. A semi-direct visual localization method that integrates point and line features mainly consists of four stages: system initialization stage, camera pose estimation stage, map update stage, and loop closure detection stage.

[0031] Step 1: Visual positioning system initialization phase. Continuously acquire image frames using a monocular camera, extract point and line features from each frame, and match the point and line features of the current and previous frames based on the feature descriptor method. If sufficient point and line features are matched, estimate the pose transformation of the current and previous frames based on epipolar geometric constraints, and initialize the 3D map.

[0032] Step 1.1: Continuously acquire RGB image frames through the camera, correct the distortion of the image frames using the camera's distortion parameters, and convert the RGB images to grayscale images; extract the point features and line features of the current frame, and calculate the descriptors corresponding to the point and line features.

[0033] The extraction and matching of point features utilizes ORB feature points and BRIEF descriptors. First, an image pyramid is constructed using the distortion-free grayscale image. Second, OpenCV functions are called to extract FAST corner points, and the orientation of each feature point is calculated using the grayscale centroid method. Third, the BRIEF descriptors of the feature points are calculated.

[0034] Line feature extraction and matching utilize LSD line features and LBD descriptors. Line features are extracted using the LSD algorithm from the OpenCV library. This algorithm merges pixels with similar grayscale gradient directions to generate a line segment support region, and then uses the minimum bounding rectangle to determine whether this support region is used as the final generated line feature. The feature descriptor used is the LBD line segment descriptor.

[0035] Step 1.2: Calculate the descriptor similarity between point features and line features in the current frame and the previous frame, and count the number of point features and line features matched in the current image frame and the previous image frame. If more than 100 point features are matched and more than 15 line features are matched, ensuring that there are enough matching features to maintain the robustness of the system, then set the current frame and the previous frame as the initial keyframes for initializing the visual positioning system.

[0036] Step 1.3: Initialize the 3D map based on the initial keyframes. First, calculate the pose transformation T between two initial keyframes K1 and K2 based on geometric constraints. 12 (3×3 rotation matrix R from the first frame to the second frame) 12 and 3×1 translation vector t 12 Secondly, a 3D world coordinate system is established with the first keyframe as the origin. Triangulation is used to map the matched point and line features to this system to obtain 3D coordinates, thus constructing an initial 3D map. Finally, a Base Alignment (BA) method based on the fusion of point and line features is used to optimize the initial camera pose and 3D map. The solver for the BA optimization problem is defined using the g2o library, and the iteration strategy uses the LM algorithm with 20 iterations to effectively remove outliers and reduce false matches.

[0037] in, Figure 3 This invention relates to the matching relationship between point features and line features in the initial keyframes K1 and K2. Specifically, it involves extracting two-dimensional point feature coordinates x and line feature coordinates l in the pixel coordinate system of the previous frame. The two endpoints of the line feature correspond to pixel coordinates p and q, respectively, and to the two endpoints of the line feature in three-dimensional space, P and Q. W represents the spatial point coordinates uniformly sampled on the three-dimensional line feature, corresponding to the image frame pixel coordinate system coordinates w. The pose transformation between the camera's optical centers O1 and O2 is achieved using the rotation matrix R between the camera's optical centers. 12 Translation vector t 12 express.

[0038] For the corresponding p, p′, etc. in the pixel coordinate system of the previous frame and the current frame, the epipolar geometry constraint is satisfied:

[0039] p′= 12 p+t 12 Based on the two-dimensional pixel coordinates of all point and line feature endpoints matched in two keyframes, the pose transformation T of the two keyframes is calculated using the Ransac method. 12 (R 12 , t 12 Based on the camera coordinate system of the first keyframe, the positions of the endpoints of point features and line features in the 3D map are calculated using a triangulation method.

[0040] Finally, the BA method based on the fusion of point features and line features is used to optimize the initial pose and 3D map points. The solver for the BA optimization problem is defined using the g2o library, and the iteration strategy uses the LM algorithm with 20 iterations to effectively remove outliers and reduce false matches.

[0041] Step 2: After initialization, the pose estimation of the current frame is performed based on the sparse feature pixel alignment method. If the pose estimation fails, the point features and line features of the current image frame are extracted, and the reprojection error model of the corresponding point and line features in the current frame and the 3D map are constructed to re-estimate the camera pose.

[0042] Step 2.1: Inversely project the point features of the previous frame and the points on the uniformly sampled line features (sampling one point every 4 points) onto the 3D map coordinates, and then project the 3D coordinate points back to the pixel coordinate system of the current frame;

[0043] Step 2.2: Based on the sparse feature pixel alignment method, the pose of the current frame is estimated. A photometric error model is constructed to estimate the pose of the current frame. The photometric error is the difference between the gray value of the pixel block formed by the point and its surrounding points in the pixel coordinate system projected onto the current frame in Step 2.1 and the gray value of the pixel block formed by the points and their surrounding points on the point features and line features observed in the previous frame. The camera pose is estimated by minimizing the photometric error. The number of iterations is set to 30. Outliers are removed. The operation of removing outliers is repeated 4 times to effectively remove outliers.

[0044] The photometric error of the point corresponding to the point feature or line feature is expressed as: e i =I c (u i ')-I c-1 (u i ), where I c-1 (u i ) represents the grayscale value of the pixel block formed by the i-th point and its surrounding points in frame c-1. c (u i ') represents the lower point u in the pixel coordinate system projected onto the current frame in 2.1. i The grayscale value of the pixel block formed by ' and its surrounding points;

[0045] Step 2.3: If the number of inliers obtained after the iterative optimization in Step 2.2 is greater than the threshold (here the threshold is set to 50), then the pose estimation is considered reliable. Otherwise, the direct method camera pose estimation is considered to have failed. Extract the ORB point features and LSD line features of the current frame, and calculate the BRIEF and LBD descriptors respectively.

[0046] Step 2.4: If the number of inliers in Step 2.3 is less than the threshold, match the map points and lines in the current frame and the 3D map based on the feature descriptor, and use the matched point features and line features to construct a reprojection error model based on point and line features to estimate the camera pose of the current frame.

[0047] Wherein, the normalization coefficient is set as Where P and Q represent the three-dimensional coordinates corresponding to the two endpoints of the line feature, respectively, and the reprojection error of the line feature is constructed. Reprojection error E at the two endpoints of the map line pline =L T π(P,θ,K),E qline =L T π(Q,θ,K), the mapping model from the endpoint spatial point coordinates P to the current frame pixel coordinates p is π(P,θ,K), where θ={R cw ,t cw} represents the camera pose (a 3x3 rotation matrix Rcw and translation vector tcw representing the transformation relationship from the 3D map coordinate system to the current frame camera coordinate system), and K represents the camera intrinsic parameters.

[0048] The reprojection error of any point feature is represented by the point position X on the 3D map. i The pixel coordinates x mapped to the current image frame i error e i =x i -π(X i ,θ,K).

[0049] A reprojection error model based on point and line feature fusion is constructed, and its reprojection error model is as follows: Where e point,k and e line,j Let Ω represent the k-th map point and j-th map line matched in the current frame, respectively. np and nl represent the total number of map points and map lines matched in the current image frame, respectively. point,k and Ω line,j This represents the observation covariance of points and lines.

[0050] Step 3: Determine whether the current frame is a keyframe based on the camera's motion constraints. If it is a keyframe, extract point and line features from the current frame, generate new map points and map lines, and remove redundant keyframes, map points, and map lines.

[0051] Step 3.1: Determine whether the current frame is a keyframe based on the camera's motion constraints. If it is a keyframe and features have not yet been extracted from the current frame, extract the ORB point features and LSD line features of the current frame, calculate the corresponding descriptors and the BOW vector of the ORB point features, and insert the current frame into the keyframe sequence.

[0052] In addition to satisfying the ORB-SLAM co-view constraint, this invention adds a motion constraint judgment to the keyframe determination condition. Compared with the previous keyframe, the threshold is set as the rotation matrix R between the current frame c and the previous keyframe r. cr The corresponding rotation degree is greater than 3°, and the corresponding translation vector is t. cr If the absolute value is greater than 0.06 to reduce the computational burden, then the determination of whether the current frame is a key frame is added.

[0053] Step 3.2: Based on the point and line features extracted from the current frame and the 3D map points and lines, generate new map points and lines using the triangulation method;

[0054] The generation of new map points and map lines requires the following conditions: the feature can be observed in 3 or more keyframes, the camera center coordinates of adjacent keyframes are obtained, the baseline of the camera optical center and feature mapping of the two image frames to the three-dimensional space is calculated, and the baseline length is greater than 0.01 to ensure the system's computational efficiency.

[0055] Step 3.3: Run point and line feature fusion BA on keyframes that share common point and line features with the current frame. Traverse all keyframes within the 3D map range, and remove redundant keyframes, map points, and map lines based on the common redundancy of feature points.

[0056] Among them, the keyframe redundancy meets the judgment condition: if there is a keyframe in the keyframe sequence, and more than 90% of the map points or map lines observed by it can be observed by at least three other keyframes, then the keyframe is removed.

[0057] Redundant map points and lines are determined by the following conditions: When traversing map points and lines, the following constraints must be met: 1) They must be found on at least 25% of the keyframes in which their corresponding features are theoretically observable; 2) If more than one keyframe has been generated since the creation of the map point or line, the feature must be observable in at least three keyframes. If the above constraints are not met, the map point or line is considered redundant.

[0058] Step 4: Use the bag-of-words model based on ORB features to perform loop closure detection on keyframes, and use the BA method that fuses point and line features to further optimize the keyframe pose and the global 3D map, reducing the cumulative error of visual localization.

[0059] Step 4.1: Use the bag-of-words model based on ORB feature points to perform loop closure detection on the current frame and keyframe sequence, extract reference keyframes with co-visible regions with the current frame, and use the random sampling consensus algorithm to calculate the similarity transformation between the current frame and the matched reference keyframes to perform pose correction of the current keyframe and adjacent keyframes.

[0060] Step 4.2: The global 3D map and keyframe pose are optimized using the BA method based on point and line feature fusion. The solver for the optimization problem is defined using the g2o library, and the iteration strategy uses the LM algorithm with 20 iterations to reduce the cumulative error of the visual positioning system and output the pose of the keyframe.

[0061] Global BA optimization is based on a reprojection error model that integrates point and line features. Where e i,k and e i,jLet be the errors in matching the k-th map line and the j-th map point from the i-th keyframe, respectively, and let np and nl be the total number of map points and map lines matched across all keyframes. Ω i,k and Ω i,j This represents the observation covariance of points and lines.

[0062] Figure 4 This invention relates to feature matching for visual positioning and motion trajectories of keyframes.

[0063] Figure 5 This is the result of the camera motion trajectory estimated by the present invention in a real long corridor environment.

[0064] To simplify the explanation of this article, the above diagrams and text describe a series of steps. However, it should be understood that these methods are not restricted by the order of operations, because some actions may occur in different sequences when implemented according to one or more steps, but those skilled in the art can understand the principles by which the actions occur.

[0065] Although illustrative specific embodiments of the invention have been described step by step to enable those skilled in the art to understand them, the invention is not limited to the scope of the specific embodiments. Those skilled in the art can make various modifications or variations within the scope of the claims, as long as such variations are within the spirit and scope of the invention as defined and determined by the appended claims.

Claims

1. A semi-direct visual localization method that integrates point and line features, characterized in that, Includes the following steps: Step 1: Visual positioning system initialization phase; continuously acquire image frames using a monocular camera, extract point and line features of each image frame, and match the point and line features of the current frame and the previous frame based on the feature descriptor method. If a sufficient number of point and line features are matched, estimate the pose transformation of the current frame and the previous frame based on epipolar geometric constraints, and initialize the 3D map. Step 2: Camera pose estimation stage; After initialization, the pose of the current frame is estimated based on the sparse feature pixel alignment method; If the pose estimation fails, the point features and line features of the current image frame are extracted, and a reprojection error model of the 3D map and the corresponding point and line features in the current frame is constructed to re-estimate the camera pose. Step 3: Map update stage; Based on the camera's motion constraints, determine whether the current frame is a keyframe. If it is a keyframe, extract point and line features from the current frame, generate new map points and lines, and remove redundant keyframes, as well as duplicate map points and lines. Step 4: Loop closure detection stage; loop closure detection is performed on keyframes using a bag-of-words model based on ORB features. The BA method, which fuses point and line features, is used to further optimize the global keyframe pose and 3D map, reduce the cumulative error of visual localization, and output the pose sequence of keyframes. Step 2 further includes the following steps: Step 2.1: Inversely project the point features observed in the previous frame and the points uniformly sampled on the line features into three-dimensional space, and then project the three-dimensional coordinate points into the pixel coordinate system of the current frame; Step 2.2: Based on the sparse feature pixel alignment method, the pose of the current frame is estimated, and a photometric error model is constructed to estimate the pose of the current frame. The photometric error is the difference between the gray value of the pixel block formed by the point projected onto the pixel coordinate system of the current frame and its surrounding points in Step 2.1 and the gray value of the pixel block formed by the point observed in the previous frame and its surrounding points. The camera pose is estimated by minimizing the photometric error. The number of iterations is set to 30. Outliers are removed. The operation of removing outliers is repeated 4 times to effectively remove outliers. Step 2.3: If the number of inliers obtained after the iterative optimization in Step 2.2 is greater than the threshold (here the threshold is set to 50), then the pose estimation is considered reliable. Otherwise, the direct method camera pose estimation is considered to have failed. Extract the ORB point features and LSD line features of the current frame, and calculate the BRIEF and LBD descriptors respectively. Step 2.4: If the number of inliers in Step 2.3 is less than the threshold, then based on the point and line feature descriptors, match the map points and map lines in the current frame and the 3D map, construct a reprojection error model based on the fusion of point and line features, and re-estimate the camera pose of the current frame. Step 3 further includes the following steps: Step 3.1: Determine whether the current frame is a keyframe based on the camera's motion constraints. If it is a keyframe and features have not yet been extracted from it, extract the ORB point features and LSD line features of the current frame, calculate the corresponding BRIEF and LBD descriptors, and the BOW vector of the ORB point features, and insert the current frame into the keyframe sequence. Here, the motion constraints for extracting keyframes satisfy the ORB-SLAM keyframe determination constraints and the pose transformation constraints between the current frame c and the previous keyframe r. The threshold is set to the rotation matrix R. cr The corresponding rotation degree is greater than 3°, and the translation vector t cr The absolute value is greater than 0.06 to reduce the computational burden; Step 3.2: Based on the descriptors of point and line features, match the features extracted in the current frame with the map points and lines in the 3D map, and generate new map points and lines based on the triangulation method; Step 3.3: Traverse the keyframe sequence and remove redundant keyframes, map points, and map lines based on the co-visibility redundancy of the features. Specifically, if a keyframe exists in the keyframe sequence and more than 90% of the map points or map lines observed by it can be observed by at least three other keyframes, then the redundant keyframe is removed. The non-redundancy conditions for map points or map lines are as follows: the number of keyframes that can find the feature corresponding to the map point or map line must be greater than 25% of the theoretically observable number of keyframes; if more than one keyframe has been generated since the creation of the map point or map line, the feature must be observable by at least three keyframes, otherwise the map point or map line is deleted.

2. The semi-direct visual localization method based on the fusion of point and line features according to claim 1, characterized in that, Step 1 further includes the following steps: Step 1.1: Continuously acquire images through the camera, extract ORB point features and LSD line features from the image frames, and calculate the BRIEF and LBD descriptors for the point and line features respectively; Step 1.2: Calculate the descriptor similarity between point features and line features in the current frame and the previous frame. Count the number of point features and line features matched by the current image frame from the previous image frame. If more than 100 point features are matched and more than 15 line features are matched, ensuring sufficient matching features to maintain the robustness of the system, then set the current frame and the previous frame as the initial keyframes for initializing the visual positioning system. Step 1.3: Initialize the 3D map based on the initial keyframes. First, calculate the pose transformation T between two initial keyframes K1 and K2 based on geometric constraints. 12 That is, the 3×3 rotation matrix R from the first frame to the second frame. 12 and 3×1 translation vector t 12 Secondly, a 3D world coordinate system is established with the first keyframe as the origin. The matching point features and line features are mapped to the 3D world coordinate system using the triangulation method to obtain their corresponding 3D coordinates and construct the initial 3D map. Finally, the initial camera pose and 3D map are optimized using the BA method based on the fusion of point features and line features. The solver for the BA optimization problem is defined using the g2o library, the iteration strategy uses the LM algorithm, and the number of iterations is set to 20.

3. The semi-direct visual localization method based on the fusion of point and line features according to claim 1, characterized in that, Step 4 further includes the following steps: Step 4.1: Use the bag-of-words model based on ORB feature points to perform loop closure detection on the current frame and keyframe sequence, extract reference keyframes with co-visible regions with the current frame, and use the random sampling consensus algorithm to calculate the similarity transformation between the current frame and the matched reference keyframes to perform pose correction of the current keyframe and adjacent keyframes. Step 4.2: The global 3D map and keyframe pose are optimized using the BA method based on point and line feature fusion. The solver for the optimization problem is defined using the g2o library, and the iteration strategy uses the LM algorithm with 20 iterations to reduce the cumulative error of the visual positioning system and output the camera pose of the keyframe.

Images