SLAM method and system based on ground surface feature constraint and point-surface cooperation

Abstract

The application relates to the technical field of image processing, in particular to a SLAM method and system based on ground plane feature constraint and point / face collaborative optimization, which comprises collecting scene image data and IMU data, extracting point features of the image and performing triangulation; classifying the point features after the triangulation, and retaining the point features with the smallest point feature values; performing ground plane feature fitting by using a RANSAC algorithm, and tracking the point features by using an LK optical flow method; fitting the plane features according to the tracked point features on the ground plane features, and correcting the inverse depth values of the point features by using the fitted plane features; correcting the deviation of the inverse depth of the point features according to the fitted plane features; and performing point / face collaborative optimization and error accumulation calculation of the camera pose constrained by the ground plane features. The application solves the problems of time consumption and calculation resource consumption in the extraction and matching of plane features.

Description

Technical Field

[0001] This invention relates to the field of image processing technology, and in particular to a SLAM method and system based on ground surface feature constraints and point-surface collaboration. Background Technology

[0002] One limitation of feature-based VSLAM is its over-reliance on scene features, such as point features used in most VSLAMs. However, it can be observed that this reliance on point extraction in some challenging scenarios can lead to low accuracy or pose estimation failures. When texture information is lost in the scene or the image is blurred due to rapid camera movement, the number of point features is often limited. In such cases, it is often impossible to reliably extract enough feature point matching pairs, resulting in reduced pose estimation accuracy or even SLAM system tracking failure. Even in scenes with sparse texture and significant lighting variations, line segments and planes can still be reliably extracted from the image. Therefore, line segments and planes can serve as a starting point for addressing the problems caused by these scenarios.

[0003] Since VSLAM transfers pose by extracting and matching features between frames, in indoor environments, especially long corridors, even in feature-rich scenarios, long-term VSLAM operation can still lead to significant accumulation of pose estimation errors. Common strategies to eliminate accumulated errors include using loop closure detection or adding other sensors to the system (such as outdoor GNSS receivers and indoor pseudo-satellites or UWB). Loop closure detection requires repeatedly achieving a certain scenario, but many scenarios do not meet the conditions for loop closure detection, and the success rate of loop closure detection cannot be guaranteed. Adding other sensors increases costs and makes the system more bloated and inflexible.

[0004] Planar features also possess good illumination invariance and are insensitive to texture. However, the extraction and parameter description methods for planar features are more complex than those for point and line features. Currently, there are many VSLAM systems based on planar features. However, the extraction and matching of planar features consume a great deal of computational resources. Both are time-consuming processes. Extracting all planar features from the scene would significantly reduce system efficiency, pose a major challenge to the real-time performance of SLAM, and increase system instability. Summary of the Invention

[0005] To address the shortcomings of existing methods, this invention solves the problems of time consumption and computational resource consumption in the extraction and matching of planar features.

[0006] The technical solution adopted in this invention is: a SLAM method based on ground surface feature constraints and point-surface collaboration, comprising the following steps:

[0007] Step 1: Acquire scene image data and IMU data, extract point features from the images and perform triangulation;

[0008] Step 2: Classify the points based on their triangulated features, preserving the key features. The point feature with the smallest value; the RANSAC algorithm is used to fit the ground surface feature, and the LK optical flow method is used to track the point feature;

[0009] Furthermore, step two specifically includes:

[0010] Step 21: Transform the point feature to the world coordinate system to obtain the coordinate values ​​of the point feature in the world coordinate system;

[0011] Step 22: Traverse the point features in the world coordinate system, based on the point features... Classify and save the values. Points with values ​​less than 0 are identified as features and stored as feature clusters C, and C is calculated. max and C min C max and C min These are the maximum and minimum values ​​in feature cluster C;

[0012] Step 23: Extract using RANSAC algorithm Less than C min +α feature points, and use RANSAC to find the optimal surface feature normal vector n. w and the center point coordinates p of the fit w And save the point features that belong to the optimal surface features to C;

[0013] Step 24: Solve for the distance d from the origin of the world coordinate system to the ground feature. w , put d w The normal vector n of the surface feature w It is stored as a global invariant.

[0014] Step 3: Fit the surface features based on the tracked point features belonging to the ground surface features, and use the fitted surface features to correct the inverse depth value of the point features.

[0015] Furthermore, step three specifically includes:

[0016] Step 31: Assuming the set of points contained in the ground surface features is C, the ground surface features are fitted using the m point features that are tracked to the surface feature set in the i+1th frame.

[0017] Step 32: Use local nonlinear optimization to refine the ground surface features, and use the minimum value of the residual equation to solve for the normal vector of the surface features in the three-dimensional phase space coordinate system of the (i+1)th frame. Distance from the origin to the ground surface feature

[0018] Furthermore, the formula for the minimum value of the residual equation is:

[0019]

[0020] in, For c i Ground surface features of the frame; π w Ground surface features in the world coordinate system w; From world coordinate system to c i The transformation matrix of the frame camera coordinate system; χ is the set of parameters to be optimized;

[0021] Step 4: Correct the deviation of the inverse depth of the point features based on the fitted ground surface features;

[0022] Furthermore, step four specifically includes:

[0023] Step 41: Traverse the new point features in the current frame that have been triangulated and do not belong to the surface features, and calculate the point feature f. k to c i The distance *dis* of the ground features in the frame is given by the formula:

[0024]

[0025] Here, ⊙ points to the dot product between quantities, which represents the dot feature. inverse depth λ k As a parameter;

[0026] Step 42: Set dis to 0 and inversely calculate... inverse depth λ k The new point features are added to the ground surface feature set.

[0027] Step 5: Perform point / surface feature collaborative optimization and ground surface feature constraint error accumulation calculation for camera pose;

[0028] Further, in step 51, perform backend optimization;

[0029] Furthermore, step 51 specifically includes:

[0030] Step 511: Assuming there are n+1 point features to be optimized in the sliding window, calculate the projection and observation features of the k-th point feature. The reprojection error between them is given by the formula:

[0031]

[0032] in, and These are the image plane c i The observed value of the feature at the k-th point in the frame; and They represent in c i The 3D coordinates of the k-th point feature in the frame camera spatial coordinate system;

[0033]

[0034] in, and For the past c i-1 f obtained by frame projection k In c i Frame observations, To obtain the point features f k In c i The inverse depth value of the frame; and These are the rotation and translation matrices from frame i-1 to frame i, respectively.

[0035] Step 512: Optimize the point-to-ground surface features using the residual equation. The formula is:

[0036]

[0037] in, Represents the dot product between two vectors. Let be the normal vector in the three-dimensional phase space coordinate system of the i-th frame. This is the distance from the origin to the ground surface feature.

[0038] Step 513: Transfer the surface feature π w Convert to ground surface feature δ w ; to make the surface feature π w Convert to camera coordinate system Will Convert to projection surface features Construct the residual equation from the projection surface features to the tracking surface features, and use the residual equation to solve the Jacobian matrix; traverse the current frame to track all feature points that do not belong to the surface features, classify them according to the distance d from the feature points to the ground surface features, and then perform tracking on the next frame image.

[0039] Furthermore, the residual equation from the projection surface features to the tracking surface features is:

[0040]

[0041] in, For c i Surface features tracked in the frame; For c i The features of the projection surface tracked by the frame.

[0042] Step 52: After nonlinear optimization, update the pose parameters and inverse depth values ​​of point features of the keyframes in the sliding window.

[0043] Furthermore, step 52 specifically includes:

[0044] Traverse the feature points that have not been added to the backend optimization after triangulation in the current frame. Check whether the feature point is inside the surface feature. If it is inside the surface feature, add the feature point to the point set of the surface feature of this frame.

[0045] Furthermore, the SLAM system based on surface feature constraints and point-surface cooperative optimization includes: a memory for storing instructions executable by a processor; and a processor for executing the instructions to implement the SLAM method based on ground surface feature constraints and point-surface cooperative optimization.

[0046] The beneficial effects of this invention are:

[0047] 1. In indoor scenes, the ground is usually flat with almost no change in elevation. Therefore, the ground in the entire scene can be regarded as a single surface feature. By detecting the change in the normal vector of the ground surface feature, the problem of system rotational drift can be solved well. Since the ground is relatively flat and the elevation has almost no change, the problem of system drift in the vertical direction can also be solved well. Attached Figure Description

[0048] Figure 1 This is a flowchart of the SLAM method based on ground surface feature constraints and point-surface collaboration of the present invention;

[0049] Figure 2 It is a point-surface feature collaborative optimization graph;

[0050] Figure 3 It is a projection map of ground surface features. Detailed Implementation

[0051] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0052] like Figure 1 As shown, the SLAM method based on ground surface feature constraints and point-surface collaboration includes the following steps:

[0053] Step 1: Use the MyMy camera to collect image data and IMU data in a relatively flat indoor scene, perform system initialization, extract point features from the image and triangulate them;

[0054] Step 2: Classify the points based on the triangulated features, retain the points with the smallest z-values, fit the ground surface features using the RANSAC algorithm, and track the points using the LK optical flow method.

[0055] Step 21: When performing ground surface feature extraction for the first time, assume there are n point features f k (i∈(1,n)) completes triangulation. To improve the efficiency and accuracy of plane fitting, the point features are first transformed into the world coordinate system frame w-frame; assuming the k-th point feature The inverse depth is λ k , and These are the image plane c i The observed value of the feature at the k-th point of the frame. For the image plane c i The observed features of the k-th point in the frame; firstly, through the extrinsic rotation matrix. and extrinsic translation matrix Point features f k By transforming from the camera coordinate system c-frame to the body coordinate system b-frame, we can obtain the coordinates in the b-frame.

[0056]

[0057] By rotation matrix and extrinsic translation matrix Point features Converting from b-frame to w-frame allows you to obtain the coordinates within the w-frame.

[0058]

[0059] in, f represents the point features respectively k Three-dimensional coordinates in the world coordinate system w.

[0060] Step 22: Then iterate through the n point features under the w-frame. Based on point features Classify the values ​​and put them into categories. Values ​​less than 0 are categorized and stored as feature clusters C, and C is calculated. max and C min Save later; in order to reduce the complexity of the system and to ensure that the vertical direction of the world coordinate system is consistent with the direction of gravity, this invention only uses the IMU for system initialization; after initialization is completed, the IMU is no longer used; therefore, the c-frame referred to thereafter refers to the b-frame.

[0061] Step 23: First, use the RANSAC algorithm to coarsely extract ground surface features. The method for fitting ground surface features using the RANSAC algorithm is as follows:

[0062] Set the threshold to α = 0.1, and first extract the data from step 22. Less than C min The feature points with +α are extracted and saved to the dataset. α can be slightly adjusted according to the actual scenario. Then, RANSAC is used to find the optimal normal vector n of the surface feature. w and the center point coordinates p of the fit w And save the point features belonging to the surface feature into the geometric feature cluster C;

[0063] Step 24: Solve for the distance d from the origin of the world coordinate system to the ground surface feature. w , put d w The normal vector n of the ground surface features w It is stored as a global invariant.

[0064] Step 3: Fit the surface features based on the tracked point features belonging to the ground surface features, and use the fitted surface features to correct the inverse depth value of the point features.

[0065] Step 31: Assume that the set of points contained in the ground surface features is C. In the (i+1)th frame, the LK optical flow method is used to track m point features belonging to the surface feature set in the i-th frame; m>=4, and these points cannot be on a straight line. In the (i+1)th frame, the ground surface features are fitted using the m point features tracked in the i-th frame.

[0066] Step 32: Use local nonlinear optimization to refine the ground surface features and solve for the normal vector of the surface features in the three-dimensional phase space coordinate system of the (i+1)th frame. Distance from the origin to the ground surface feature Assuming the sliding window w_size = 10, the normal vector of the ground surface feature is... The distances to the ground surface features are respectively Put n w and d w As the parameters to be estimated, the optimization equation can be constructed as follows:

[0067]

[0068] in, For c i Ground surface features of the frame; π w Ground surface features in the world coordinate system w; From world coordinate system to c i The transformation matrix of the frame camera coordinate system; χ is the set of parameters to be optimized; This is the minimum value of the residual equation; the normal vector and distance of the ground surface feature in the world coordinate system can be obtained as n. w and d w Solve for n w and d w Afterwards, we continued front-end tracking and back-end optimization. In subsequent tracking and optimization, n w and d w All are treated as fixed values.

[0069] Step 4: Correct the deviation of the inverse depth of the point features based on the fitted surface features;

[0070] Step 41: Traverse the new point features (point cloud) in the current frame that have been triangulated and do not belong to surface features, and calculate the point feature f. k to c i The distance dis of the ground surface features in the frame, such as Figure 2 We retain the feature of points where dis ≤ β, and β is 0.1. The formula for the distance dis is:

[0071]

[0072] Here, ⊙ points to the dot product between quantities, which represents the dot feature. inverse depth λ k As a parameter;

[0073] Step 42: Directly set dis to 0 and inversely calculate... inverse depth λ k Then, the new point features are added to the surface feature set.

[0074] Step 5: Perform nonlinear optimization based on the point features, ground surface features, and constraint relationships between the point / surface features extracted within the sliding window to constrain the accumulation of camera pose errors;

[0075] Step 51: Backend optimization mainly consists of three parts, specifically including:

[0076] Step 511: Optimization of point features observed in different frames;

[0077] Formula (5) is the three-dimensional point feature in c i The reprojection error at a frame describes the projected and observed features of the k-th point. The reprojection error between points; assuming there are n+1 point features to be optimized in the sliding window, the formula for the reprojection error is:

[0078]

[0079] in, and These are the image plane c i The observed value of the feature at the k-th point in the frame; and They represent in c i The 3D coordinates of the k-th point feature in the frame camera spatial coordinate system.

[0080] Optimization of distance constraints from points observed in the same frame to ground surface features;

[0081] First, the feature points belonging to the ground surface features are transformed into the current frame camera's three-dimensional spatial coordinate system using formula (6); assuming the point feature f k In c i-1 The inverse depth of the frame is Let be the observed value of this surface feature. Point features f k The equations transformed to the phase space coordinate system of the i-th frame are:

[0082]

[0083] in, and For the past c i-1 f obtained by frame projection k In c i Frame observations, To obtain the point features f k In c i The inverse depth value of the frame; and These are the rotation and translation matrices from frame (i-1) to frame (i).

[0084] Step 512, the point feature in the three-dimensional phase space coordinate system is Assumption Point features that belong to the set of ground surface features; such as Figure 2 Theoretically, a point feature located on a surface feature has a distance of 0 to the surface feature. However, due to error factors, the distance is usually not exactly 0. The residual equation for optimizing the point-to-surface feature relationship is:

[0085]

[0086] in, This represents the dot product between two vectors; the inverse depth parameter of the point feature can be constrained according to this equation.

[0087] Step 513: Optimization of ground surface features tracked in different frames;

[0088] The Hessian representation of a known surface feature is π. w =(n w ,d w )T ; where n w d is the normal vector of the feature in the world coordinate system. w Let |n| be the distance from the origin to the ground surface feature in the world coordinate system. w || = 1; because n w and d w Linear dependence, because as n w Changes, d w It also changes accordingly; therefore, the Hessian representation is redundant and belongs to overparameterization; to solve this problem, during optimization, let δ w =n w / d w =(x w ,y w ,z w ), δ w This provides a compact representation of ground surface features, thus reducing the degrees of freedom of the Hessian representation to 3; for example... Figure 3 , π w This is the Hessian representation of ground surface features in the world coordinate system. Ground surface features in camera c i Frame projection, For camera c i The representation of surface features extracted from frames in the camera coordinate system. c i The surface feature Hessian representation extracted from the frame is as follows:

[0089] π of the surface features w Transform from world coordinate system to camera coordinate system for:

[0090]

[0091] in, and These are the rotation and translation matrices from the world coordinate system w to the camera coordinate system c, respectively.

[0092] Then put Convert to for:

[0093]

[0094] The residual equation from the projection surface features to the tracking surface features can be constructed as follows:

[0095]

[0096] in, For ci Surface features tracked in the frame; For c i The projection surface features tracked in the frame, δ, have both magnitude and direction; minimizing... It can optimize both size and orientation simultaneously.

[0097] The Jacobian matrix can be written as:

[0098]

[0099]

[0100] in, For c i Frame camera pose parameters; δ w =n 3×1 / d=(x,y,z), where δ is the three-parameter representation of the surface feature, δ w Indicated in the world coordinate system; δ c This is indicated in the camera's world coordinate system.

[0101] After performing local optimization at the back end, all feature points that do not belong to the surface features are tracked in the current frame. The feature points are classified according to the distance d from the feature points to the ground surface features. Feature points with a distance d < 0.1 are added to the geometric point set C, and then the tracking of the next frame image is performed.

[0102] Step 52: After nonlinear optimization, update the pose parameters and inverse depth values ​​of point features in the keyframes of the sliding window; δ can be... w The Hessian representation of the surface features is as follows:

[0103]

[0104] Traverse the feature points that have not been added to the back-end optimization after triangulation in the current frame, and check whether the feature points are in the surface features according to formula (14). If they are in the surface features, add the point features to the point set of the surface features of the frame.

[0105]

[0106] in, The inverse depth of the feature points.

[0107] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A SLAM method based on ground surface feature constraint and point-surface collaboration, characterized in that, The method comprises the following steps: Step one, collecting scene image data and IMU data, extracting point features of the image and triangulating; Step two, classifying according to the triangulated point features, and retaining the point feature with the smallest point feature value; Ground surface feature fitting is performed by using the RANSAC algorithm, and point feature tracking is performed by using the LK optical flow method; Step two specifically comprises: Step 21, converting the point features to the world coordinate system to obtain coordinate values of the point features in the world coordinate system; Step 22, traverse the point features under the world coordinate system, classify according to the value of the point features, save point features with a value less than 0 and store as feature clusters , and calculate and ; Step 23, extracting the feature points with RANSAC algorithm whose value is less than the optimal normal vector of the surface feature and the center coordinate of the fitting with RANSAC ; and saving the point features belonging to the optimal surface feature into ​​ Step 24, solve for the distance from the origin of the world coordinate system to the ground plane feature , and save as a global invariant , and the normal vector of the ground plane feature as a global invariant Step three, fitting the surface features according to the tracked point features on the ground surface features, and correcting the inverse depth values of the point features by using the fitted surface features; Step four, correcting the deviation of the inverse depth of the point features according to the fitted surface features; Step five, point / surface feature collaborative optimization and error accumulation calculation of camera pose constrained by ground surface features are performed; Step five specifically comprises: Step 51, performing back-end optimization; Specifically comprising: Step 511, assuming there are n +1 point elements in the sliding window that need to be optimized, calculate the re-projection error between the projection of the k th point feature and the observed feature , the formula is: （5） wherein, , and are the image planes frame k point feature observations; , and denote the 3D coordinates of the frame camera space coordinate system k point feature; (6) wherein, and are the estimated frame projection in frame observation, are the estimated point features in frame inverse depth values; and are the rotation and translation matrices from the i -1th frame to the i th frame, respectively. Step 512, performing point-to-ground surface feature optimization by using a residual equation, and the formula is: (7) wherein, denotes the dot product between two vectors, is the first i is the normal vector in the three-dimensional phase space coordinate system of the frame, is the distance from the origin to the ground plane feature; Step 513: Add surface features Convert to ground surface features ; surface features Convert to camera coordinate system ;Will Convert to projection surface features Construct the residual equation from the projected surface features to the tracked surface features, and solve for the Jacobian matrix using the residual equation; traverse the current frame to track all feature points that do not belong to the surface features, and calculate the distance from each feature point to the ground surface feature. Classify the data, and then track the next frame. Step 52, after nonlinear optimization, updating the pose parameters of the key frames and the inverse depth values of the point features in the sliding window; Specifically comprising: Traverse the feature points of the current frame which are triangulated but not added to the back-end optimization, and check whether the feature points are in the surface features, if in the surface features, add the point features to the point set of the surface features of the frame.

2. The SLAM method based on ground surface feature constraint and point-surface collaboration according to claim 1, characterized in that, Step three specifically comprises: Step 31, assuming the ground plane feature contains a point set , the first i +1 frame ground plane feature fitting is performed using the m point features that belong to the plane feature set of the first i frame; Step 32, ground surface feature fine extraction using local nonlinear optimization, using the minimum value of the residual equation to solve the surface feature in the first i Normal vector in three-dimensional phase space coordinate system of +1 frame Distance from the origin to the ground surface feature . 3.The SLAM method based on ground surface feature constraint and point-surface collaboration according to claim 2, characterized in that, The formula of the minimum value of the residual equation is: （3） wherein is a ground plane feature of the frame; is a ground plane feature in a world coordinate system w; is a transformation matrix from the world coordinate system to a frame camera coordinate system; is a set of parameters to be optimized; .

4. The SLAM method based on ground surface feature constraint and point-surface collaboration according to claim 1, characterized in that, Step four specifically comprises: Step 41, traverse the new point features in the current frame which are triangulated and not belong to the face features, find the point feature to the distance of the ground face feature in the frame , the distance The formula is: （4） wherein, ⊙ denotes the dot product between vectors, and the point feature inverse depth is taken as a parameter; Step 42, let be 0, and find the inverse depth of Add the new point feature to the set of face features.

5. The SLAM method based on ground surface feature constraint and point-surface collaboration according to claim 1, characterized in that, The residual equation of projecting the surface features to the tracked surface features is: (10) wherein, is a face feature tracked by the frame; is a projected face feature tracked by the frame.

6. A SLAM system based on face feature constraint and point-face collaborative optimization, characterized in that, Comprises: A memory for storing instructions executable by the processor; A processor for executing the instructions to implement the SLAM method based on ground surface feature constraint and point / surface collaborative optimization according to any one of claims 1-5.

Images