A Monocular Visual SLAM Method for Dynamic Environments

Abstract

The present invention discloses a monocular vision SLAM method for a dynamic environment, including: 1) initialization, including reading the first two frames of an image sequence, extracting and matching ORB feature points, establishing a world coordinate system, setting monocular scale information, establishing an initial map, constructing a key frame sequence and a key frame sliding window; 2) tracking a reference frame and estimating an initial pose; 3) eliminating dynamic feature points and optimizing the pose; 4) according to the tracking result, inserting a key frame and tracking a reference key frame, and then constructing map points and inserting them into the map; 5) eliminating dynamic map points on the sliding window and performing local bundle adjustment optimization; 6) eliminating redundant key frames and map points; 7) if the device computing power is sufficient, performing global bundle adjustment optimization; 8) repeating steps 2)-7) until all image frames in the sequence are processed. The present invention solves the problems of weak robustness and poor accuracy in positioning and mapping when the vision SLAM method is applied to a dynamic environment.

Description

Technical Field

[0001] The present invention relates to the technical field of visual simultaneous localization and mapping, and in particular to a monocular visual SLAM method for dynamic environments. Background Art

[0002] Visual Simultaneous Localization and Mapping (abbreviated as visual SLAM) is one of the key technologies for unmanned autonomous systems. Its function is to enable intelligent robots and the like to perceive the surrounding environmental information through visible light cameras in unfamiliar environments without relying on external devices, and thus complete the accurate measurement of their own poses and the accurate construction of environmental maps. In addition to intelligent robots, unmanned autonomous systems involving visual SLAM also include advanced technology fields such as autonomous driving, intelligent unmanned aerial vehicles, and AR / VR.

[0003] Currently, the main achievements of visual SLAM research are based on the assumption of static environments, and these methods can achieve good working effects in static environments. However, in practical applications, moving objects will inevitably appear. For example, driverless cars cannot drive on a completely static highway, and intelligent robots often encounter pedestrians, animals, vehicles and other dynamic objects during the task execution process. The dynamic objects that appear in the environment will cause the performance of the existing mainstream visual SLAM methods based on static assumptions to decline or even be unable to work. Therefore, the lack of consideration for moving objects limits the practical application of the current mainstream visual SLAM methods based on static environment assumptions.

[0004] Based on the above discussion, it is of high practical application value to invent a visual SLAM method that can work in dynamic environments. Summary of the Invention

[0005] The object of the present invention is to overcome the disadvantages and deficiencies of the prior art, and propose a monocular vision SLAM method for dynamic environments. According to the principle of motion consistency, dynamic feature points are identified and removed, and then the camera pose is estimated through static feature points and a sparse map is constructed. This method is executed in two threads: a tracking thread and a mapping thread. In the tracking thread, ORB feature extraction and matching, dynamic feature point filtering, initial pose estimation, and key frame decision are mainly completed. The core link among them is dynamic feature point filtering, and the feature points of fast-moving objects are identified and removed by comparing the geometric information and photometric information consistency of the matching feature points between the current frame and the previous frame. The mapping thread is executed after the tracking thread inserts a key frame, and mainly completes dynamic map point removal, static map construction, and global bundle adjustment optimization. The core link among them is dynamic map point removal, and the dynamic map points on slow-moving objects are identified and removed by comparing the geometric information consistency of the map points on the key frames in the sliding window. Through the dynamic point identification and removal operations in the above two places, the influence of dynamic feature points on the visual SLAM method is effectively reduced, and the pose estimation and sparse map construction are completed only by using static feature point information, improving the robustness and accuracy of the visual SLAM method in dynamic environments.

[0006] To achieve the above object, the technical solution provided by the present invention is: a monocular vision SLAM method for dynamic environments, including the following steps:

[0007] 1) Initialization, including reading the first two frames of the image sequence, extracting and matching ORB feature points, establishing a world coordinate system, setting monocular scale information, establishing an initial map, constructing a key frame sequence, and a key frame sliding window;

[0008] 2) Read the image sequence as the current frame, set the previous frame as the reference frame of the current frame, complete the feature matching between the current frame and the reference frame, and estimate the relative pose T k-1,k , where the current frame is the k-th frame and the reference frame is the (k - 1)-th frame;

[0009] 3) According to the relative pose in step 2), compare the geometric information consistency and photometric information consistency of the matching feature point pairs between the current frame and the reference frame, remove the inconsistent matching feature point pairs and the corresponding map points, and optimize the relative pose between the current frame and the reference frame according to the remaining matching feature point pairs by minimizing the reprojection error function;

[0010] 4) If the number of remaining matching feature point pairs in step 3) is less than the set threshold T m , or the number of frames between the current frame and the current key frame exceeds the threshold T n , then construct a new key frame according to the current frame;

[0011] 5) If a new key frame is constructed in step 4), insert the new key frame into the key frame sequence and the sliding window, set the new key frame as the current key frame, and set the previous key frame as the reference key frame of the current key frame; then, according to the optimized camera pose in step 3), perform feature point matching between the current key frame and the reference key frame. For the matching feature point pairs of the map points that have not been initialized, construct new map points by the triangulation method and insert them into the map M.

[0012] 6) Identify and remove the dynamic map points on the sliding window according to the geometric information consistency, and then perform local bundle adjustment optimization on the sliding window.

[0013] 7) If the number of key frames in the sliding window reaches the maximum value T K , and the pose T n,w of the current key frame and the pose T n-1,w of the reference key frame satisfy then remove the reference key frame in the sliding window. If then remove the key frame with the earliest time on the sliding window. Here, the operator ||·|| represents taking the matrix norm, and δ t is the set threshold; then traverse all key frames and map points. If more than 90% of the map points observed by a key frame are observed by at least three other key frames, then remove the key frame. If a map point is observed less than three times, then remove the map point.

[0014] 8) After completing step 7), start a new thread to perform a global bundle adjustment optimization; repeat steps 2) to 8) until all the images in the sequence are processed.

[0015] Furthermore, in step 1), convert the read RGB image into a grayscale image, continuously downsample the grayscale image to construct an image pyramid, extract the corresponding number of FAST corner points according to the resolution size of each layer of the image, and calculate the BRIEF descriptors of all corner points. Perform feature point matching between two frames according to the Hamming distance between the descriptors; then set the world coordinate system as the pose of the first frame, and calculate the pose of the second frame by random sample consensus, which specifically includes the following steps:

[0016] 1.1) Randomly select 8 pairs of matching feature points, and calculate the essential matrix E using the eight-point method 12 ;

[0017] 1.2) According to the result of step 1.1), check the epipolar constraint deviation i1 ,q i2 ) of all matching feature point pairs (q If e d > T d, the matching feature point pairs are marked as outliers, otherwise marked as inliers, where q i1 and q i2 are the homogeneous coordinates of the feature points of the first frame and the second frame respectively, K is the camera internal parameter matrix, and T d is the set threshold;

[0018] 1.3) According to the result of step 1.2), if the proportion of the number of inlier matching feature point pairs in the total number of matching feature point pairs exceeds the threshold T r , 0.7 < T r ≤1, perform singular value decomposition (SVD) on the essential matrix in step 1.1), and obtain the relative pose T 12 between the first frame and the second frame, where the translation vector in the pose is set as a unit vector, that is, the scale information is determined; if the inlier proportion is lower than T r , repeat steps 1.1) to 1.3) until the inlier proportion is not lower than T r ; if all combinations are selected and a combination that satisfies the inlier proportion greater than T r is still not found, select the essential matrix corresponding to the combination with the largest inlier proportion for singular value decomposition, and obtain T 12 ;

[0019] 1.4) According to the result of step 1.3), triangulate all inliers to calculate the 3D map points, and construct the initial map M; construct the key frame KF1 according to the first frame, insert it into the key frame sequence and the sliding window, and set this key frame as the current key frame.

[0020] Furthermore, in step 2), first convert the read RGB image to a grayscale image, continuously downsample to construct an image pyramid, extract the corresponding number of FAST corner points according to the resolution of each layer of the image, and calculate the BRIEF descriptors of all corner points; then find the best match in the reference frame for each extracted feature point through the BRIEF descriptor. If the Hamming distance of the best match is less than the threshold T o , retain the feature point match, otherwise discard the feature point match; finally, according to the random sample consensus strategy, use the Epnp algorithm to calculate the relative pose T k-1,k between the current frame and the reference frame, which specifically includes the following steps:

[0021] 2.1) Randomly select 4 pairs of matching feature points, and solve the pose T k-1,k through the Epnp algorithm;

[0022] 2.2) Calculate the reprojection error e i of the feature points in the current frame:

[0023]

[0024] where \(i\) represents the index number of the map point, \(N\) represents the number of 3D map points, and \(P\) i k-1 is the map point in the reference frame coordinate system, is the feature point in the current frame, and \(\pi(\cdot)\) is the dehomogeneous operation;

[0025] 2.3) If \(\left\|\mathbf{e}\right\|\) i \(>\) \(T\) l , then the matching feature point pair is marked as an outlier, otherwise it is marked as an inlier; calculate the total proportion of inliers. If it is greater than \(T\) n , then keep the pose in step 2.1). Otherwise, repeat steps 2.1) to 2.3) until the total proportion of inliers is greater than \(T\) n ; if after selecting all combinations, the condition still cannot be met, then select the pose with the highest inlier ratio as the estimated pose, where \(T\) l and \(T\) n are set thresholds.

[0026] Furthermore, in step 3), in order to eliminate the influence of feature points on fast-moving objects, dynamic feature point recognition and removal are performed through photometric information consistency and geometric information consistency, and the relative pose between the current frame and the reference frame is optimized, which specifically includes the following steps:

[0027] 3.1) According to the feature point pairs marked as inliers in step 2), calculate the pixel values of the image patches near the feature points. If the difference in the pixel values of the regions of the feature point pairs is greater than the threshold \(T\) g , then mark the corresponding matching feature point pair as a dynamic feature point and remove the matching feature point pair and its corresponding map point;

[0028] 3.2) For the feature point pairs that have passed the test in step 3.1) and have 3D map points, project the corresponding map points into the current frame and the reference frame and construct a reprojection error function. Finally, the spatial coordinates of the map points are iteratively optimized by minimizing the reprojection error function. The specific expression is as follows:

[0029]

[0030] The above formula is solved iteratively by the Gauss-Newton method; during the optimization process, if the deviation satisfies then mark \(P\) l k-1 as a dynamic map point, and then remove the map point and the corresponding matching feature point pair. In the formula, \(l\) represents the index number of the map point after inspection, \(l = 1,2,\cdots,N'\), \(\arg\min(\cdot)\) represents the parameter value when the function takes the minimum value, \(\pi(\cdot)\) represents the dehomogeneous transformation, and \(T\) k-1,k is the relative pose between the current frame and the reference frame, and are the feature points of the reference frame and the current frame respectively, P l k is the map point in the coordinate system of the k-th frame, N' is the number of remaining map points after inspection, and ΔP l k-1 represents the change in the position of the map point P l k-1 before and after optimization, and Ω l is the covariance matrix of the map point P l k-1 and T h is the set threshold, and K is the camera internal parameter;

[0031] 3.3) According to the filtered matching feature point pairs in step 3.2), optimize the relative pose by solving the reprojection error function, which is specifically expressed as:

[0032]

[0033] The above formula is iteratively solved by the Gauss-Newton method; in the formula, u represents the index number of the filtered map point, and N'' is the number of filtered map points.

[0034] Furthermore, in step 5), match the feature points of the current key frame with those of the reference key frame using the BRIEF descriptor, which specifically includes the following steps:

[0035] 5.1) According to the optimized relative pose in step 3), obtain the relative pose between the current key frame and the reference key frame by cumulative multiplication, and then calculate the essential matrix E = t^R between the two key frames, where t is the translation vector and R is the rotation matrix, and the symbol ∧ represents the skew-symmetric transformation;

[0036] 5.2) According to the essential matrix in step 5.1), calculate the corresponding two-dimensional coordinate points on the reference key frame image for the two-dimensional pixel coordinates of the feature points in the current key frame through the epipolar constraint, and find the best matching feature point pairs near the calculated two-dimensional coordinate points using the BRIEF descriptor. If the Hamming distance of the BRIEF descriptors of the best matching feature point pairs is less than the threshold T k , then retain the matching result, otherwise discard the matching result.

[0037] Furthermore, in step 6), check the spatial coordinate consistency of the 3D map points through non-linear optimization, identify and remove the slow-moving dynamic map points, and perform local bundle adjustment optimization, which specifically includes the following sub-steps:

[0038] 6.1) Project all the map points on the sliding window onto the observed key frames to obtain two-dimensional projection coordinates, and construct the reprojection error function e:

[0039]

[0040] In the formula, m represents the index number of the map point on the sliding window, m = 1, 2, ..., N a , j represents the index number of the key frame on the sliding window, j = 1, 2, ..., N s , N s is the number of key frames in the sliding window, N a is the number of map points in the sliding window, δ mj represents the reprojection error constructed by the m-th map point and the j-th key frame, and the specific expression is T j,w is the relative pose from the world coordinate system to the j-th key frame, is the map point in the world coordinate system, represents the feature point on the j-th key frame, π(·) represents the dehomogeneous transformation, and K is the camera internal parameter;

[0041] 6.2) Take the error function in step 6.1) as the objective function to be minimized, and iteratively optimize the three-dimensional map point coordinates. The specific expression is:

[0042]

[0043] The above formula is solved iteratively by the Gauss-Newton method; during the optimization process, if the deviation satisfies then is marked as a dynamic map point, and then this map point and the corresponding feature point are removed. Among them, argmin(·) represents the parameter value when the function takes the minimum value, is the map point in the world coordinate system, represents the map point before and after optimization the change amount of the position, Ω mw is the covariance matrix of the map point , T hw is the set threshold size;

[0044] 6.3) Optimize the key frame pose and map point coordinates on the sliding window by minimizing the reprojection error function, which is specifically expressed as:

[0045]

[0046] The above formula is solved by the Gauss-Newton method.

[0047] Furthermore, in step 8), a new thread is started to optimize all key frame poses and map point positions. The specific expression is as follows:

[0048]

[0049] The above formula is solved iteratively by the Gauss-Newton method; in the formula, g represents the index number of the key frame in the map, g = 1, 2,..., N sw , b represents the index number of the map point in the map, b = 1, 2,..., N aw , T g,w represents the relative pose from the world coordinate system to the g-th key frame, represents the map point coordinate, argmin(·) represents the parameter value when the function takes the minimum value, and e′ is the reprojection error function to be optimized, and its specific expression is δ bg is the map point and the key frame T g,w constructs the reprojection error, and the specific expression is if the key frame g observes the map point π(·) is the homogeneous transformation removal, K is the camera internal parameter, is the feature point of the g-th key frame, N sw is the number of key frames, N aw is the number of map points, is the map point in the world coordinate system.

[0050] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0051] 1. The present invention is a monocular vision SLAM method applicable to dynamic environments. Currently, the mainstream vision SLAM methods are all based on the assumption of static environments and pay less attention to the influence caused by dynamic objects.

[0052] 2. The present invention uses a monocular camera as the environmental perception device and can work well in both indoor and outdoor environments. Currently, many technical inventions of vision SLAM methods for dynamic environments are based on RGB-D cameras because these technologies must use the depth images provided by the cameras. Compared with such technologies, the novelty of the present invention lies in the different recognition strategies for dynamic feature points. The present invention identifies dynamic feature points by comparing the photometric value differences of feature point pairs and the large fluctuations of the corresponding map points during the optimization process, while other methods often need to directly compare the absolute differences of depth values.

[0053] 3. The monocular camera used in the present invention has the advantages of low price, small volume, and light weight. At the same time, the present invention can also be extended to binocular cameras and RGB-D cameras.

[0054] 4. The present invention designs different dynamic map point recognition and elimination strategies in the tracking thread and the mapping thread, comprehensively ensuring the robustness and running efficiency of visual SLAM. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1Flow chart of the method embodiment of the present invention. Detailed implementation manners

[0056] The present invention will be further described in detail below in conjunction with embodiments and the accompanying drawings, but the implementation manners of the present invention are not limited thereto.

[0057] This embodiment discloses a monocular vision SLAM method for a dynamic environment. As Figure 1 shown, this method is executed in two threads: a tracking thread and a mapping thread. In the tracking thread, monocular vision SLAM initialization, ORB feature extraction and matching, dynamic feature point filtering, initial pose estimation, and key frame decision-making are mainly completed. The core link among them is dynamic feature point filtering, which identifies and eliminates the feature points of fast-moving objects by comparing the geometric information and photometric information consistency of the matching feature points between the current frame and the previous frame. The mapping thread is executed after the tracking thread inserts a key frame, and mainly completes dynamic map point elimination, static map construction, and global bundle adjustment optimization. The core link among them is dynamic map point elimination, which identifies and eliminates the dynamic map points on slow-moving objects by comparing the geometric information consistency of the map points on the key frames in the sliding window. The specific process is as follows:

[0058] I. Tracking thread:

[0059] 1) Initialization, including reading the first two frames of the image sequence, extracting and matching ORB feature points, establishing a world coordinate system, setting monocular scale information, establishing an initial map, constructing a key frame sequence, and a key frame sliding window. The specific situation is as follows:

[0060] Convert the read RGB image into a grayscale image, continuously downsample the grayscale image to construct an image pyramid, extract the corresponding number of FAST corner points according to the resolution of each layer of the image, and calculate the BRIEF descriptors of all corner points. Feature point matching is performed between two frames according to the Hamming distance between the descriptors; subsequently, set the world coordinate system to the pose of the first frame, and calculate the pose of the second frame through random sample consensus. The specific steps include the following:

[0061] 1.1) Randomly select 8 pairs of matching feature points, and calculate the essential matrix E using the eight-point method 12 ;

[0062] 1.2) According to the result of step 1.1), check the epipolar constraint deviation of all matching feature point pairs (q i1 , q i2 ) If e d > T d , then mark the matching feature point pair as an outlier, otherwise mark it as an inlier. Among them, q i1 and qi2 are the homogeneous coordinates of the feature points of the first frame and the second frame respectively, K is the camera internal parameter matrix, and T d is the set threshold;

[0063] 1.3) According to the result of step 1.2), if the proportion of the number of pairs of feature points with the inner perimeter value matching in the total number of pairs of matching feature points exceeds the threshold T r , 0.7 < T r ≤1 (preferably 0.8), then perform singular value decomposition (SVD) on the essential matrix in step 1.1), and obtain the relative pose T 12 between the first frame and the second frame. Among them, let the translation vector in the pose be a unit vector, that is, the scale information is determined; if the proportion of the inner perimeter value is lower than T r , then repeat steps 1.1) to 1.3) until the proportion of the inner perimeter value is not lower than T r ; if all combinations are selected and a combination that satisfies the proportion of the inner perimeter value greater than T r is still not found, then select the essential matrix corresponding to the combination with the largest proportion of the inner perimeter value for singular value decomposition, and obtain T 12 ;

[0064] 1.4) According to the result of step 1.3), triangulate all the inner perimeter values to calculate the 3D map points, and construct the initial map M; construct the key frame KF1 according to the first frame, insert it into the key frame sequence and the sliding window (the length of the sliding window is set to 20), and set this key frame as the current key frame.

[0065] 2) Read the image sequence as the current frame, set the previous frame as the reference frame of the current frame, complete the feature matching between the current frame and the reference frame, and estimate the relative pose T k-1,k , where the current frame is the k-th frame and the reference frame is the (k - 1)-th frame, specifically as follows:

[0066] First, convert the read RGB image to a grayscale image, use a scaling factor of 1.2, continuously downsample 7 times, and construct an 8-layer image pyramid; extract the corresponding number of FAST corner points according to the resolution of each layer of the image, extract a total of 1000 FAST corner points, and calculate the BRIEF descriptors of all the corner points; then find the best match for each extracted feature point in the reference frame through the BRIEF descriptor. If the Hamming distance of the best match is less than the threshold T o , then retain the feature point match, otherwise discard the feature point match; finally, according to the random sample consensus strategy, use the Epnp algorithm to calculate the relative pose T k-1,k between the current frame and the reference frame, which specifically includes the following steps:

[0067] 2.1) Randomly select 4 pairs of matching feature points, and solve for the pose T through the Epnp algorithmk-1,k ;

[0068] 2.2) Calculate the reprojection error e of the feature points in the current frame i :

[0069]

[0070] In the formula, i represents the index number of the map point, N represents the number of 3D map points, and P i k-1 is the map point in the reference frame coordinate system, is the feature point in the current frame, and π(·) is the dehomogeneous operation;

[0071] 2.3) According to the result of step 2.2), if ||e i || > T l (preferably 5), then the matching feature point pair is marked as an outlier, otherwise it is marked as an inlier; calculate the total proportion of inliers. If it is greater than T n (preferably 0.8), then retain the pose in step 2.1). Otherwise, repeat steps 2.1) to 2.3) until the total proportion of inliers is greater than T n ; if after selecting all combinations, the condition still cannot be met, then select the pose with the highest proportion of inliers as the estimated pose, where T l and T n are set thresholds.

[0072] 3) According to the relative pose in step 2), compare the geometric information consistency and photometric information consistency of the matching feature point pairs between the current frame and the reference frame, remove the inconsistent matching feature point pairs and the corresponding map points, and optimize the relative pose between the current frame and the reference frame by minimizing the reprojection error function according to the remaining matching feature point pairs. The specific steps are as follows:

[0073] 3.1) According to the feature point pairs marked as inliers in step 2), calculate the sum of the gray values of the 5×5 pixel block centered on the feature point. If the difference in the pixel values of the regions of the feature point pair is greater than the threshold T g , then mark the corresponding matching feature point pair as a dynamic feature point and remove the matching feature point pair and its corresponding map point;

[0074] 3.2) For the feature point pairs that have passed the test in step 3.1) and have 3D map points, project the corresponding map points into the current frame and the reference frame and construct a reprojection error function. Finally, iteratively optimize the spatial coordinates of the map points by minimizing the reprojection error function. The specific expression is as follows:

[0075]

[0076] The above formula is solved iteratively by the Gauss-Newton method; during the optimization process, if the deviation satisfies Then mark P l k-1 as a dynamic map point, and then remove this map point and the corresponding matching feature point pair. In the formula, l represents the index number of the map point after inspection, l = 1, 2,..., N′, arg min(·) represents the parameter value when the function takes the minimum value, π(·) represents the de - homogeneous transformation, T k-1,k is the relative pose between the current frame and the reference frame, and are the feature points of the reference frame and the current frame respectively, P l k is the map point in the coordinate system of the k - th frame, N′ is the number of remaining map points after inspection, ΔP l k-1 represents the change in the position of the map point P l k-1 before and after optimization, Ω l is the covariance matrix of the map point P l k-1 h is the set threshold, and K is the camera internal parameter;

[0077] 3.3) According to the filtered matching feature point pairs in step 3.2), optimize the relative pose by solving the reprojection error function, which is specifically expressed as:

[0078]

[0079] The above formula is iteratively solved by the Gauss - Newton method; in the formula, u represents the index number of the filtered map point, and N” is the number of filtered map points.

[0080] 4) If the number of remaining static matching feature point pairs in step 3) is less than 50, or the number of frames between the current frame and the current key frame exceeds 10, then construct a new key frame according to the current frame.

[0081] II. Mapping thread:

[0082] 5) If a new key frame is constructed in step 4), insert the new key frame into the key frame sequence and the sliding window, set the new key frame as the current key frame, and set the previous key frame as the reference key frame of the current key frame; according to the optimized relative pose in step 3), obtain the relative pose between the current key frame and the reference key frame by cumulative multiplication, and then calculate the essential matrix E = t^R between the two key frames, where t is the translation vector and R is the rotation matrix, and the symbol ∧ represents the skew-symmetric transformation; according to the essential matrix, calculate the corresponding two-dimensional coordinate points on the reference key frame image through the epipolar constraint for the two-dimensional pixel coordinates of the feature points in the current key frame, and search for the best matching feature point pairs near the calculated two-dimensional coordinate points through the BRIEF descriptor. If the Hamming distance of the BRIEF descriptors of the best matching feature point pairs is less than the threshold T k , then retain the matching result, otherwise discard the matching result; for the matching pairs of map points that have not been initialized, construct new map points through the triangulation method and insert them into the map M.

[0083] 6) According to the geometric information consistency, verify the spatial coordinate consistency of the three-dimensional map points through non-linear optimization, identify and remove the dynamic map points with slow movement, and perform local bundle adjustment optimization. The specific steps are as follows:

[0084] 6.1) Project all the map points on the sliding window onto the observed key frames to obtain two-dimensional projection coordinates, and construct the reprojection error function e:

[0085]

[0086] In the formula, m represents the index number of the map point on the sliding window, m = 1, 2,..., N a , j represents the index number of the key frame on the sliding window, j = 1, 2,..., N s , N s is the number of key frames in the sliding window, N a is the number of map points in the sliding window, δ mj represents the reprojection error constructed by the m-th map point and the j-th key frame, and the specific expression is if the key frame j observes the map point P i w , T j,w is the relative pose from the world coordinate system to the j-th key frame, is the map point in the world coordinate system, represents the feature point on the j-th key frame, π(·) represents the dehomogeneous transformation, and K is the camera internal parameter;

[0087] 6.2) Take the error function in step 6.1) as the objective function to be minimized, and iteratively optimize the three-dimensional map point coordinates. The specific expression is:

[0088]

[0089] The above formula is iteratively solved by the Gauss-Newton method; during the optimization process, if the deviation satisfies then is marked as a dynamic map point, and then this map point and the corresponding feature points are removed, where arg min(·) represents the parameter value when the function takes the minimum value. is a map point in the world coordinate system. represents the change in the position of the map point before and after optimization, and Ω mw is the covariance matrix of the map point , and T hw is the set threshold value.

[0090] 6.3) Optimize the poses of key frames and the coordinates of map points on the sliding window by minimizing the reprojection error function, which is specifically expressed as:

[0091]

[0092] The above formula is solved by the Gauss-Newton method.

[0093] 7) If the number of key frames in the sliding window reaches 20, and the pose T n,w of the current key frame is close to the pose T n-1,w of the reference key frame, that is then remove the reference key frame in the sliding window; if the number of key frames in the sliding window reaches 20, and the difference between the pose of the current key frame and the pose of the reference key frame is large, that is then remove the key frame with the earliest time on the sliding window; traverse all the key frames in the key frame sequence, if more than 90% of the map points observed by a key frame are observed by at least three other key frames, then remove this key frame; traverse the map, if the number of observations of a map point is less than three, then remove this map point.

[0094] 8) After completing step 7), a new thread is started for a global bundle adjustment optimization, and the specific expression is:

[0095]

[0096] The above formula is iteratively solved by the Gauss-Newton method; in the formula, g represents the index number of the key frame in the map, g = 1, 2,..., N sw , b represents the index number of the map point in the map, b = 1, 2,..., N aw , T g,w represents the relative pose from the world coordinate system to the g-th key frame. Indicates the coordinates of a map point. arg min(·) represents the parameter value when the function takes the minimum value. e′ is the reprojection error function to be optimized, and its specific expression is δ bg is the map point and the key frame T g,w constructed reprojection error, and its specific expression is if the key frame g observes the map point π(·) is the dehomogeneous transformation, and K is the camera internal parameter is the feature point of the g-th key frame, N sw is the number of key frames, N aw is the number of map points is the map point in the world coordinate system; repeat steps 2) to 8) until all the image frames in the sequence are processed.

[0097] Finally, the above monocular visual SLAM method for dynamic environments in this embodiment obtains a globally consistent camera pose estimation in the dynamic environment and constructs a sparse map.

[0098] In summary, through the above two dynamic point recognition and elimination operations, the present invention effectively reduces the influence of dynamic feature points on the visual SLAM method, and only uses static feature point information to complete pose estimation and sparse map construction, improving the robustness and accuracy of the visual SLAM method in dynamic environments, and solving the problems of weak robustness and poor accuracy in positioning and mapping when the visual SLAM method is applied to dynamic environments, which is worthy of promotion.

[0099] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principle of the present invention shall be equivalent replacement methods and are all included in the protection scope of the present invention.

Claims

1. A monocular vision SLAM method for dynamic environments, characterized in that, Including the following steps: 1) Initialization, including reading the first two frames of the image sequence, extracting and matching ORB feature points, establishing a world coordinate system, setting monocular scale information, establishing an initial map, constructing a key frame sequence, and a key frame sliding window; 2) Read the image sequence as the current frame, set the previous frame as the reference frame for the current frame, complete the feature matching between the current frame and the reference frame, and estimate the relative pose T k-1,k , where the current frame is the k-th frame and the reference frame is the (k-1)-th frame; 3) According to the relative pose in step 2), compare the geometric information consistency and photometric information consistency of the matching feature point pairs between the current frame and the reference frame, eliminate the inconsistent matching feature point pairs and the corresponding map points, and optimize the relative pose between the current frame and the reference frame by minimizing the reprojection error function based on the remaining matching feature point pairs; 4) If the number of remaining matching feature point pairs in step 3) is less than the set threshold T m , or the number of frames between the current frame and the current key frame exceeds the threshold T n ', then a new key frame is constructed based on the current frame; 5) If a new key frame is constructed in step 4), insert the new key frame into the key frame sequence and the sliding window, set the new key frame as the current key frame, and set the previous key frame as the reference key frame of the current key frame; then, according to the optimized camera pose in step 3), perform feature point matching between the current key frame and the reference key frame. For the matching feature point pairs of the map points that have not been initialized, construct new map points by triangulation and insert them into the map M; 6) Identify and eliminate the dynamic map points on the sliding window according to the geometric information consistency, and then perform local bundle adjustment optimization on the sliding window; 7) If the number of key frames in the sliding window reaches the maximum value T K , and the pose T of the current key frame n,w and the pose T of the reference key frame n-1,w satisfy then the reference key frame in the sliding window is removed. If then the key frame with the earliest time in the sliding window is removed. Among them, the operator ||·|| represents taking the matrix norm, and δ t is the set threshold; then all key frames and map points are traversed. If more than 90% of the map points observed by key frame A are observed by at least three other key frames, then key frame A is removed. If the number of observations of map point B is less than three, then map point B is removed; 8) After completing step 7), start a new thread to perform a global bundle adjustment optimization; repeat steps 2) to 8) until all the images in the sequence are processed.

2. The monocular vision SLAM method for a dynamic environment according to claim 1, wherein In step 1), convert the read RGB image into a grayscale image, continuously downsample the grayscale image to construct an image pyramid, extract the corresponding number of FAST corner points according to the resolution size of each layer of the image, calculate the BRIEF descriptors of all the corner points, and perform feature point matching between two frames according to the Hamming distance between the descriptors; subsequently, set the world coordinate system as the pose of the first frame, and calculate the pose of the second frame by random sample consensus, which specifically includes the following steps: 1.1) Randomly select 8 pairs of matching feature points and calculate the essential matrix E using the eight-point method 12 ; 1.2) According to the result of step 1.1), check the epipolar constraint deviation of all matched feature point pairs (q i1 , q i2 ). If e d > T d , mark the matched feature point pair as an outlier, otherwise mark it as an inlier, where q i1 and q i2 are the homogeneous coordinates of the feature points in the first and second frames respectively, K is the camera intrinsic matrix, and T d is the set threshold. 1.3) According to the result of step 1.2), if the proportion of the number of pairs of feature points with the inner perimeter value matching in the total number of pairs of matched feature points exceeds the threshold T r , 0.7 < T r ≤ 1, then perform singular value decomposition on the essential matrix in step 1.1) and obtain the relative pose T 12 of the first frame and the second frame, where the translation vector in the pose is made a unit vector, that is, the scale information is determined; if the inner perimeter value proportion is lower than T r , then repeat steps 1.1) to 1.3) until the inner perimeter value proportion is not lower than T r ; if all combinations have been selected and a combination satisfying the inner perimeter value proportion greater than T r has not been found, then select the essential matrix corresponding to the combination with the largest inner perimeter value proportion for singular value decomposition and obtain T 12 ; 1.4) According to the results of step 1.3), perform triangulation calculation on all the inner values to obtain three-dimensional map points, and construct the initial map M; construct the key frame KF1 according to the first frame, insert it into the key frame sequence and the sliding window, and set the key frame KF1 as the current key frame.

3. A monocular vision SLAM method for a dynamic environment according to claim 2, characterized in that, In step 2), first convert the read RGB image into a grayscale image, continuously downsample to construct an image pyramid, extract a corresponding number of FAST corner points according to the resolution of each layer of the image, and calculate the BRIEF descriptors of all corner points; then find the best feature point match in the reference frame for each extracted feature point through the BRIEF descriptor. If the Hamming distance of the best feature point match is less than the threshold T o , then retain the best feature point match, otherwise discard the best feature point match; finally, according to the random sample consensus strategy, use the Epnp algorithm to calculate the relative pose T k-1,k between the current frame and the reference frame, which specifically includes the following steps: 2.1) Randomly select 4 pairs of matching feature points, and solve for the pose T through the Epnp algorithm k-1,k ; 2.2) Calculate the reprojection error e of the feature points in the current frame i : where \(i\) represents the index number of the map point, \(N\) represents the number of 3D map points, and \(P\) i k-1 is the map point in the reference frame coordinate system, is the feature point in the current frame, and \(\pi(\cdot)\) is the de - homogeneous operation; 2.3) If ||e i || > T l , the matching feature point pairs are marked as outliers, otherwise marked as inliers; calculate the total proportion of inliers. If it is greater than T n , keep the pose in step 2.1), otherwise, repeat steps 2.1) to 2.3) until the total proportion of inliers is greater than T n ; if all combinations are selected and the condition still cannot be met, select the pose with the highest inlier ratio as the estimated pose, where T l and T n are set thresholds.

4. A monocular vision SLAM method for a dynamic environment according to claim 3, characterized in that In step 3), in order to eliminate the influence of the feature points on fast-moving objects, identify and eliminate dynamic feature points through photometric information consistency and geometric information consistency, and optimize the relative pose between the current frame and the reference frame, which specifically includes the following steps: 3.1) Calculate the pixel values of the image patches near the feature points according to the feature point pairs marked as the inner perimeter values in step 2). If the difference in the pixel values of the regions of the feature point pairs is greater than the threshold T g , then mark the corresponding matching feature point pairs as dynamic feature points and remove the matching feature point pairs and their corresponding map points; 3.2) For the feature point pairs that have passed the inspection in step 3.1) and have three-dimensional map points, project the corresponding map points into the current frame and the reference frame and construct a reprojection error function, and finally iteratively optimize the spatial coordinates of the map points by minimizing the reprojection error function. The specific expression is as follows: The above formula is solved iteratively by the Gauss-Newton method; during the optimization process, if the deviation satisfies then mark P l k-1 as a dynamic map point, and then remove the dynamic map point P l k-1 and the corresponding matching feature point pairs. In the formula, l represents the index number of the map point after inspection, l = 1, 2,..., N′, argmin(·) represents the parameter value when the function takes the minimum value, π(·) represents the dehomogeneous transformation, T k-1,k is the relative pose between the current frame and the reference frame, and are the feature points of the reference frame and the current frame respectively, P l k-1 is the map point in the coordinate system of the (k - 1)-th frame, N' is the number of remaining map points after inspection, ΔP l k-1 represents the change in the position of the map point P l k-1 before and after optimization, Ω l is the covariance matrix of the map point P l k-1 T h is the set threshold, and K is the camera internal parameter; 3.3) According to the filtered matching feature point pairs in step 3.2), optimize the relative pose by solving the reprojection error function, which is specifically expressed as: The above formula is solved iteratively by the Gauss-Newton method; in the formula, u represents the index number of the filtered map points, and N” is the number of filtered map points.

5. A monocular vision SLAM method for a dynamic environment according to claim 4, characterized in that, In step 5), match the feature points of the current key frame and the feature points of the reference key frame using the BRIEF descriptor, which specifically includes the following steps: 5.1) According to the optimized relative pose in step 3), the relative pose between the current key frame and the reference key frame is obtained by cumulative multiplication, and then the essential matrix E = t ∧ R is calculated, where t is the translation vector, R is the rotation matrix, and the symbol ∧ represents the skew-symmetric transformation; 5.2) Based on the essential matrix in step 5.1), the two-dimensional pixel coordinates of the feature points in the current key frame are calculated through the epipolar constraint to obtain the corresponding two-dimensional coordinate points on the reference key frame image, and the best matching feature point pairs are searched near the calculated two-dimensional coordinate points through the BRIEF descriptor. If the Hamming distance of the BRIEF descriptors of the best matching feature point pairs is less than the threshold T k , the matching result is retained; otherwise, the matching result is discarded.

6. A monocular vision SLAM method for a dynamic environment according to claim 5, characterized in that, In step 6), the spatial coordinate consistency of the three-dimensional map points is verified through non-linear optimization, the dynamic map points with slow movement are identified and removed, and local bundle adjustment optimization is performed, which specifically includes the following steps: 6.1) Project all the map points on the sliding window onto the observed key frames to obtain two-dimensional projection coordinates, and construct a reprojection error function e: where \(m\) represents the index number of the map point on the sliding window, \(m = 1,2,\cdots,N\) a , \(j\) represents the index number of the key frame on the sliding window, \(j = 1,2,\cdots,N\) s , \(N\) s is the number of key frames in the sliding window, \(N\) a is the number of map points in the sliding window, \(\delta\) mj represents the reprojection error constructed by the \(m\)-th map point and the \(j\)-th key frame, and the specific expression is if the key frame \(j\) observes the map point \(T\) j,w is the relative pose from the world coordinate system to the \(j\)-th key frame, is the map point in the world coordinate system, represents the feature point on the \(j\)-th key frame, \(\pi(\cdot)\) represents the dehomogeneous transformation, and \(K\) is the camera internal parameter; 6.2) Take the error function in step 6.1) as the objective function to be minimized, and iteratively optimize the three-dimensional map point coordinates. The specific expression is: The above equation is solved iteratively by the Gauss-Newton method; during the optimization process, if the deviation satisfies then is marked as a dynamic map point, and then this dynamic map point and the corresponding feature points are removed, where argmin(·) represents the parameter value when the function takes the minimum value, is the map point in the world coordinate system, represents the change in the position of the map point before and after optimization , Ω mw is the covariance matrix of the map point , and T hw is the set threshold size; 6.3) Optimize the key frame poses and map point coordinates on the sliding window by minimizing the reprojection error function, which is specifically expressed as: The above formula is solved by the Gauss-Newton method.

7. A monocular vision SLAM method for a dynamic environment according to claim 6, characterized in that, In step 8), a new thread is started to optimize all the key frame poses and map point positions. The specific expression is as follows: The above equation is solved iteratively by the Gauss-Newton method; in the equation, g represents the index number of the key frames in the map, g = 1, 2, …, N sw , b represents the index number of the map points in the map, b = 1, 2, …, N aw , T g,w represents the relative pose from the world coordinate system to the g-th key frame, represents the map point coordinates, argmin(·) represents the parameter value when the function takes the minimum value, and e′ is the reprojection error function to be optimized, and its specific expression is δ bg is the map point P b w and the key frame T g,w constructs the reprojection error, and its specific expression is if the key frame g observes the map point π(·) is the dehomogeneous transformation, K is the camera internal parameter, is the feature point of the g-th key frame, N sw is the number of key frames, N aw is the number of map points, is the map point in the world coordinate system.

Images