Improved geometric constraint multi-feature visual manhattan-slam

Abstract

The application provides a multi-feature visual Manhattan-SLAM with improved geometric constraints, comprising: S1, multi-feature extraction and matching, fully utilizing scene structure information to extract point, line and surface multi-feature information; S2, line feature hidden parameter visualization and short line feature merging, part of parameters in the LSD algorithm are visualized and adjusted, then a judgment condition is established, short line features meeting the condition are merged, and the merged line features are used for feature matching to improve the accuracy of visual tracking; S3, a line feature matching and tracking strategy with geometric and appearance constraints is established, constraints are established for the spatial position and geometric relationship of the line feature, and the accuracy of line feature matching and tracking is improved; S4, combined with pose estimation of structured scenes and unstructured scenes, a re-projection error is constructed for point, line and surface multi-dimensional features, the re-projection error is optimized by using a Levenberg-Marquardt algorithm, and translation and rotation estimation in a structured environment and an unstructured environment is obtained; and S5, the extracted point, line and surface multi-features are fused, and a sparse map is established and updated.

Description

Improved geometrically constrained multi-feature visual Manhattan-SLAM Technical Field

[0001] This invention relates to the field of indoor navigation and positioning technology, specifically to a synchronous positioning and mapping method with spatial geometric constraints established through multi-feature fusion. Background Technology

[0002] With the continuous development of science and technology, and the advancement of wireless communication technology and the Internet, the requirements for positioning accuracy based on location services are becoming increasingly stringent. While outdoor positioning systems such as the BeiDou Navigation Satellite System and the Global Navigation Satellite System have achieved superior accuracy, they are unsuitable for indoor positioning due to factors such as building obstructions and signal interference. However, since 80% of people's major social activities take place indoors, the demand for high-precision and highly reliable indoor positioning technology is constantly increasing.

[0003] Currently, mainstream SLAM technologies are primarily based on point features. A point is the smallest unit of information expression in image feature extraction and tracking, and it has the advantages of low computational resource consumption and ease of tracking. However, in point feature-based methods, the quality of the point features is crucial to the robustness and accuracy of the algorithm. In structured scenes, the structure and size of objects change relatively stably and regularly, exhibiting obvious geometric features, such as walls and tables. Due to their low surface texture, sufficient point features cannot be detected for tracking in such scenes. Summary of the Invention

[0004] The purpose of this invention is to fully utilize the geometric features of indoor scenes, using an RGB-D camera as the sensor, points and lines as the main features, and planes as auxiliary features, to perform pose tracking and estimation. First, the LSD algorithm is improved by making hidden parameters explicit, merging available short-line features, and filtering low-quality features to improve detection efficiency. Then, a feature matching strategy combining geometry and appearance is proposed, and a graph optimization algorithm is used to optimize camera pose. This method improves the robustness of camera pose estimation in indoor scenes, allowing the entire system to adapt well to changes in environmental texture.

[0005] The technical solution of this invention to solve the above problems is as follows: An improved geometrically constrained multi-feature visual Manhattan-SLAM, comprising the following steps:

[0006] S1, multi-feature extraction and matching;

[0007] S2, the hidden parameters of line features are made explicit and short line features are merged;

[0008] S3, establish line feature matching and tracking based on geometric and appearance constraints;

[0009] S4, combining pose estimation of structured and unstructured scenes;

[0010] S5 integrates the extracted point, line, and surface features and builds a sparse map.

[0011] The beneficial effects of this invention are:

[0012] This invention proposes a novel line feature extraction and tracking algorithm for structured scenes. By improving the LSD algorithm, it effectively utilizes the numerous short line features present in the scene. Subsequently, it proposes a matching and tracking algorithm based on geometric and appearance constraints for line features, and optimizes the error of line features to refine the spatial position information of 3D line features. By establishing the Manhattan world hypothesis to decouple pose information, it effectively reduces the influence of outliers and improves the accuracy of pose estimation. Compared with current mainstream algorithms, this improved algorithm demonstrates superior accuracy and robustness, providing a new method for localization technology. Attached Figure Description

[0013] Figure 1 is a system overall block diagram of the present invention;

[0014] Figure 2 is a schematic diagram of the spatial linear residual in this invention;

[0015] Figure 3 shows the coordinate relationship under the MW assumption;

[0016] Figure 4 shows a comparison of trajectories in the of-kt0 dataset;

[0017] Figure 5 shows a comparison of trajectories in the of-kt2 dataset;

[0018] Figure 6 shows a comparison of trajectories in the of-kt3 dataset;

[0019] Figure 7 shows a comparison of trajectories in the fr3_s_nt_far dataset. Detailed Implementation

[0020] The present invention will be further illustrated below with specific examples. These examples are only used to explain the present invention and are not intended to limit the scope of the present invention.

[0021] As shown in Figure 1, the improved geometrically constrained multi-feature visual Manhattan-SLAM includes the following steps:

[0022] S1, multi-feature extraction and matching;

[0023] S2, the hidden parameters of line features are made explicit and short line features are merged;

[0024] S3, establish line feature matching and tracking based on geometric and appearance constraints;

[0025] S4, combining pose estimation of structured and unstructured scenes;

[0026] S5 integrates the extracted point, line, and surface features and builds a sparse map.

[0027] Further, in step S1, the point features are extracted using the ORB feature extraction algorithm based on the FAST keypoint detector and BRIEF descriptors, and the 3D points are projected onto the image. The Hamming distance between the respective descriptors is used to find the best feature matching pair.

[0028] Line feature extraction employs the LSD detection algorithm. By downsampling and Gaussian filtering the RGB image, line features are extracted. The Line Band Descriptor (LBD) algorithm is then used to obtain a descriptor for each line feature, which is used for subsequent feature matching. After obtaining the two-dimensional parameterized representation of the image frame, its normalized form is calculated:

[0029]

[0030] Among them, l obs For normalized linear representation, p start and p end These are the starting and ending coordinates of the two-dimensional line, respectively.

[0031] Planar features π are extracted from the depth image and a hierarchical clustering (AHC) method is used to perform plane matching by comparing the angle between normals and the plane distance, minimizing the plane representation.

[0032]

[0033] Where, n = (n x n y n z ) represents the unit plane normal, d is the distance from the plane to the camera origin, and φ and ψ are the azimuth and elevation angles of the plane normal.

[0034] Further, in step S2, the LSD line feature extraction algorithm uses an N-layer Gaussian pyramid to create a multi-scale representation, where the original image is downsampled N-1 times at a fixed ratio r and blurred N times using a Gaussian filter. Then, OpenCV designs an image scale parameter s∈(0,1) to scale the image at each layer, and then uses LSD to extract line features. LSD also defines a minimum density threshold d to filter out useless edge information. This invention makes these parameters explicit so that they can be set and changed, where s = 0.5 and d = 0.6 are set. The remaining parameters r, N, angle tolerance, etc., all use default values.

[0035] The short line features that appear during the detection process are determined based on the following conditions:

[0036] (1) The short line feature is detected in at least two consecutive frames;

[0037] (2) Does the included angle of the short lines meet the threshold?

[0038] (3) Whether the descriptor distance between short-line features meets the threshold.

[0039] After all the above conditions are met, the least squares method is used to merge the available short line features, and the merged line features are used for feature matching to improve the accuracy of visual tracking.

[0040] Further, in step S3, firstly, the direction angle cosine values ​​of two corresponding line segments in the reference frame and the current frame are defined:

[0041]

[0042] Among them, l j l k Let be the normalized unit vector of the line matching pair. This paper sets the cosine threshold to 0.94; if it is greater than this threshold, the match is considered valid (i.e., less than 20°).

[0043] Next, the positional information of the line segments in the image is extracted, the differences between the start and end points of the two line segments in the x and y directions are calculated, and these differences are compared with preset thresholds.

[0044]

[0045] in, These represent the x and y coordinates of the starting point of the line segment in the current frame, respectively. δ represents the x and y coordinates of the starting point of the line segment in the previous frame, respectively. width δ height These are the positional difference thresholds set based on the image size.

[0046] To obtain accurate 3D line feature location information, the Levenberg-Marquardt nonlinear algorithm is used to optimize the line features in the current frame. A Huber loss optimization function is established to minimize the impact of errors and outliers.

[0047] e lines =||m j -m' j || 2 (5)

[0048] H=argmin(∑ρ(elines (6)

[0049] Where, m j m' j Let ρ(·) be the projected line and the actual observed line, respectively, and let ρ(·) be the Huber cost function.

[0050] Further, in step S4, in the structured scene, the indoor environment is modeled as MW, and the drift-free rotation estimate R between the camera frame and MF is estimated. CM MF can be accessed through Align with the camera frame, that is:

[0051]

[0052] Among them, R MW This represents the rotation matrix from the world coordinate system to MF, obtained during MW initialization. It is the rotation matrix from MF to the (k+1)th camera frame. These two matrices are calculated using the Mean-Shift method, where the normal and normalized vanishing direction are projected onto the tangent plane of the current rotation estimate; then a mean shift is performed on the tangent plane to generate a new center, which is then back-projected onto the sphere as a new estimate.

[0053] Translation information can be fully utilized to estimate multi-dimensional features of points, lines, and surfaces, and to construct the reprojection error:

[0054]

[0055] Where, p k For three-dimensional point features P j Two-dimensional point projection in k frames P represents j Reprojection error when projected onto k frames Three-dimensional line features The normalized form of the projection on the plane, Π(·) is the projection function, e l e π These are the reprojection errors for line features and surface features, respectively. The transformation relationship between the world coordinate system and the camera coordinate system is given by q(π). C ) represents the minimized representation of the planar features in the camera coordinate system. Equation (8) is minimized to solve for the translation information:

[0056]

[0057] Where z∈{p,l,π}, ρ z (),Λ zThese are the Huber cost function and the inverse covariance matrix, respectively.

[0058] In unstructured scenarios, point, line, and surface features are directly tracked and matched with corresponding 3D features in the map to construct the reprojection error as shown in Equation (8) above. Then, the Levenberg-Marquardt algorithm is used to optimize the reprojection error:

[0059]

[0060] {R k,j ,t k,j} = argmin(e y (12)

[0061] Where y∈{p,l,π}, ρ y (·), Λ z These are the Huber cost function and the inverse covariance matrix, R. k,j t k,j These are the rotation and translation information from frame j to frame k, respectively, from which the translation and rotation estimation in the unstructured environment is obtained.

[0062] Further, in step S5, the system builds and maintains a sparse map composed of keyframes. New keyframes are added only when a sufficient number of new landmarks are observed. Using the depth map provided by the RGB-D image, new point, line, and polygon features are initialized and added from the keyframes based on the optimized map and camera trajectory for map building and updating.

[0063] Example 1:

[0064] To verify the overall performance of the geometrically constrained multi-feature visual Manhattan-SLAM method, experiments were conducted on the public datasets TUM RGB-D and ICL-NUIM. Absolute trajectory error (ATE) and root mean square error (RMSE) were used as reference metrics for comparison with other algorithms. Specific results are shown in Tables 1 and 2 and Figures 4 to 7.

[0065] Table 1. Comparison of Root Mean Square Error between this Algorithm and Other Algorithms (TUM-Datasets) (Unit: meters)

[0066]

[0067] Table 2 Comparison of Root Mean Square Error between this Algorithm and Other Algorithms (ICL-NUIM) (Unit: meters)

[0068]

[0069]

[0070] Example 1 shows that by combining line features with appearance constraints, this algorithm has significantly improved the localization accuracy compared with the current mainstream algorithms. The sequence fr3_s_nt_far (TUM dataset) was collected in an empty room and could not provide enough feature points for tracking and pose estimation. In such scenarios, line feature tracking has better advantages. The MW hypothesis fits the characteristics of such structured scenarios well, and the improved multi-feature fusion algorithm is closer to the true trajectory value.

[0071] Example 2:

[0072] To verify the improvement in runtime of the multi-feature fusion tracking method after short-line feature merging in the present invention, the average runtime of the above dataset was compared with that of current mainstream algorithms. The specific results are shown in Table 3.

[0073] Table 3 Comparison of average tracking time between this algorithm and the original algorithm (unit: seconds)

[0074]

[0075] As shown in Example 2, the present invention proposes an improved LSD feature extraction algorithm, and merges and filters out a large number of short line features that may exist during the detection process. As can be seen from the data in the table, the improved algorithm effectively shortens the average time of feature extraction and tracking process and improves detection efficiency.

Claims

1. An improved geometrically constrained multi-feature visual Manhattan-SLAM method, characterized in that, Includes the following steps: S1, multi-feature extraction and matching of points, lines, and polygons: Point features are extracted using the ORB feature extraction algorithm, which uses the Hamming distance between their respective descriptors to find the best feature matching pair; line features are extracted using the LSD detection algorithm, and the descriptor for each line feature is obtained using the Line Band Descriptor (LBD) algorithm for subsequent feature matching. After obtaining the two-dimensional parameterized representation of the image frame, its normalized form is calculated: in, To represent a normalized straight line, and These are the coordinates of the start and end points of a two-dimensional line; planar features. Extracted from depth images, using the Agglomerative Hierarchical Clustering (AHC) method, plane matching is performed by comparing the angle between normals and the plane distance; S2, line feature hiding parameters are made explicit and short line features are merged. By making the LSD hiding parameters explicit, adjusting the minimum density threshold and image scale parameters for line feature extraction, and judging a large number of short line features below the length threshold extracted by the LSD line segment detection algorithm: (1) the short line feature is detected in at least two consecutive frames; (2) whether the angle between the short lines meets the threshold; (3) whether the descriptor distance between the short line features meets the threshold; then, the detected short line features are merged according to the judgment conditions, and the available short line features are merged using the least squares method, and the merged line features are used for feature matching to improve the accuracy of visual tracking; S3, tracking method based on geometric and appearance constraints. For the direction and position information of line features, geometric and appearance constraints are used to establish line feature matching tracking, and the direction angle cosine values ​​of two corresponding line segments in the reference frame and the current frame are defined: in, 、 For the normalized unit vector of the line matching pair, this paper sets the cosine threshold to 0.

94. If it is greater than this threshold, the match is considered valid (i.e., less than 20°). Next, the position information of the line segments in the image is extracted, the difference between the start and end points of the two line segments in the x and y directions is calculated, and then compared with the set thresholds respectively. in, These represent the x and y coordinates of the starting point of the line segment in the current frame, respectively. These represent the x and y coordinates of the starting point of the line segment in the previous frame. S4, S5, S6, S7, S8, S9, S10, S2, S3, S4, S5, S6, S7, S8, S9, S10, S11, S2, S3, S4, S5, S6, S7, S8, S9, S11, S2, S3, S4, S12, S3, S4, S13, S4, S5, S6, S7, S8, S9, S12, S3, S4, S13, S4, S14, S14, S15, S6, S16, S7, S17, S8, S18, S19, S12, S3, S13, S4, S14, S15, S16, S17, S18, S19, S14, S16, S17, S18, S19, S16, S17, S18, S19, S16, 2. The improved geometrically constrained multi-feature visual Manhattan-SLAM method according to claim 1, characterized in that, In S4, within the structured scene, the indoor environment is modeled as MW, and a drift-free rotation estimate between the camera frame and MF is estimated. MF can be accessed through Align with the camera frame, that is: in, This represents the rotation matrix from the world coordinate system to MF, obtained during MW initialization. It is the rotation matrix from MF to the (k+1)th camera frame; the translation information can be estimated by making full use of the multi-dimensional features of points, lines, and surfaces to construct the reprojection error: in, 3D point features Two-dimensional point projection in k frames express Reprojection error when projected onto k frames Three-dimensional line features The normalized form of a projection onto a plane. For projection function, 、 These are the reprojection errors for line features and surface features, respectively. This defines the transformation relationship between the world coordinate system and the camera coordinate system. To minimize the planar features in the camera coordinate system, equation (5) is minimized to solve for the translation information: in, ， 、 The Huber cost function and the inverse covariance matrix are respectively. In unstructured scenarios, point, line and surface features are directly tracked and matched with the corresponding three-dimensional features in the map to construct the reprojection error as shown in Equation (7) above. Then, the Levenberg-Marquardt algorithm is used to optimize the reprojection error.

3. The improved geometrically constrained multi-feature visual Manhattan-SLAM method according to claim 1, characterized in that, In S5, using the depth map provided by the RGB-D image, new point, line, and surface features are initialized and added from the keyframes based on the optimized map and camera trajectory to build and update the map.

Images