A Visual-Inertial SLAM Method

Abstract

The present invention relates to a visual-inertial SLAM method, comprising the following steps: Step 1: extracting and tracking feature points on an image; Step 2: performing Delaunay triangulation on the set of feature points and converting them to a world coordinate system to obtain a triangular mesh; Step 3: extracting bounded planes and performing data association; Step 4: performing feature point inspection and merging on planes within a sliding window; Step 5: constructing a factor graph model, and introducing geometric constraints of the planes into the factor graph model to optimize global pose. The method proposed in the present invention is applicable to the process of extracting bounded plane parameters in a scene using a monocular camera, and can implement boundary constraints of bounded planes and data fusion between planes. The present invention implements a complete visual-inertial SLAM system, and verification results of the system on EUROC and TUM benchmark datasets show that the method significantly reduces the root mean square error without sacrificing real-time performance.

Description

Technical Field

[0001] The present invention relates to the field of positioning and map construction, and in particular to a visual-inertial SLAM method. Background Art

[0002] Simultaneous localization and mapping is a key technology for autonomous positioning and scene mapping. This technology uses visual or inertial sensors to acquire information about the surrounding environment, thereby simultaneously building a map of the surrounding scene while autonomously positioning. Vision-based SLAM systems are becoming a mainstream research direction due to their small size and low cost. Furthermore, some studies have shown that using low-cost inertial measurement units (IMUs) to assist with visual positioning often achieves more robust results. Currently, visual-inertial SLAM technology has demonstrated excellent performance on drones and mobile robots.

[0003] Existing visual-inertial systems can be categorized by the number of states to be optimized in a single estimation: filtering-based methods, fixed-lag smoothing-based methods, and full-smoothing-based methods. Filtering-based methods assume that the current state is only related to the previous state; fixed-lag smoothing-based methods, also known as sliding window methods, only optimize states within a sliding window, marginalizing older states while adding new ones.

[0004] With the advancement of computer technology and hardware, current research focus has shifted to methods based on fixed-lag smoothing and full smoothing. Fixed-lag smoothing methods optimize historical states within a sliding window, using marginalization to remove older information. Furthermore, robust kernel functions are introduced to reduce the impact of outliers. Fixed-lag smoothing methods need to consider multiple states, not just the latest state, in a single estimation. Therefore, they require higher computing power than filtering-based methods and are more accurate than filtering-based solutions.

[0005] Full smoothing methods estimate the entire historical state by solving a single, nonlinear optimization problem. This method optimizes the positions of all waypoints while updating the state, thus ensuring the highest accuracy. However, updating waypoints also complicates the optimization problem, requiring more expensive computing platforms, which limits the practical application of full smoothing methods.

[0006] One of the limitations of single-feature-based visual-inertial SLAM technology is its over-reliance on such scene features. Therefore, multi-feature fusion positioning is a major development trend in indoor scene positioning (especially in areas with weak textures). Indoor environments are the application scenarios of many service robots. In addition to feature points, such scenes also contain many high-level geometric features, such as line features and surface features. These high-level features enable fast and robust data matching and association. Using these geometric constraints helps achieve long-term association of planes, thereby reducing cumulative errors. However, the fusion of feature points and plane features will introduce new errors and there is a possibility of degradation. Summary of the Invention

[0007] The present invention provides a visual-inertial SLAM method to solve the above technical problems.

[0008] To solve the above technical problems, the present invention provides a visual-inertial SLAM method, comprising the following steps:

[0009] Step 1: Extract and track feature points on the image;

[0010] Step 2: Perform Delaunay triangulation on the set of feature points and convert them into a world coordinate system to obtain a triangular mesh;

[0011] Step 3: Extract bounded planes and perform data association;

[0012] Step 4: Check and merge the feature points of the plane within the sliding window;

[0013] Step 5: Construct a factor graph model and introduce geometric constraints of the plane into the factor graph model to optimize the global pose.

[0014] Preferably, in step 1, a KLT optical flow algorithm is used to track feature points.

[0015] Preferably, step 2 further includes: obtaining an initial value in conjunction with an inertial measurement unit.

[0016] Preferably, the step of obtaining the initial value includes:

[0017] Step 21: Find the corresponding frame that can achieve stable tracking and sufficient parallax by checking the feature correspondence between the latest frame and all previous frames;

[0018] Step 22: Restoring the relative rotation and non-real scale translation between the corresponding frames;

[0019] Step 23: triangulate all features observed in the corresponding frame;

[0020] Step 24: Based on the triangulation results, estimate the poses of other frames in the sliding window;

[0021] Step 25: Minimize the total reprojection error of all feature observations.

[0022] Preferably, step 3 includes:

[0023] Step 31: Reconstructing a single-frame plane, and obtaining parameters of the bounded plane based on the clustering result of the triangular mesh;

[0024] Step 32: Associating the single-frame plane with the parameters of the bounded plane;

[0025] Step 33: Count all bounded planes in a sliding window and perform face-to-face data association on these bounded planes.

[0026] Preferably, in step 32, clustering is performed on the triangular mesh to obtain the clustering result, and the clustering method includes:

[0027] Checking for a deformed triangular mesh, where the relationship between the edges and angles of the deformed triangular mesh satisfies at least one of the following conditions: containing two acute angles whose angles are less than a first threshold; a ratio of the longest edge to the shortest edge being greater than a second threshold; and a length of at least one edge being greater than a third threshold;

[0028] Horizontal plane clustering; and

[0029] Vertical face clustering.

[0030] Preferably, in step 5, the factor graph model is constructed using the inertial measurement residual model, the visual residual model, the prior information residual model and the point-surface fusion residual model, and its least squares solution is obtained using the overall optimization equation.

[0031] Preferably, in step 5, loop closure detection is used to obtain candidate frames of loop closure, and the global pose is optimized based on the candidate frames.

[0032] Preferably, the method of obtaining candidate frames by loop detection includes:

[0033] Detect several additional corner points from the keyframes optimized by the backend;

[0034] Add the key frame to the key frame database, calculate the similarity score between the current frame and the bag of words, and compare it with all frames in the key frame database;

[0035] Perform loop consistency detection to obtain candidate frames for loop closure.

[0036] Preferably, the method for optimizing the global pose based on the candidate frame includes:

[0037] When a closed loop is detected, descriptor-based neighborhood matching is performed;

[0038] When the number of matching points is greater than the threshold, outliers are removed from the matching points and the relative pose is solved;

[0039] When the number of matching points after elimination still exceeds the threshold, the frame is included in the optimization.

[0040] Compared with the prior art, the visual-inertial SLAM method provided by the present invention has the following advantages:

[0041] 1. The method proposed in this invention can be applied to the process of extracting bounded plane parameters in a scene using a monocular camera, and can realize boundary constraints of bounded planes and data fusion between planes;

[0042] 2. The present invention realizes a complete visual-inertial SLAM system, and the verification results of the system in the EUROC and TUM benchmark datasets show that: in the EUROC dataset, the RMSE of this scheme is lower than that of the typical monocular visual-inertial system; in the TUM dataset, most sequences can be achieved better than the typical method. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 This is a flow chart of a visual-inertial SLAM method in a specific embodiment of the present invention;

[0044] Figure 2 Schematic diagram of Delaunay triangulation of a single-frame image in a specific embodiment of the present invention;

[0045] Figure 3 A schematic diagram of a deformed triangular mesh obtained after triangulation in one embodiment of the present invention;

[0046] Figure 4a and 4b are respectively one-dimensional histograms before and after filtering in a specific embodiment of the present invention;

[0047] Figure 5 Schematic diagram of the relationship between the projection point and the bounded plane in a specific embodiment of the present invention;

[0048] Figure 6 This is a schematic diagram of plane fusion in a specific embodiment of the present invention;

[0049] Figure 7 It is an optimization variable factor graph in a specific embodiment of the present invention;

[0050] Figure 8 This is a schematic diagram of the visual residual of a unit sphere in a specific embodiment of the present invention;

[0051] Figure 9 A schematic diagram of a marginalization strategy in a specific embodiment of the present invention;

[0052] Figure 10Schematic diagram of point-surface fusion residual in a specific embodiment of the present invention. DETAILED DESCRIPTION

[0053] In order to describe the technical solution of the above invention in more detail, specific embodiments are listed below to demonstrate the technical effects; it should be emphasized that these embodiments are used to illustrate the present invention and are not used to limit the scope of the present invention.

[0054] The visual inertial SLAM method provided by the present invention is as follows: Figure 1 As shown, the following steps are included:

[0055] Step 1: Extract and track feature points on the image.

[0056] Step 2: Perform Delaunay triangulation on the set of feature points and convert them into a world coordinate system to obtain a triangular mesh.

[0057] Step 3: Extract bounded planes and perform data association.

[0058] Step 4: Check and merge the feature points of the plane within the sliding window.

[0059] Step 5: Construct a factor graph model and introduce geometric constraints of the plane into the factor graph model to optimize the global pose.

[0060] The system corresponding to the present invention includes a measurement value preprocessing module, a monocular visual inertial odometry module, and a loop detection module. The measurement value preprocessing module receives the measurement value of the sensor, detects and tracks the feature points in the image, and triangulates the feature point set. The monocular visual inertial odometry module is mainly divided into two parts: plane extraction and sliding window optimization. The plane extraction part parameterizes the bounded plane in the scene, and the sliding window optimization part combines the IMU residual, point-surface fusion residual, prior information residual and loop detection information to perform state estimation within the sliding window range. After determining that a loop has occurred, the loop detection module detects the loop position of the key frame through feature retrieval and performs global four-degree-of-freedom posture optimization.

[0061] In some embodiments, Shi-Tomasi corner points are used and the KLT optical flow algorithm is used for feature point tracking. The pixel grayscale value change E(u,v) caused by moving the window (u,v) centered on the pixel is defined as follows:

[0062]

[0063]

[0064] S=min(λ1,λ2) (1)

[0065] Where I(x,y) represents the pixel grayscale value, w(x,y) represents the weight of the pixel (x,y) within the window, the larger E(u,v) is, the more robust the corner feature is, and λ1 and λ2 represent the eigenvalues (variation in orthogonal directions) of the diagonalized real symmetric matrix. S represents the response score of each window. If the score is greater than the set threshold, it is considered a corner. The corner points in the previous frame are then tracked using the optical flow constraint equation:

[0066]

[0067] Where I u , I v Represents the image gradient of the pixel grayscale along the X and Y directions, I t represents the time gradient along t, Represent the velocity vector of the optical flow along the X-axis and Y-axis respectively.

[0068] like Figure 2 As shown in Figure 1, after the visual preprocessing is completed, the Delaunay triangulation is calculated for the feature point set obtained by optical flow tracking. The Delaunay triangulation can maximize the minimum angle of each triangle in the feature point set while ensuring that there are no mutually contained triangles.

[0069] Then, the IMU (Inertial Measurement Unit) pre-integration is performed. Specifically, since the sampling frequency of the image and the IMU are inconsistent, aligning the data of the two is a necessary condition for obtaining the relative pose between the two image frames. Deviation from gyroscope measurement The influence of time interval [t k , t k+1 ], the relative motion from the kth image frame to the k+1th image frame can be obtained by formula (3):

[0070]

[0071]

[0072]

[0073]

[0074] in, represents the rotation at time t, Respectively represent from t k Time to t k+1 Relative translation, rotation, and velocity at each moment, represents the disturbance of angular velocity. represents the acceleration deviation at time t, represents the gyroscope bias at time t, which is defined as a random walk bias whose first-order derivative conforms to Gaussian white noise.

[0075] The monocular visual-inertial SLAM proposed in this application is a nonlinear system, so it is necessary to obtain relatively accurate initial values through initialization. This embodiment needs to combine the IMU to obtain accurate initial values, which can specifically include the following steps:

[0076] Step 21: Find the corresponding frame that can achieve stable tracking and sufficient parallax by checking the feature correspondence between the latest frame and all previous frames;

[0077] Step 22: using a five-point method to recover the relative rotation and non-real scale translation between the corresponding frames;

[0078] Step 23: triangulate all features observed in the corresponding frame;

[0079] Step 24: Based on the triangulation results, perform the PnP method to estimate the poses of other frames in the sliding window;

[0080] Step 25: Minimize the total reprojection error of all feature observations through a global bundle adjustment.

[0081] The monocular visual-inertial odometry module is primarily used for plane extraction and state estimation. The plane extraction component extracts bounded planes from a single image frame and fuses the plane data within a sliding window. The state estimation component constructs an optimization model to optimize the state within the sliding window and solves it using the Gauss-Newton method.

[0082] In some embodiments, step 3 includes:

[0083] Step 31: Reconstructing a single-frame plane, and obtaining parameters of the bounded plane based on the clustering result of the triangular mesh;

[0084] Step 32: Associating the single-frame plane with the parameters of the bounded plane;

[0085] Step 33: Count all bounded planes in a sliding window and perform face-to-face data association on these bounded planes.

[0086] Specifically, to reconstruct a single-frame plane, the candidate Delaunay triangle needs to be converted into a triangular mesh in the world coordinate system. The camera coordinate system can be converted to the world coordinate system using formula (4):

[0087]

[0088]

[0089]

[0090] Among them, x c 、y c They represent the coordinates in the camera coordinate system after dedistortion, x′ and y′ represent the coordinates in the camera coordinate system with distortion, and k1, k2, k3, p1, p2, and r represent the camera distortion parameters. w ,Y w ,Z w ] T Represents the coordinates in the world coordinate system, Represents the rotation and translation from the IMU coordinate system to the world coordinate system, Represents the rotation and translation from the camera coordinate system to the IMU coordinate system, and s represents the depth value.

[0091] In order to filter out deformed triangle meshes and check the geometric relationship between planes, this application clusters the triangle meshes and performs preliminary parameterization using normals and distances to the origin [n, d]. The clustering steps are as follows:

[0092] Step 1: Check the deformed triangle mesh: The relationship between the edges and angles of the deformed triangle mesh meets at least one of the following conditions: it contains two sharp angles with angles less than the first threshold (almost no plane information in the scene can be provided), the ratio of the longest side to the smallest side is greater than the second threshold (there may be outliers as vertices), and the length of at least one side is greater than the third threshold (overly large triangles may affect subsequent plane fusion). Figure 3 The triangle shown is filled with diagonal lines.

[0093] Step 2: Horizontal plane clustering: First, according to formula (5), the triangular meshes that are close to horizontal or vertical planes are selected;

[0094]

[0095] Among them, g represents the unit vector of gravity direction, n π represents the normal of the triangular mesh, and θ represents the angle between the normal and the direction of gravity. Secondly, a one-dimensional histogram of the height of the triangular mesh vertices is constructed; finally, the local maximum value in the histogram is extracted to avoid the following Figure 4a There are multiple local maxima in the histogram, so it is necessary to perform Gaussian filtering on the histogram. The effect after filtering is as follows Figure 4b shown.

[0096] Step 3: Vertical Plane Clustering: First, vertical planes are filtered out. Second, a two-dimensional histogram containing the triangular mesh information is constructed, where one axis represents the distance to the origin and the other represents the angle between the normal and the direction of gravity. Finally, local maxima in the histogram are extracted. The specific scheme is the same as in Step 2 and will not be detailed here.

[0097] According to the distance from the point to the plane, the vertices of the triangle mesh are divided into two categories: point set g P represents the set of vertices whose distance to the plane is less than the threshold; point set r P represents the set of vertices whose distance is greater than the threshold. The initial plane M parameter can be determined by formula (6):

[0098]

[0099]

[0100] Where P∈ g P represents the point set g Points inside P, d p Represents the distance from point P to the initial plane M, Num(A) represents the number of points in set A, and the set k P stands for g Satisfaction in P A set of points where any three points are not collinear, d M is the distance threshold. k The number of points in P is greater than or equal to 3, and any three points P1(x1,y1,z1), P2(x2,y2,z2), and P3(x3,y3,z3) are selected from it. new is defined as follows:

[0101]

[0102] In addition, in order to avoid the over-parameterization problem, the normal vector is constrained using formula (8), that is:

[0103]

[0104] Formula (9) unifies the scale of the plane parameters to obtain the initial parameters of the plane M.

[0105]

[0106]

[0107] The disadvantage of the above plane extraction scheme is that the extracted plane is regarded as an infinite plane. However, the real plane in the structured scene has a definite boundary, outline and size. In order to avoid unnecessary errors, it is necessary to extract a bounded plane M from the initial plane M. limit The specific method is to reconstruct the rectangular coordinate system XoY containing the plane M. g d in P set p The smallest point is taken as the origin, the direction of the normal of plane M is taken as the Y axis, the direction perpendicular to the normal is taken as the X axis, and gAll points in P are projected into the coordinate system XoY, and then the ellipse boundary m constraint point set is constructed. i ,y i ), the polynomial F(x,y) represents the distance from the point (x,y) to the given conic section, which is modeled as a constrained optimization problem:

[0108]

[0109] stb 2 -4ac<0 (10)

[0110] Define coefficient vector A = [a,b,c,d,e,f] T , parameter vector N i =[x i 2 ,x i y i ,y i 2 ,x i ,y i ,1]. The constraints can be transformed into equality constraints at a certain scale: 4ac-b 2 = 1. Let matrix D = [N1, N2, ..., N j ], formula (10) is rewritten as:

[0111] min||DA|| 2 sA T CA=1

[0112]

[0113] Furthermore, formula (11) can be written as

[0114] min||DA 2 =A T D T DA stA T CA=1 (12)

[0115] Construct the Lagrangian function:

[0116] L(D,μ)=A T D T DA-μ(A T CA-1) (13)

[0117] Let D T D=S, and taking the partial derivative of the Lagrangian function L(D,μ) we get:

[0118]

[0119] The minimum optimization problem now becomes finding S -1 The eigenvector of C. Formula (12) can be rewritten as:

[0120] min||DA|| 2 =A T D T DA=A T SA=A T μCA=μ (15)

[0121] In order to obtain the optimal solution, we only need to find the largest eigenvalue μ -1 The corresponding eigenvector, and the constraint condition A T CA=1 can be scaled back to the original scale.

[0122] Then calculate the distance from all landmark points on the current frame to the plane, and for distances less than d M The landmark point is placed in the set Γ. By introducing the plane collision probability model shown in formula (16), it can be determined whether the point is on the bounded plane:

[0123]

[0124] Among them, P(ρ∈M limit ) indicates that the landmark point ρ falls on the bounded plane M limit The probability of Represents the distance from point ρ to plane M Less than d M The probability of Represents the projection point of point ρ The probability of being within the ellipse m, note that when the point The shortest distance from the ellipse boundary to the ellipse boundary Less than the threshold d m We still think that There is a probability that it is within the ellipse boundary.

[0125] like Figure 5 As shown, the black points represent the cases that fall within the ellipse m Gray points represent points outside the ellipse The shortest distance to the ellipse boundary is less than the threshold White points represent points outside the ellipse The shortest distance to the ellipse is greater than the threshold, and formula (17) defines

[0126]

[0127] The plane is associated with multiple feature points, and the positions of the feature points are also affected by noise. Therefore, is defined as follows:

[0128]

[0129] represents the distance measurement, assuming the noise n M 、n m Obey Gaussian distribution, that is:

[0130]

[0131] Combining formulas (17)-(19), we can obtain:

[0132]

[0133]

[0134] Similarly, The calculation of is similar to formula (20).

[0135] Plane data association consists of two stages. In the first stage, bounded plane parameters are used to associate single-frame planes. In order to avoid false positive results, the angle and distance between planes are compared using formula (21):

[0136] ||n2-n1||<N

[0137] |d2-d1|<d M (twenty one)

[0138] Among them, N represents the threshold of the normal modulus length, and [n1, d1] and [n2, d2] are the plane parameters of the plane to be fused 1 and the plane to be fused 2, respectively. Then, the constraint model is used to construct the boundary conditions of the plane association. Specifically, the following contents are: first, use formula (21) to determine whether each set of planes meets the association conditions; then project the planes that meet the conditions into the two-dimensional coordinate system; then use the elliptical affine transformation to determine whether the two elliptical boundaries intersect; then check whether the boundary point values contained by the planes are greater than the given threshold, such as Figure 6 As shown; finally, the new plane boundary is adaptively adjusted using formula (10).

[0139] The second stage adopts a plane data association strategy that fuses feature points, counts all planes in a sliding window, and performs data association on these planes similar to formula (21); after each data association, it is still necessary to check whether these planes already exist in the optimization problem to avoid duplicate plane variables.

[0140] In some embodiments, the present application utilizes an inertial measurement residual model, a visual residual model, a priori information residual model, and a point-surface fusion residual model to construct the factor graph model, and utilizes an overall optimization equation to obtain its least squares solution.

[0141] like Figure 7 As shown, x, ρ, and π represent the state quantity, landmark point, and plane, respectively. First, the state vector to be optimized in the sliding window is defined as follows:

[0142]

[0143]

[0144]

[0145] x k Indicates the IMU state corresponding to the k-th frame image, that is, t k The position of the IMU in the world coordinate system at this moment speed Rotation And the acceleration deviation b in the IMU coordinate system a Deviation from angular acceleration b g . Represents the external parameters of the camera and IMU. n represents the number of keyframes in the sliding window, m represents the number of landmark points in the sliding window, and λ i represents the inverse depth of the camera coordinate system corresponding to the image frame where the i-th landmark is first observed, and f represents the number of planar features in the sliding window.

[0146] Using Bayes' theorem, we can get the state vector X with respect to the measurement value The posterior probability of :

[0147]

[0148] Based on the standard independence assumption between measurements, the measurement factors can be further classified and formula (23) can be rewritten as:

[0149]

[0150]

[0151] Among them, φ0 represents the prior information, φ F Represents point-surface association information, φ CAM Represents visual information, φ IMU Represents IMU information, φ P Represents the plane constraint information. The factor graph model can be used to express the dependency relationship of each variable in formula (24).

[0152] Given measurement value The state vector X can be calculated by the maximum a posteriori probability. Under the assumption of zero-mean Gaussian noise, the negative logarithm of the posterior probability of formula (24) is minimized. Therefore, the optimization function is defined as:

[0153]

[0154] Represents IMU pre-integration and measurement residuals, represents the visual reprojection residual, r PF Represents the residual of prior information, H PF represents the information matrix, represents the point-surface coupling residual.

[0155] The prior information residual model is constructed as follows:

[0156] In order to reduce the complexity of the overall system during state estimation, this application uses marginalization to selectively remove landmarks and bounded planes within the sliding window. The prior information generated by marginalization will be added to the existing prior and become the prior residual in the next optimization. Figure 9 As shown, if x n-1 is a key frame, then x0 and the corresponding state vector move out of the sliding window; if x n-1 Not a keyframe, x n-1 will be removed from the sliding window.

[0157] Formula (26) represents the marginalization principle based on Schur complement, where δy a ,δy b Represent the camera pose and landmark points respectively.

[0158]

[0159]

[0160] Among them, Λ b T Λ a -1 Λ b called Λ a In Λ b The Schur term in δy b It can be directly calculated.

[0161] The IMU residual model is constructed as follows:

[0162] Two adjacent frames b k and b k+1 The IMU measurement residual between can be defined as:

[0163]

[0164] in[·] xyz Indicates the vector part of the quaternion Q to be extracted, Represents the multiplication operation of quaternions, Indicates the error status of the quaternion.

[0165] The visual residual model is constructed as follows:

[0166] Different from the traditional pinhole camera model where the reprojection error is defined on the image plane, e.g. Figure 8 As shown, the camera measurement residual is defined on the unit sphere. Consider the lth feature observed for the first time in the i-th image, and the residual of the feature observation in the j-th image is defined as:

[0167]

[0168]

[0169] in Represents the coordinates of the landmark point ρ in the camera coordinate system. Since the degree of freedom of the visual residual is two, the residual vector is projected onto the tangent plane. [b1,b2] T yes Two arbitrarily chosen orthogonal bases of the tangent plane.

[0170] The point-surface fusion residual model is constructed as follows:

[0171] Visual residuals rely on feature matching of landmark points and are easily affected by lighting changes and textures. Figure 10 The single-frame point cloud geometric constraints and plane associations shown are used to improve the accuracy of plane estimation and the stability of motion estimation in textureless scenes.

[0172] A simple geometric constraint consists of summing the distances from each landmark point to its corresponding plane. e Distance d to the plane ⊥ for:

[0173] d ⊥ =n T ·ρ+d (29)

[0174] The optimal landmark point ρ * And the plane parameters (n * ,d * ) can get the optimal solution of formula (30):

[0175]

[0176] Will Defined as:

[0177]

[0178] in, Represents ρ e The homogeneous coordinates of . In this residual, the state quantity to be optimized is:

[0179]

[0180] The Jacobian matrix of the residual with respect to each state variable is defined as follows:

[0181]

[0182] in, represents the Jacobian for translation and rotation, represents the Jacobian of the inverse depth of the feature point, represents the Jacobian matrix of the plane features, Represents the Jacobian matrix for the landmark points.

[0183]

[0184]

[0185]

[0186]

[0187] It’s important to note that a single coplanarity constraint isn’t sufficient to optimize the state variables in the above equation; at least three are required. However, if the three points are approximately on a straight line, they will not constrain the plane, and the plane model will degenerate. Therefore, it’s important to ensure that a plane candidate has the minimum number of constraints before adding it to the optimization problem.

[0188] This application uses a sliding window and marginalization in the VIO module, which reduces the computational complexity and optimization time, but also introduces system cumulative error. To effectively eliminate drift, this application proposes a loop detection module that is tightly coupled with the VIO module.

[0189] Loop detection uses the DBoW2 bag-of-words method based on the BRIEF descriptor. Because the number of corner points recognized by the front-end is far from enough for loop detection, 500 additional FAST corner points are detected in the key frames optimized by the back-end. Then, the key frames are added to the key frame database, and the similarity score between the current frame and the bag-of-words is calculated and compared with all frames in the key frame database. Finally, loop consistency detection is performed to obtain candidate frames for loops.

[0190] When a closed loop is detected, the BRIEF descriptor is first used to perform neighborhood matching based on the descriptor between the FAST corner points of the candidate frame and the Tomasi corner points of the current frame. When the number of matching points exceeds the threshold, the RANSAC algorithm is used to remove outliers from the matching points and solve the relative pose for subsequent global pose optimization. When the number of matching points after elimination still exceeds the threshold, the candidate frame is considered to be a correct closed loop frame and is included in the optimization. The overall objective function is defined as:

[0191]

[0192]

[0193] in, represents the calculated relative pose between the two frames, i represents the index of the frame in the keyframe database that is most similar to the candidate frame, j represents the index of the candidate frame, h[·] is the huber function, represents the feature observation of the corresponding frame where the loop is detected, and τ represents the corresponding frame where the loop is detected. Represents the difference in yaw angle between two frames. Since gravity has been aligned in advance in the VIO system, the pitch angle θ and the roll angle is considerable, so only the heading angle ψ needs to be optimized. and The definition is as follows:

[0194]

[0195]

[0196] The above method was used to validate the corresponding system on the EUROC and TUM benchmark datasets. Experimental results show that the RMSE of the proposed scheme is lower than that of typical monocular visual-inertial systems on the EUROC dataset, and it can achieve better results than the typical method in most sequences on the TUM dataset.

[0197] In summary, the visual inertial SLAM method provided by the present invention comprises the following steps: Step 1: extracting and tracking feature points on an image; Step 2: performing Delaunay triangulation on the set of feature points and converting them to a world coordinate system to obtain a triangular mesh; Step 3: extracting bounded planes and performing data association; Step 4: performing feature point inspection and merging on the planes within the sliding window; Step 5: constructing a factor graph model, and introducing geometric constraints of the planes into the factor graph model to optimize the global pose. The present invention can be applied to the process of extracting bounded plane parameters in a scene using a monocular camera, and can realize boundary constraints of bounded planes and data fusion between planes, thereby achieving global four-degree-of-freedom pose optimization.

[0198] Obviously, those skilled in the art may make various changes and modifications to the invention without departing from the spirit and scope of the invention. Thus, if such changes and modifications fall within the scope of the claims and their equivalents, the present invention is intended to include such changes and modifications.

Claims

1. A visual-inertial SLAM method, characterized in that: The steps include: Step 1: Extract and track feature points on the image; Step 2: Perform Delaunay triangulation on the set of feature points and convert them into a world coordinate system to obtain a triangular mesh; Step 3: Extract bounded planes and perform data association, including: Step 31: Reconstructing a single-frame plane, and obtaining parameters of the bounded plane based on the clustering result of the triangular mesh; Step 32: Associating the single-frame plane with the parameters of the bounded plane; Step 33: Count all bounded planes in a sliding window and perform face-to-face data association on these bounded planes; Step 4: Check and merge the feature points of the plane within the sliding window; Step 5: Construct a factor graph model and introduce plane geometric constraints into the factor graph model to optimize the global pose; wherein, the factor graph model is constructed using an inertial measurement residual model, a visual residual model, a priori information residual model, and a point-surface fusion residual model, and its least squares solution is obtained using the overall optimization equation.

2. The visual-inertial SLAM method according to claim 1, wherein In step 1, the KLT optical flow algorithm is used to track feature points.

3. The visual-inertial SLAM method according to claim 1, wherein Step 2 also includes: obtaining an initial value in conjunction with an inertial measurement unit.

4. The visual-inertial SLAM method according to claim 3, wherein The step of obtaining the initial value includes: Step 21: Find the corresponding frame that can achieve stable tracking and sufficient parallax by checking the feature correspondence between the latest frame and all previous frames; Step 22: Restoring the relative rotation and non-real scale translation between the corresponding frames; Step 23: triangulate all features observed in the corresponding frame; Step 24: Based on the triangulation results, estimate the poses of other frames in the sliding window; Step 25: Minimize the total reprojection error of all feature observations.

5. The visual-inertial SLAM method according to claim 1, wherein In step 31, clustering is performed on the triangular mesh to obtain the clustering result. The clustering method includes: Checking for a deformed triangular mesh, where the relationship between the edges and angles of the deformed triangular mesh satisfies at least one of the following conditions: containing two acute angles whose angles are less than a first threshold; a ratio of the longest edge to the shortest edge being greater than a second threshold; and a length of at least one edge being greater than a third threshold; Horizontal plane clustering; and Vertical face clustering.

6. The visual-inertial SLAM method according to claim 1, wherein In step 5, loop closure detection is used to obtain candidate frames of loop closure, and the global pose is optimized based on the candidate frames.

7. The visual-inertial SLAM method according to claim 6, wherein Methods for obtaining candidate frames using loop closure detection include: Detect several additional corner points from the keyframes optimized by the backend; Add the key frame to the key frame database, calculate the similarity score between the current frame and the bag of words, and compare it with all frames in the key frame database; Perform loop consistency detection to obtain candidate frames for loop closure.

8. The visual-inertial SLAM method according to claim 7, wherein The method for optimizing the global pose based on the candidate frame includes: When a closed loop is detected, descriptor-based neighborhood matching is performed; When the number of matching points is greater than the threshold, outliers are removed from the matching points and the relative pose is solved; When the number of matching points after elimination still exceeds the threshold, the frame is included in the optimization.

Images