Visual inertia SLAM method and system based on point-line fusion

Abstract

The invention discloses a visual inertia SLAM (Simultaneous Localization and Mapping) method and system based on point-line fusion, relates to the technical field of image processing, and solves the problem that stable and distinctive features cannot be effectively selected when a current SLAM method processes multi-line features. According to the scheme, point feature extraction and tracking are carried out on visual data based on a tracking algorithm combining forward and reverse optical flows, and line feature selection and optimization are carried out on the visual data based on a line segment detection model after line feature selection and homogenization optimization; marginalizing the visual data and the inertial data based on a sliding window model and converting the visual data and the inertial data into prior information, and improving marginalization steps based on a distributed optimization strategy; special feature detection and matching are set during data preprocessing, marginalized residual processing sliding window optimization is set during back-end nonlinear optimization, the stability and accuracy of point-line features are enhanced, and the operation efficiency of the system is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of image processing, and more specifically, to a visual inertial SLAM method and system based on point-line fusion. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is a key technology used to enable devices such as robots or self-driving cars to navigate in unknown environments. It involves the use of various sensors such as lidar, cameras, IMU (inertial measurement unit), etc. to collect environmental data, and process these data through algorithms to determine the location of the device and build an environmental map. Visual-Inertial SLAM (VI-SLAM) is a SLAM technology that combines visual information and inertial measurement unit (IMU) data. It uses visual information captured by the camera and acceleration and angular velocity data provided by the IMU to achieve precise positioning and mapping of intelligent bodies such as robots or unmanned vehicles.

[0003] In recent years, with the rapid development of artificial intelligence and robotics, SLAM technology, as a key component, has attracted increasing attention. SLAM technology enables robots to autonomously navigate, build maps, and achieve precise positioning in unknown environments, providing strong support for the autonomy and intelligence of robots and unmanned vehicles. In SLAM technology, because line features have richer geometric information, they can make up for the poor performance of point features alone in scenes such as low texture and motion blur, making the method based on the combination of point and line features gradually become the mainstream. However, this method requires a large amount of extraction and matching calculations, and the real-time performance of the SLAM system will be challenged, especially when too many line features are extracted, it becomes particularly difficult to select stable and distinguishing features.

[0004] Therefore, the present application provides a visual-inertial SLAM method and system based on point-line fusion to solve the above problems. Summary of the invention

[0005] The purpose of this application is to provide a visual-inertial SLAM method and system based on point-line fusion, so as to solve the problem that the current SLAM method cannot effectively select stable and distinctive features when processing multiple line features, resulting in high system calculation complexity, low optical flow tracking accuracy and poor real-time performance; this application enhances the stability and accuracy of point-line features and improves the operating efficiency of the system by setting special feature detection and matching during data preprocessing and setting marginalized residual processing sliding window optimization during back-end nonlinear optimization.

[0006] The present application first provides a visual-inertial SLAM method based on point-line fusion, including: S1, obtaining visual data of a camera and inertial data of an IMU, preprocessing the visual data and inertial data, including: feature detection and matching of the visual data, and IMU pre-integration of the inertial data; S2, performing visual-inertial joint initialization based on the preprocessed visual data and inertial data, including: visual initialization and visual-inertial alignment to obtain initial parameters; S3, performing back-end nonlinear optimization of an improved sliding window based on the initial parameters; S4, performing closed-loop detection and global Pose optimization, including: performing loop detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a keyframe database; wherein, feature detection and matching of visual data include: extracting and tracking point features of visual data based on a tracking algorithm combining forward and reverse optical flows, and selecting and optimizing line features of visual data based on a line segment detection model after line feature selection and homogenization optimization; improving the back-end nonlinear optimization of the sliding window based on initial parameters include: marginalizing visual data and inertial data respectively based on the sliding window model and converting them into prior information, and improving the marginalization step based on a distributed optimization strategy.

[0007] In a possible implementation, a tracking algorithm based on a combination of forward and reverse optical flows is used to extract and track point features of visual data, including: extracting feature points in the current frame image based on a feature point detection algorithm; converting the solution to the minimum difference between the current frame and the target frame into an objective function optimization based on the grayscale invariance assumption; constructing a Jacobian matrix based on the objective function; expanding a point to a point set near the point based on the Gauss-Newton method; iteratively calculating the position difference of feature points in consecutive frames based on the forward optical flow algorithm, and in the iterative process, reversely compensating the position of feature points in the current frame based on the reverse optical flow algorithm, and updating the Jacobian matrix.

[0008] In a possible implementation, line feature selection and optimization are performed on visual data based on a line segment detection model after line feature selection and homogenization optimization, including: constructing a line feature set through a line feature extraction algorithm, and traversing all elements in the line feature set through a line feature iterator; line feature selection: if the number of triangulated frames of the line feature is greater than or equal to a threshold, or the line feature is not triangulated in the current frame, then skip this frame, otherwise count the observation frames of the line feature and store the observation data, and filter out line features with endpoint distances less than a threshold based on the endpoint coordinates of the line feature; line feature optimization: calculate the normal vector of the line feature based on the start and end points of the line feature, calculate the distance from the start and end points of another line feature to the normal vector, analyze whether the distance is less than a distance threshold, calculate the direction vector and the angle between the direction vectors of two line segments, analyze whether the angle between the direction vectors is less than an angle threshold, and homogenize the line features based on the distance threshold and the angle threshold.

[0009] In a possible implementation, visual data and inertial data are marginalized and converted into prior information based on a sliding window model, including: based on a sliding window model, visual data and inertial data are marginalized and converted into prior information while marginalizing a previous data frame.

[0010] In one possible implementation, the marginalization step is improved based on a distributed optimization strategy, including: converting the earliest frame and its associated inertial data and observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

[0011] The present application also provides a visual inertial SLAM system based on point-line fusion, including: a data preprocessing unit, used to obtain visual data from a camera and inertial data from an IMU, preprocessing the visual data and inertial data, including: feature detection and matching of the visual data, IMU pre-integration of the inertial data, feature detection and matching of the visual data, including: point feature extraction and tracking of the visual data based on a tracking algorithm combining forward and reverse optical flows, line feature selection and optimization of the visual data based on a line segment detection model after line feature selection and homogenization optimization; an initialization unit, used to perform visual feature matching based on the preprocessed visual data and inertial data Inertial joint initialization, including: visual initialization and visual-inertial alignment, to obtain initial parameters; back-end nonlinear optimization unit, used to improve the back-end nonlinear optimization of the sliding window based on the initial parameters, the back-end nonlinear optimization of the sliding window based on the initial parameters includes: marginalizing the visual data and inertial data respectively based on the sliding window model and converting them into prior information, and improving the marginalization step based on the distributed optimization strategy; closed-loop detection and global pose optimization unit, used to perform closed-loop detection and global pose optimization based on the results of back-end nonlinear optimization, including: performing loop detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a key frame database.

[0012] In a possible implementation, the initialization unit includes a feature point extraction optimization unit, which is used to extract feature points in the current frame image based on a feature point detection algorithm; based on the grayscale invariance assumption, convert the solution to the minimum difference between the current frame and the target frame into an objective function optimization; construct a Jacobian matrix based on the objective function; expand the point to a point set near the point based on the Gauss-Newton method; iteratively calculate the position difference of the feature points in consecutive frames based on the forward optical flow algorithm, and in the iterative process, reversely compensate for the position of the feature points in the current frame based on the reverse optical flow algorithm, and update the Jacobian matrix.

[0013] In a possible implementation, the initialization unit includes a feature line extraction optimization unit, which is used to construct a line feature set through a line feature extraction algorithm, and traverse all elements in the line feature set through a line feature iterator; line feature selection: if the number of triangulated frames of the line feature is greater than or equal to a threshold, or the line feature is not triangulated in the current frame, then this frame is skipped, otherwise the observation frames of the line feature are counted and the observation data is stored, and the line features with endpoint distances less than a threshold are screened out based on the endpoint coordinates of the line feature; line feature optimization: the normal vector of the line feature is calculated using the start and end points of the line feature, the distance from the start and end points of another line feature to the normal vector is calculated, and whether the distance is less than a distance threshold is analyzed, the direction vectors and the angle between the direction vectors of two line segments are calculated, and whether the angle between the direction vectors is less than an angle threshold is analyzed, and the line features are homogenized based on the distance threshold and the angle threshold.

[0014] In a possible implementation, the back-end nonlinear optimization unit includes a sliding window model optimization unit for marginalizing and converting the visual data and the inertial data into prior information respectively while marginalizing the previous data frame based on the sliding window model.

[0015] In one possible implementation, the back-end nonlinear optimization unit includes a marginalization optimization unit for converting the earliest frame and its associated inertial data and observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

[0016] Compared with the prior art, this application has the following beneficial effects:

[0017] In terms of data preprocessing, this application designs an improved optical flow tracking method based on the grayscale invariance assumption and spatial consistency, and optimizes the traditional line segment detection model through line feature selection and homogenization methods; first, the reverse optical flow algorithm is introduced into the traditional object estimation method to solve the problem of inaccurate forward optical flow estimation under occlusion and light change environments, and the feature tracking stability and the accuracy of pose estimation are improved by estimating the displacement of feature points; secondly, the line feature extraction methods such as edge extraction, line feature selection and homogenization are optimized to solve the problem of poor line feature quality;

[0018] In terms of back-end nonlinear optimization, this application marginalizes the visual and inertial information separately, eliminates unknown variables and generates marginalization factors, and then merges the marginalization results of the two, effectively improving the overall operating efficiency and positioning accuracy of the system, and solving the problem that the current SLAM method cannot effectively select stable and distinctive features when processing multi-line features, resulting in high system calculation complexity, low optical flow tracking accuracy and poor real-time performance. It also adopts a distributed optimization strategy to improve the marginalization step, reduce the difficulty of system optimization, and improve the efficiency and accuracy of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The drawings described herein are used to provide a further understanding of the embodiments of the present invention, constitute a part of this application, and do not constitute a limitation of the embodiments of the present invention. In the drawings:

[0020] Figure 1 A network structure diagram of a visual-inertial SLAM method based on point-line fusion provided in an embodiment of the present application;

[0021] Figure 2 Schematic diagram of optical flow tracking;

[0022] Figure 3 This is a comparison chart of the effects of forward optical flow and reverse optical flow tracking;

[0023] Figure 4 This is a comparison chart of the effects of different line feature extraction methods;

[0024] Figure 5 Optimize the distribution of visual and inertial data;

[0025] Figure 6 Comparison of the Schur complement matrix constructed before and after marginalization improvement;

[0026] Figure 7 This is a comparison chart of ATE distribution between this method and the VINS-Mono and PL-VINS methods in the MH_05 sequence under the EuRoC dataset. DETAILED DESCRIPTION

[0027] Hereinafter, the term "include" or "may include" used in various embodiments of the present application indicates the presence of the function, operation or element applied for, and does not limit the increase of one or more functions, operations or elements. In addition, as used in various embodiments of the present application, the terms "include", "have" and their cognates are intended only to indicate specific features, numbers, steps, operations, elements, components or a combination of the foregoing items, and should not be understood as first excluding the presence of one or more other features, numbers, steps, operations, elements, components or a combination of the foregoing items or the possibility of increasing one or more features, numbers, steps, operations, elements, components or a combination of the foregoing items.

[0028] The terms used in the various embodiments of the application are only used to describe the purpose of specific embodiments and are not intended to limit the various embodiments of the application. As used herein, the singular form is intended to also include the plural form, unless the context clearly indicates otherwise. Unless otherwise limited, all terms used here (including technical terms and scientific terms) have the same meaning as the meanings commonly understood by ordinary technicians in the field of the various embodiments of the application. Terms (such as terms defined in generally used dictionaries) will be interpreted as having the same meaning as the contextual meaning in the relevant technical field and will not be interpreted as having idealized meanings or too formal meanings, unless clearly defined in the various embodiments of the application.

[0029] In order to make the objectives, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with examples and drawings. The illustrative implementation scheme of the present application and its description are only used to explain the present application and are not intended to limit the present application.

[0030] See also Figure 1 As shown, Figure 1 A network structure diagram of a visual-inertial SLAM method based on point-line fusion provided in an embodiment of the present application. In order to solve the problem that the current SLAM method cannot effectively select stable and distinguishing features when processing multiple line features, resulting in high system calculation complexity, low optical flow tracking accuracy and poor real-time performance, the present application provides a visual-inertial SLAM method based on point-line fusion, and the colored parts are the improvements of the present application. The method includes: S1, obtaining visual data of a camera and inertial data of an IMU, preprocessing the visual data and inertial data, including: feature detection and matching of the visual data, and IMU pre-integration of the inertial data; S2, performing visual-inertial joint initialization based on the preprocessed visual data and inertial data, including: visual initialization and visual-inertial alignment, and obtaining initial parameters; S3, performing back-end nonlinear optimization of an improved sliding window based on the initial parameters; S4, performing closed-loop detection and global pose optimization based on the result of the back-end nonlinear optimization, including: performing loop detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a keyframe database; wherein, feature detection and matching of the visual data includes: extracting and tracking point features of the visual data based on a tracking algorithm combining forward and reverse optical flows, and selecting and optimizing line features of the visual data based on a line segment detection model after line feature selection and homogenization optimization; improving the back-end nonlinear optimization of the sliding window based on the initial parameters includes: marginalizing and converting the visual data and the inertial data into prior information based on a sliding window model, and improving the marginalization step based on a distributed optimization strategy.

[0031] The improvement of this application is that, in view of the high system calculation complexity, low optical flow tracking accuracy and poor real-time performance of the current SLAM method when dealing with multiple line features, a visual inertial SLAM method based on point-line fusion is designed based on the grayscale invariance assumption and spatial consistency, the optical flow tracking method is improved, and the existing line segment detection model and back-end data processing model are improved to improve the overall system operation efficiency and positioning accuracy. Specifically including: Improvement of the line segment detection model: adding a reverse optical flow algorithm on the basis of the forward optical flow to improve the robustness of the system; improvement of the back-end data processing model: on the basis of the original sliding window model to marginalize the previous data frame, the visual and inertial information are marginalized separately. This operation can separate different types of information, avoid mutual interference during the optimization process, and improve the efficiency and accuracy of the system.

[0032] In a possible implementation, a tracking algorithm based on a combination of forward and reverse optical flows is used to extract and track point features of visual data, including: extracting feature points in the current frame image based on a feature point detection algorithm; converting the solution to the minimum difference between the current frame and the target frame into an objective function optimization based on the grayscale invariance assumption; constructing a Jacobian matrix based on the objective function; expanding a point to a point set near the point based on the Gauss-Newton method; iteratively calculating the position difference of feature points in consecutive frames based on the forward optical flow algorithm, and in the iterative process, reversely compensating the position of feature points in the current frame based on the reverse optical flow algorithm, and updating the Jacobian matrix.

[0033] Specifically, a reverse optical flow algorithm is added on the basis of forward optical flow to improve the robustness of the system. After the visual data (image) is input, in order to minimize the difference between the current frame and the target frame, it is abstracted as an optimization objective function:

[0034] f(dx,dy)=I(x,y)-T(x+dx,y+dy)

[0035] Among them, I(x,y) represents the gray value of (x,y) at time point t+1, T(x+dx,y+dy) represents the gray value of (x+dx,y+dy) at time point t, and dx and dy are displacement parameters; thus, the Jacobian matrix of f(dx,dy) can be obtained:

[0036]

[0037] Using the Gauss-Newton method, expand to the point set near this point:

[0038]

[0039] During the iteration of the forward optical flow, the feature difference keeps changing, and the Jacobian matrix J needs to be recalculated each time. Therefore, the reverse optical flow is introduced here. It is known that the transformation amount of data A before each iteration is p, and the reverse optical flow is to find the transformation amount q of data B, so that the difference between the transformed q(B) and the current p(A) is minimized, and the optimization objective function is:

[0040] f(q)=W(q(B))-W(p(A))

[0041] The Jacobian matrix of f(q) is:

[0042]

[0043] From q = 0; q (B) = B; And W(), p(), and A are all independent of q.

[0044] The Jacobian matrix of f(q) can be simplified to:

[0045]

[0046] The q-transform increment for the current iteration:

[0047] Δq=-(J T J) -1 J T (W(B)-W(p(A)))

[0048] Optical flow tracking can be used to estimate the distance between two consecutive frames. The method of inferring motion vectors by analyzing the motion changes at the pixel level is simple and intuitive. Figure 2 Schematic diagram of optical flow tracking shown.

[0049] However, when an object moves quickly or undergoes non-rigid deformation, the accuracy of optical flow tracking may decrease, resulting in inaccurate tracking results. To solve this problem, this application introduces reverse optical flow technology, which predicts the position of feature points in the current frame through reverse compensation to enhance the stability and accuracy of feature points.

[0050] Use optical flow vectors to match feature points and improve the robustness of feature matching. Forward optical flow and reverse optical flow provide different angles and information to estimate the motion of an object. By combining the results of the two, we can overcome their respective limitations and obtain more accurate motion estimation. This method of fusing the two estimation results helps reduce errors and improve overall accuracy. Figure 3 As shown, Figure 3 This is a comparison chart of the effects of forward optical flow and reverse optical flow tracking. Figure 3 (a) is the forward optical flow tracking effect. Figure 3 (b) is the reverse optical flow tracking effect.

[0051] In a possible implementation, line feature selection and optimization are performed on visual data based on a line segment detection model after line feature selection and homogenization optimization, including: constructing a line feature set through a line feature extraction algorithm, and traversing all elements in the line feature set through a line feature iterator; line feature selection: if the number of triangulated frames of the line feature is greater than or equal to a threshold, or the line feature is not triangulated in the current frame, then skip this frame, otherwise count the observation frames of the line feature and store the observation data, and filter out line features with endpoint distances less than a threshold based on the endpoint coordinates of the line feature; line feature optimization: calculate the normal vector of the line feature based on the start and end points of the line feature, calculate the distance from the start and end points of another line feature to the normal vector, analyze whether the distance is less than a distance threshold, calculate the direction vector and the angle between the direction vectors of two line segments, analyze whether the angle between the direction vectors is less than an angle threshold, and homogenize the line features based on the distance threshold and the angle threshold.

[0052] Specifically, in the online feature optimization part, the main work is divided into two parts: line feature selection and line feature homogenization. In the process of online feature selection, all elements in the line feature set are traversed through the line feature iterator. If the number of triangulated frames of the feature reaches or exceeds the set threshold, or the triangulation fails on the current frame, the frame is skipped. Otherwise, the observation frames of the current line feature are counted and the observation data is stored; at the same time, the endpoint coordinates of the line segment are added, and the distance between the endpoints is calculated to avoid extracting particularly short line features. Through the line feature extractor, useful line segments are screened out to reduce unnecessary information and noise.

[0053] After completing the line feature selection, we homogenize it to make its distribution more uniform. Line feature homogenization is mainly based on the following two judgment criteria: 1. Using the starting point and end point of the straight line as the basis, the vector obtained by cross multiplication is used as the normal vector of the straight line, and then the distance from the starting point and end point of the other line to the above normal vector is calculated to check whether it is less than the preset threshold. This shortest distance can measure the relative position and proximity of the two lines; 2. Calculate the direction vectors of the two line segments, and further calculate the angle between them. This angle is used to judge the similarity between the two vectors. If the angle is close to 0 degrees, it means that the two vectors are very close and have almost the same direction. Otherwise, the directions are opposite. For the convenience of comparison, the dot product and norm of the vector are converted into angles. When the angle is less than the preset threshold, the two line segments can be considered to be collinear. Through the two judgment criteria, dense and redundant line features are reduced.

[0054] like Figure 4 As shown in the figure, the difference between this method and the PL-VINS method in optimizing offline features in the same scenario is shown. Figure 4The PL-VINS method shown in (a) extracts too many line features from the image, resulting in information redundancy that increases the computational burden of tracking and optimization; however, the line features proposed by this method are fewer in number and of better quality, and longer and more continuous line segments can provide a more complete and accurate shape description. It can be seen that this application can effectively even out the distribution of line features, reduce redundancy, and improve the overall tracking and optimization effect.

[0055] In a possible implementation, visual data and inertial data are marginalized and converted into prior information based on a sliding window model, including: based on a sliding window model, visual data and inertial data are marginalized and converted into prior information while marginalizing a previous data frame.

[0056] Specifically, the sliding window model is used as a basis for improvement, and then used for backend optimization. Based on the marginalization of the previous data frame by the original sliding window model, the visual data and inertial data are marginalized separately and converted into prior information.

[0057] According to the marginalization strategy, the edge information is formed and its residual is taken into consideration. The Schur complement method is used. The relevant matrix contains visual constraints and inertial (IMU) constraints. The corresponding Jacobian matrix (H matrix) is calculated and the prior residual is continuously updated until the Jacobian matrix is ​​stable and no longer changes. Subsequently, the constructed H matrix is ​​marginalized and the prior information is formed as the basis for the optimization process.

[0058] The variables that need to be optimized here are: para_Pose (6 dimensions, camera pose), para_SpeedBias (9 dimensions, camera speed, acceleration bias, angular velocity bias), para_Ex_Pose (6 dimensions, camera IMU external parameters), para_Feature (1 dimension, feature point depth), para_Td (1 dimension, calibration synchronization time). Due to the different data volumes of the above variables to be optimized, putting all state quantities into a Schur complement matrix for optimization will result in a waste of resources. In order to solve the above problems, the marginalization method is improved by adopting a distributed optimization strategy.

[0059] In one possible implementation, the marginalization step is improved based on a distributed optimization strategy, including: converting the earliest frame and its associated inertial data and observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

[0060] Specifically, marginalization operations such as Figure 5 As shown in Figure 2, the steps of variable marginalization and prior information formation are shown in detail. This strategy transforms the earliest frame along with the associated IMU data and observed landmark point information into prior information, which is then incorporated into the optimization objective function.

[0061] Assuming that the earliest observed frame is k landmark points, the total state variable is expressed as follows:

[0062] pos=2×sp 0:1 +11×p 0:10 +1×T bc +1×t b +k×f 1:k

[0063] Where sp corresponds to the camera speed, acceleration, angular velocity bias para_SpeedBias, and p corresponds to the camera pose para_Pose. The variables that need to be marginalized can be rewritten as:

[0064] m=1×p0+1×sp0+k×f 1:k

[0065] First marginalize sp and f to form a new H matrix and then marginalize p.

[0066] The comparison of the Schur complement matrix before and after improvement is as follows: Figure 6 Through the above operation, different types of information can be separated to avoid mutual interference during the optimization process, thereby improving the efficiency and accuracy of the system; and after separation, the difficulty of system optimization is reduced, and the efficiency and real-time performance of the system are improved.

[0067] It can be understood that the visual-inertial SLAM method based on point-line fusion proposed in this embodiment, in terms of data preprocessing, designs an improved optical flow tracking method based on the grayscale invariance assumption and spatial consistency, and optimizes the traditional line segment detection model through line feature selection and homogenization methods; first, the reverse optical flow algorithm is introduced into the traditional object estimation method to solve the problem of inaccurate forward optical flow estimation in occlusion and light changing environments, and improves the feature tracking stability and the accuracy of pose estimation by estimating the displacement of feature points; secondly, the line feature extraction methods such as edge extraction, line feature selection and homogenization are optimized to solve the problem of poor line feature quality. In terms of back-end nonlinear optimization, this application marginalizes the visual and inertial information separately, eliminates unknown variables and generates marginalization factors, and then merges the marginalization results of the two, effectively improving the overall operating efficiency and positioning accuracy of the system, and solving the problem that the current SLAM method cannot effectively select stable and distinctive features when processing multi-line features, resulting in high system calculation complexity, low optical flow tracking accuracy and poor real-time performance. It also adopts a distributed optimization strategy to improve the marginalization step, reduce the difficulty of system optimization, and improve the efficiency and accuracy of the system.

[0068] In order to verify the effectiveness of the method provided in the application example, the present application example also combines Figure 6Tables 1-2 provide specific experimental verifications, and the performance of the proposed method is evaluated on the EuRoC dataset. The experiments show that compared with other SLAM methods, this method has achieved the best results in evaluation indicators such as positioning accuracy (root mean square error RMSE of absolute trajectory error ATE) and running time (ms). This effectively proves the rationality and superiority of this method.

[0069] Table 1 Comparison of ATE RMSE of different algorithms under EuRoC dataset

[0070]

[0071] Table 2 Average running time comparison (ms)

[0072]

[0073] *“--------” VINS-Mono and VINS-Fusion are visual-inertial SLAM algorithms based on feature points, so they do not include the extraction and tracking of line features.

[0074] In Table 1, this method is compared with VINS-Mono, PL-VINS, Line-Based and other methods, and good results are achieved under the ATE RMSE indicator (bold font indicates the best result). It can be found that except for the performance under the V203 sequence, the algorithm shows excellent performance in other sequences. This is because the homogenization of line features reduces the number of line features available in the matching process. When the object moves faster, the line features extracted from the current frame are difficult to effectively match with the line features in the subsequent frames, which leads to a higher root mean square error in the V203 sequence. In addition, without closed-loop constraints, the algorithm is improved by 36.6% compared with VINS-Mono, 29.3% compared with PL-VINS, and 11.8% relative to the latest Line-Based.

[0075] In Table 2, the average running time of each thread of this method is compared with that of VINS-Mono, VINS-Fusion, and PL-VINS on the complete sequence of the EuRoc dataset. It can be found that compared with the comparison methods, the running time of this method on each thread is basically the same as that of the other comparison methods, but combined with the excellent positioning accuracy shown by this method in Table 1, it is enough to see that this method has excellent computational efficiency. Specifically, compared with the VINS-Mono and VINS-Fusion methods that only use point features for tracking, this method and the PL-VINS method spend more time on the Local VIO part due to the addition of line feature extraction and matching modules; in addition, compared with PL-VINS, the accuracy of feature tracking is improved by introducing the reverse optical flow module, which increases some time consumption in the point feature tracking process. However, through the line feature homogenization module, while optimizing the performance of line feature tracking, the time cost of calculation is also reduced.

[0076] exist Figure 7 middle, Figure 7 The ATE distribution comparison diagram of this method, VINS-Mono and PL-VINS methods in the MH_05 sequence under the EuRoC dataset is shown in Figure 2. Figure 7 The curve symmetry of (c) is obviously better than Figure 7 (a) and Figure 7 (b) and Figure 7 The curve distribution range of (c) is smaller, which indicates that the method has smaller systematic deviation and better stability; in addition, Figure 7 (a) std (standard deviation) compared to Figure 7 (a) and Figure 7 (b) is narrower, which indicates that the estimation accuracy of this method is higher. It can be seen that this method has more reliable performance.

[0077] In order for those skilled in the art to clearly understand the complete process of the embodiment method, the embodiment of the present application also provides specific examples to illustrate the complete process of the SLAM method.

[0078] Set basic parameters: Regarding line feature selection and homogenization, a custom line feature extractor is used to process the input image and output line feature information. The number of sampled line segments is set to the default value of 8; the minimum number of line segment feature extraction is set to the default value: 150. If the extension angle of two lines is within degrees and the distance between the endpoints of the two short lines is less than 0.1, a fusion judgment is made. To prevent the distribution of line segments from being too dense, the distance between line segments is set to 15 pixels. The extracted line segments are selected and homogenized through the above operations. Ensure that sufficient line features can be obtained even in areas with weak textures.

[0079] This method mainly includes the following key tasks: 1. When detecting features of the input image, the feature tracking part introduces the reverse optical flow method, predicts the position of the feature points in the current frame through reverse compensation, screens the feature points, and enhances the stability and accuracy of the point and line features; 2. Selects the line features and optimizes them after the selection is completed to improve the stability and accuracy of the overall tracking; 3. When optimizing the back-end data, the sliding window model is used as the basis, and the marginalization operation is added to improve the operating efficiency of the system. The specific implementation steps are as follows.

[0080] Step 1: Input the visual data (data frame image) collected by the camera and the inertial data (IMU data) collected by the IMU. Initialize the two types of input data separately, and the IMU data is processed using the commonly used pre-integration method. The preprocessing of the data frame image can be divided into two parts: point and line feature extraction and tracking, and line segment selection and optimization. When performing point feature extraction and tracking, after the data frame image is input, the initial feature point data is first obtained using the FAST corner point, and then optimized. In order to minimize the difference between the current frame and the target frame, based on the grayscale invariance assumption, this problem can be abstracted into the following objective function:

[0081] f(dx,dy)=I(x,y)-T(x+dx,y+dy)

[0082] The grayscale value of the point at time t is expressed as T(x+dx,y+dy).

[0083] Based on the above formula, the Jacobian matrix of f(dx,dy) can be obtained:

[0084]

[0085] Using the Gauss-Newton method, expand to the point set near this point:

[0086]

[0087] During the forward optical flow iteration, the feature difference keeps changing, and the Jacobian matrix J needs to be recalculated each time.

[0088] Therefore, when performing point feature extraction and tracking, the reverse optical flow method is introduced on the basis of the forward optical flow method. It is known that the transformation amount of A before each iteration is p, and the reverse optical flow is to obtain the transformation amount q of the data B, so that the difference between the transformed q(B) and the current p(A) is minimized. After the reverse optical flow is introduced, the optimization objective function that minimizes the difference between the current frame and the target frame becomes the following formula:

[0089] f(q)=W(q(B))-W(p(A))

[0090] The Jacobian matrix of f(q) in the above formula is:

[0091]

[0092] Where q = 0; q(B) = B; Moreover, W(); p(), A are all independent of q.

[0093] Then J(q) in the above formula can be simplified as:

[0094]

[0095] In the current iteration, the transformation increment of the introduced reverse optical flow is:

[0096] Δq=-(J T J) -1 J T (W(B)-W(p(A)))

[0097] By introducing the reverse optical flow technology, the position of the feature points in the current frame is predicted through reverse compensation, which enhances the stability and accuracy of the feature points. The forward optical flow and the reverse optical flow provide different angles and information to estimate the motion of the object. By combining the two, their respective limitations can be overcome and more accurate motion estimation can be obtained. Through the above operations, the selection of feature points is completed.

[0098] In the online feature selection and optimization part, the LSD algorithm is first used to obtain the initial line feature information. Then all elements in the line feature set are traversed through the line feature iterator. If the triangulated frame of the line feature is greater than or equal to the window size minus 1, or the feature is not triangulated, skip this frame. Otherwise, count the observation frames of the current line feature and store the observation data. At the same time, add the endpoint coordinates of the line segment and calculate the distance between the endpoints to avoid extracting particularly short line features. Through the line feature extractor, useful line segments are screened out to reduce unnecessary information and noise. After completing the line feature selection, it is homogenized to make its distribution more uniform. Line feature homogenization is mainly based on the following two judgment criteria:

[0099] Criterion 1: Use the starting point and end point of a line as the basis, and use the vector obtained by cross product as the normal vector of the line. Then calculate the distance from the starting point and end point of another line to the normal vector to check whether it is less than the preset 35.0 / 460.0. This shortest distance can measure the relative position and proximity of the two lines.

[0100] Criterion 2: Calculate the direction vectors of the two line segments and further calculate the angle between them. This angle is used to judge the similarity between the two vectors. If the angle is close to 0 degrees, it means that the two vectors are very close and have almost the same direction, otherwise they are in opposite directions. For the convenience of comparison, the dot product and norm of the vectors are converted into angles. When the angle is less than the preset 3.5 degrees, it can be considered that the two line segments are collinear.

[0101] Step 2: After completing the above operations, the processed camera data frame image data and IMU pre-integrated data are fused and initialized. After initialization, the back-end data is associated and optimized.

[0102] Step 3: In the process of associating and optimizing the backend data, based on the sliding window model, the visual data and inertial data are marginalized separately on the basis of marginalizing only the previous data frame, and both are converted into prior information. Then, the distribution optimization strategy is adopted to convert the earliest frame and the associated IMU data and observed landmark information into prior information. The variables that need to be optimized here are: para_Pose (6 dimensions, camera pose), para_SpeedBias (9 dimensions, camera speed, acceleration bias, angular velocity bias), para_Ex_Pose (6 dimensions, camera IMU external parameters), para_Feature (1 dimension, feature point depth), para_Td (1 dimension, calibration synchronization time). Due to the different data volumes of these variables to be optimized, putting all state quantities into a Schur complement matrix for optimization will result in a waste of resources.

[0103] In order to solve the above problems, the marginalization method is improved by adopting a distributed optimization strategy. Figure 5 As shown in Figure 2, the steps of variable marginalization and prior information formation are shown in detail. This strategy converts the earliest frame and the associated IMU data and observed landmark information into prior information, which is then incorporated into the overall optimization objective function. Assuming that the earliest observed frame is k landmarks, the total state variable is expressed as follows:

[0104] pos=2×sp 0:1 +11×p 0:10 +1×T bc +1×t b +k×f 1:k

[0105] Among them, sp corresponds to the camera speed, acceleration, angular velocity bias para_SpeedBias, and p corresponds to the camera pose para_Pose.

[0106] The variables that need to be marginalized can be rewritten as:

[0107] m=1×p0+1×sp0+k×f 1:k

[0108] First marginalize sp and f to form a new H matrix and then marginalize p. Through the above operation, different types of information can be separated to avoid mutual interference during the optimization process, thereby improving the efficiency and accuracy of the system; and after separation, the difficulty of system optimization is reduced, and the efficiency and real-time performance of the system are improved.

[0109] Step 4: After completing all the above operations, estimate the global pose information of the target object and perform closed-loop detection.

[0110] See also Figure 1 As shown, the embodiment of the present application also provides a visual inertial SLAM system based on point-line fusion, including: a data preprocessing unit, used to obtain visual data from a camera and inertial data from an IMU, preprocess the visual data and inertial data, including: feature detection and matching of the visual data, IMU pre-integration of the inertial data, feature detection and matching of the visual data including: point feature extraction and tracking of the visual data based on a tracking algorithm combining forward and reverse optical flows, line feature selection and optimization of the visual data based on a line segment detection model after line feature selection and homogenization optimization; an initialization unit, used to perform initialization based on the preprocessed visual data and inertial data Perform visual-inertial joint initialization, including: visual initialization and visual-inertial alignment to obtain initial parameters; a back-end nonlinear optimization unit, used to improve the back-end nonlinear optimization of the sliding window based on the initial parameters, and the back-end nonlinear optimization of the sliding window based on the initial parameters includes: marginalizing the visual data and the inertial data respectively based on the sliding window model and converting them into prior information, and improving the marginalization step based on the distributed optimization strategy; a closed-loop detection and global pose optimization unit, used to perform closed-loop detection and global pose optimization based on the results of the back-end nonlinear optimization, including: performing loop detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a key frame database.

[0111] In a possible implementation, the initialization unit includes a feature point extraction optimization unit, which is used to extract feature points in the current frame image based on a feature point detection algorithm; based on the grayscale invariance assumption, convert the solution to the minimum difference between the current frame and the target frame into an objective function optimization; construct a Jacobian matrix based on the objective function; expand the point to a point set near the point based on the Gauss-Newton method; iteratively calculate the position difference of the feature points in consecutive frames based on the forward optical flow algorithm, and in the iterative process, reversely compensate for the position of the feature points in the current frame based on the reverse optical flow algorithm, and update the Jacobian matrix.

[0112] In a possible implementation, the initialization unit includes a feature line extraction optimization unit, which is used to construct a line feature set through a line feature extraction algorithm, and traverse all elements in the line feature set through a line feature iterator; line feature selection: if the number of triangulated frames of the line feature is greater than or equal to a threshold, or the line feature is not triangulated in the current frame, then this frame is skipped, otherwise the observation frames of the line feature are counted and the observation data is stored, and the line features with endpoint distances less than a threshold are screened out based on the endpoint coordinates of the line feature; line feature optimization: the normal vector of the line feature is calculated using the start and end points of the line feature, the distance from the start and end points of another line feature to the normal vector is calculated, and whether the distance is less than a distance threshold is analyzed, the direction vectors and the angle between the direction vectors of two line segments are calculated, and whether the angle between the direction vectors is less than an angle threshold is analyzed, and the line features are homogenized based on the distance threshold and the angle threshold.

[0113] In a possible implementation, the back-end nonlinear optimization unit includes a sliding window model optimization unit for marginalizing and converting the visual data and the inertial data into prior information respectively while marginalizing the previous data frame based on the sliding window model.

[0114] In one possible implementation, the back-end nonlinear optimization unit includes a marginalization optimization unit for converting the earliest frame and its associated inertial data and observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

[0115] It should be noted that the visual-inertial SLAM system based on point-line fusion provided in the embodiment of the present application is used to implement the above-mentioned visual-inertial SLAM method based on point-line fusion, corresponds to the method one by one, and has corresponding technical effects, so it will not be elaborated on here.

[0116] The above specific implementation methods further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above are only specific implementation methods of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A visual-inertial SLAM method based on point-line fusion, characterized in that: include: S1, obtaining visual data from a camera and inertial data from an IMU, and preprocessing the visual data and the inertial data, including: performing feature detection and matching on the visual data, and performing IMU pre-integration on the inertial data; S2. Performing visual-inertial joint initialization based on the preprocessed visual data and the inertial data, including: visual initialization and visual-inertial alignment to obtain initial parameters; S3, backend nonlinear optimization of improved sliding window based on initial parameters; S4, performing loop closure detection and global pose optimization based on the results of backend nonlinear optimization, including: performing loop closure detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a keyframe database; Among them, feature detection and matching of visual data includes: extracting and tracking point features of visual data based on a tracking algorithm combining forward and reverse optical flows, and selecting and optimizing line features of visual data based on a line segment detection model after line feature selection and homogenization optimization; The back-end nonlinear optimization of the improved sliding window based on the initial parameters includes: marginalizing the visual data and the inertial data respectively based on the sliding window model and converting them into prior information, and improving the marginalization step based on the distributed optimization strategy.

2. A visual-inertial SLAM method based on point-line fusion according to claim 1, characterized in that: The tracking algorithm based on the combination of forward and reverse optical flow is used to extract and track point features of visual data, including: Extract feature points in the current frame image based on feature point detection algorithm; Based on the grayscale invariance assumption, the solution to the minimum difference between the current frame and the target frame is transformed into the objective function optimization; Construct the Jacobian matrix based on the objective function; Expand the point to a point set near the point based on the Gauss-Newton method; The position difference of feature points in consecutive frames is iteratively calculated based on the forward optical flow algorithm. During the iteration, the position of feature points in the current frame is reversely compensated based on the reverse optical flow algorithm, and the Jacobian matrix is ​​updated.

3. A visual-inertial SLAM method based on point-line fusion according to claim 1, characterized in that: Based on the line feature selection and homogenization optimized line segment detection model, line feature selection and optimization are performed on the visual data, including: Construct a line feature set through the line feature extraction algorithm, and traverse all elements in the line feature set through the line feature iterator; Line feature selection: If the number of triangulated frames of the line feature is greater than or equal to the threshold, or the line feature is not triangulated in the current frame, then skip this frame, otherwise count the observation frames of the line feature and store the observation data, and filter out the line features whose endpoint distance is less than the threshold based on the endpoint coordinates of the line feature; Line feature optimization: Calculate the normal vector of the line feature based on the start and end points of the line feature, calculate the distance from the start and end points of another line feature to the normal vector, analyze whether the distance is less than the distance threshold, calculate the direction vector and the angle between the direction vectors of the two line segments, analyze whether the angle between the direction vectors is less than the angle threshold, and homogenize the line features based on the distance threshold and the angle threshold.

4. A visual-inertial SLAM method based on point-line fusion according to claim 1, characterized in that: Based on the sliding window model, the visual data and the inertial data are marginalized and converted into prior information respectively, including: based on the sliding window model, while marginalizing the previous data frame, the visual data and the inertial data are marginalized and converted into prior information respectively.

5. A visual-inertial SLAM method based on point-line fusion according to claim 1, characterized in that: The marginalization step is improved based on a distributed optimization strategy, including: converting the earliest frame and its associated inertial data and observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

6. A visual inertial SLAM system based on point-line fusion, characterized in that: include: A data preprocessing unit, used to obtain visual data from the camera and inertial data from the IMU, and preprocess the visual data and the inertial data, including: feature detection and matching of the visual data, IMU pre-integration of the inertial data, and feature detection and matching of the visual data, including: point feature extraction and tracking of the visual data based on a tracking algorithm combining forward and reverse optical flows, and line feature selection and optimization of the visual data based on a line segment detection model after line feature selection and homogenization optimization; An initialization unit, configured to perform visual-inertial joint initialization based on the preprocessed visual data and the inertial data, including: visual initialization and visual-inertial alignment to obtain initial parameters; A back-end nonlinear optimization unit is used for improving the back-end nonlinear optimization of the sliding window based on the initial parameters. The back-end nonlinear optimization of the sliding window based on the initial parameters includes: marginalizing the visual data and the inertial data respectively based on the sliding window model and converting them into prior information, and improving the marginalization step based on the distributed optimization strategy; The loop closure detection and global pose optimization unit is used to perform loop closure detection and global pose optimization based on the results of back-end nonlinear optimization, including: performing loop detection and relocalization, performing four-degree-of-freedom pose graph optimization and maintaining a keyframe database.

7. A visual-inertial SLAM system based on point-line fusion according to claim 6, characterized in that: The initialization unit includes a feature point extraction optimization unit, which is used to extract feature points in the current frame image based on a feature point detection algorithm; based on the grayscale invariance assumption, the minimum difference solution between the current frame and the target frame is converted into an objective function optimization; Construct the Jacobian matrix based on the objective function; Based on the Gauss-Newton method, the point is expanded to a point set near the point; based on the forward optical flow algorithm, the position difference of the feature point in the consecutive frames is iteratively calculated. During the iteration process, the position of the feature point in the current frame is reversely compensated based on the reverse optical flow algorithm, and the Jacobian matrix is ​​updated.

8. A visual-inertial SLAM system based on point-line fusion according to claim 6, characterized in that: The initialization unit includes a feature line extraction optimization unit, which is used to construct a line feature set through a line feature extraction algorithm, and traverse all elements in the line feature set through a line feature iterator; line feature selection: if the number of triangulated frames of the line feature is greater than or equal to a threshold, or the line feature is not triangulated in the current frame, then this frame is skipped, otherwise the observation frames of the line feature are counted and the observation data is stored, and line features with endpoint distances less than a threshold are screened out based on the endpoint coordinates of the line feature; line feature optimization: the normal vector of the line feature is calculated using the start and end points of the line feature, the distance from the start and end points of another line feature to the normal vector is calculated, and whether the distance is less than a distance threshold is analyzed, the direction vectors and the angle between the direction vectors of two line segments are calculated, and whether the angle between the direction vectors is less than an angle threshold is analyzed, and the line features are homogenized based on the distance threshold and the angle threshold.

9. A visual-inertial SLAM system based on point-line fusion according to claim 6, characterized in that: The back-end nonlinear optimization unit includes a sliding window model optimization unit, which is used to marginalize the visual data and the inertial data respectively and convert them into prior information while marginalizing the previous data frame based on the sliding window model.

10. A visual-inertial SLAM system based on point-line fusion according to claim 6, characterized in that: The back-end nonlinear optimization unit includes a marginalization optimization unit for converting the earliest frame and the inertial data associated therewith and the observed landmark information into prior information, and then incorporating the prior information into the overall optimization objective function.

Images