Dynamic scene slam positioning method based on target detection algorithm yolov4 and geometric constraint

Abstract

The application discloses a dynamic scene SLAM positioning method based on a target detection algorithm YOLOv4 and geometric constraints. Firstly, an image is collected through a visual sensor, a potential moving object area in the image sequence is recognized by using the YOLOv4 target detection algorithm, feature points of a part outside the area are extracted, and the matching feature points are calculated by using an optical flow method. After a transformation matrix of two frames is calculated, whether the feature points are dynamic points is judged by using an epipolar geometry epipolar constraint. When the epipolar constraint is invalid due to degenerate motion, whether the feature points are dynamic feature points is judged by using a boundary constraint. Finally, a dynamic object area is determined in combination with a target detection result, and after the corresponding area is removed, tracking, local mapping and loop detection threads are sequentially performed, a SLAM algorithm for a dynamic scene is realized, and the existing visual SLAM algorithm is inaccurate in pose estimation in a dynamic environment and is prone to be lost. The application significantly improves the visual SLAM pose precision in a dynamic scene while ensuring real-time performance, and can smoothly run on an embedded system.

Description

A Dynamic Scene SLAM Localization Method Based on YOLOv4 Object Detection Algorithm and Geometric Constraints Technical Field

[0001] This invention belongs to the field of computer vision and robot localization, specifically involving a dynamic scene SLAM localization method based on the target detection algorithm YOLOv4 and geometric constraints. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology applied to intelligent robots. It enables robots to estimate their pose and build maps of their surroundings in real time during movement, even in completely unfamiliar environments, using their onboard sensors. Visual SLAM uses low-cost cameras and can acquire richer semantic and image texture information compared to laser SLAM. Furthermore, with the continuous advancement of computer hardware, visual SLAM technology has become a hot research topic.

[0003] Most visual SLAM algorithms are based on the assumption of a static environment, but in real-world applications, moving objects such as pedestrians and vehicles are unavoidable. When moving objects occupy a small proportion of the image captured by the camera, outliers can be eliminated using the Random Sample Consensus (RANSAC) estimation algorithm, reducing their impact on inter-frame image matching. However, when moving objects constitute a large proportion, the RANSAC algorithm may treat dynamic feature points as inliers, filtering out the necessary static feature points, which has a counterproductive effect and causes the SLAM system to fail. Because it cannot distinguish between the feature points of dynamic and static objects, the robustness of traditional feature-point-based visual SLAM techniques is greatly affected.

[0004] To improve system stability, accurately removing feature points of dynamic objects in the environment is crucial. Current solutions for dynamic scenes include traditional geometric methods and deep learning-based methods. Traditional geometric methods struggle to guarantee accuracy in identifying dynamic objects and are incomplete in removing dynamic points. Deep learning-based object detection methods, relying solely on category labels to determine dynamic objects, are too coarse and prone to omissions. More refined instance segmentation methods consume excessive computational resources. Therefore, a solution suitable for outdoor drone operations is needed, ensuring both accuracy and recognition performance within limited computational resources. Summary of the Invention

[0005] To address the issue that traditional visual SLAM cannot guarantee accuracy and robustness in dynamic scenes.

[0006] The technical solution adopted in this invention is as follows:

[0007] A dynamic scene SLAM localization method based on the YOLOv4 object detection algorithm and geometric constraints includes the following steps:

[0008] Step S1: Read the image sequence through the visual sensor, use the YOLOv4 object detection algorithm to identify all classified and trained objects in the image and their bounding box positions, and proceed to step S4; if no object is detected, proceed to step S2.

[0009] Step S2: If no object can be detected in the current frame, the YOLOv4 object detection algorithm fails to recognize the image network, and proceed to step S3;

[0010] Step S3: Determine whether there is a dynamic object in the previous frame; if so, match the two frames according to the optical flow method, and shift the bounding box of the dynamic object in the previous frame accordingly, then proceed to step S4; if not, the current frame process ends.

[0011] Step S4: Extract feature points and perform optical flow matching on the part outside the bounding box region of the potential moving object, and calculate the basic transformation matrix between the two frames;

[0012] Step S5: and For a pair of feature points matched using optical flow in two adjacent image frames, the solution is obtained based on the transformation matrix. The epipolar line corresponding to the feature point in another frame image ;

[0013] Step S6: Based on feature points to the poles To determine whether a feature point is a dynamic feature point, we need to check if the distance exceeds a threshold.

[0014] Step S7: If degenerate motion occurs, that is, when the camera and the object undergo a pure translational motion parallel to each other, the image of that point moves along the epipolar line. In this case, it is impossible to determine whether the feature point has moved based on the distance.

[0015] Step S8: Use the boundary constraint method to determine whether the feature point has moved by limiting the maximum and minimum values ​​of the feature point displacement;

[0016] Step S9: If and For point The image points on the two frames of the image, then Image point coordinates are represented as ;

[0017] Step S10: Obtain the result using the boundary constraint method formula. If the pixel coordinates of a point are outside the range, then the feature point has moved and is a dynamic feature point.

[0018] Step S11: If the proportion of dynamic feature points within the bounding box of an object exceeds a threshold in step S1, the object is considered a dynamic object.

[0019] Step S12: Perform subsequent tracking, local mapping, and loop closure detection threads on the area outside the bounding box of the dynamic object.

[0020] Preferably, step S7 specifically involves the following steps: when degenerate motion occurs, the polar constraint can no longer determine whether the feature points of the object are dynamic feature points, and the boundary constraint method is used to determine whether the object has moved by constraining the displacement of the feature points.

[0021] Preferably, the boundary constraint method formula in step S10 is:

[0022]

[0023] in, For the camera intrinsic parameter matrix, Let be the camera translation vector. For point Relative to the camera's depth.

[0024] Because of the adoption of the above technical solution, the present invention has the following advantages:

[0025] The dynamic scene SLAM method of this invention, which combines object detection and geometric constraints, is more flexible than methods that rely solely on object detection and category labels to identify dynamic objects, and more accurate than methods that rely solely on geometric constraints. Furthermore, it maintains stability in recognition even when the object detection network temporarily fails, and it also maintains stability when degenerate motion epipolar constraints fail. Therefore, this invention can operate with high precision, robustness, and real-time performance in dynamic environments. Attached Figure Description

[0026] Figure 1 is a flowchart of a dynamic SLAM process that combines target detection and geometric constraints as disclosed in this invention.

[0027] Figure 2 shows the effect of dynamic object detection and removal of dynamic feature points in the method of the present invention. Detailed Implementation

[0028] The present invention will be further illustrated below with reference to specific examples and accompanying drawings. The embodiments are implemented based on the technical solutions of the present invention. It should be understood that these embodiments are only used to illustrate the present invention and are not intended to limit the scope of the present invention.

[0029] A dynamic scene SLAM localization method based on the YOLOv4 target detection algorithm and geometric constraint algorithm includes the following steps:

[0030] Step S1: Read the image sequence through the visual sensor, use the YOLOv4 object detection algorithm to identify all classified and trained objects in the image and their bounding box positions, and proceed to step S4; if no object is detected, proceed to step S2; Step S2: If no object can be detected in the current frame, the YOLOv4 object detection algorithm network for identifying the image fails, and proceed to step S3;

[0031] Step S3: Determine whether there is a dynamic object in the previous frame; if so, match the two frames according to the optical flow method, and shift the bounding box of the dynamic object in the previous frame accordingly, then proceed to step S4; if not, the current frame process ends.

[0032] Step S4: Extract feature points and perform optical flow matching on the part outside the bounding box region of the potential moving object, and calculate the basic transformation matrix between the two frames;

[0033] Step S5: and For a pair of feature points matched using optical flow in two adjacent image frames, the solution is obtained based on the transformation matrix. The epipolar line corresponding to the feature point in another frame image ;

[0034] Step S6: Based on feature points to the poles To determine whether a feature point is a dynamic feature point, we need to check if the distance exceeds a threshold.

[0035] Step S7: If degenerate motion occurs, that is, when the camera and the object undergo a pure translational motion parallel to each other, the image of that point moves along the epipolar line. In this case, it is impossible to determine whether the feature point has moved based on the distance.

[0036] Step S8: Use the boundary constraint method to determine whether the feature point has moved by limiting the maximum and minimum values ​​of the feature point displacement;

[0037] Step S9: If and For point The image points on the two frames of the image, then Image point coordinates are represented as ;

[0038] Step S10: Obtain the result using the boundary constraint method formula. If the pixel coordinates of a point are outside the range, then the feature point has moved and is a dynamic feature point.

[0039] Step S11: If the proportion of dynamic feature points within the bounding box of an object exceeds a threshold in step S1, the object is considered a dynamic object.

[0040] Step S12: Perform subsequent tracking, local mapping, and loop closure detection threads on the area outside the bounding box of the dynamic object.

[0041] Furthermore, in steps S1-S4, among the 80 objects pre-trained by YOLOv4, moving objects such as people and animals, as well as potentially moving objects such as cars, tables and chairs, and books, are selected, and the area outside the bounding box of these objects in the image is identified as the background area.

[0042] The YOLOv4 network may fail. If no object is detected in the current frame, but the algorithm determined that a moving object existed in the previous frame, optical flow is used to determine the motion of points within the bounding box, and the bounding box is moved for subsequent calculations. The specific process is as follows:

[0043] Feature points are extracted from the bounding box region of the dynamic object in the previous frame, and optical flow is used to track these feature points. Optical flow assumes that the pixel grayscale value of the same spatial point remains constant across different images. At that moment, located The grayscale value of a pixel at a given location can be written as:

[0044] (1)

[0045] Assumption Moment pixel movement to At this point, based on the assumption of invariant grayscale:

[0046] (2)

[0047] Taylor expansion yields:

[0048] (3)

[0049] Based on the assumption of invariant gray level, the following calculations are performed:

[0050] (4)

[0051] Simultaneously have to:

[0052] (5)

[0053] In formula (5) and For pixel along shaft and The speed of the axis is denoted as and . and The grayscale value of the image at this pixel shaft and The gradient change of the axis is denoted as... and .Will Recorded as We obtain the following formula:

[0054] (6)

[0055] In the optical flow method, assuming that pixels within the same sub-image have the same motion, each pixel can be represented by equation (6), which can be solved using the least squares method. and That is, displacement change and .

[0056] For each detected object, the YOLOv4 object detection algorithm returns the coordinates of the top-left corner of the object's bounding box. bounding box width and height Optical flow tracing is performed on the feature points within the bounding box of the moving object detected in the previous frame, and the average displacement change of all points is calculated. and The bounding box is moved by the corresponding pixel value and used as the current frame bounding box for subsequent calculations.

[0057] In steps S4-S6, feature extraction and optical flow tracking are performed on the background region outside the bounding box of the potential moving object, and then the fundamental matrix F is estimated using the eight-point method. and For a pair of feature points matched using optical flow in two adjacent frames, the coordinates in the normalized plane are... and It can be represented as

[0058] (7)

[0059] in and Represents the coordinate values ​​of a pixel, epipolar lines It can be represented as:

[0060] (8)

[0061] Feature points arrive Corresponding polar lines The distances are as follows:

[0062] (9)

[0063] By setting a threshold Determine the state of the feature points, if Then the feature points are considered to be For dynamic feature points, if , considering feature points It is a stationary point.

[0064] In steps S7-S9, when the camera undergoes a pure translational motion parallel to the object, the image of that point moves along the epipolar line. At this point, it's impossible to determine whether the feature point has moved based on distance alone. Therefore, a boundary constraint method is used to determine whether the feature point has moved by limiting the maximum and minimum values ​​of its displacement. Assuming the camera speed remains constant for a short period, the pose calculated in the previous frame is used to estimate the camera translation in the current frame. . and For point Image points on two frames of images, Pixel coordinates can be represented as , For the camera intrinsic parameter matrix, The depth of an object from the camera can be expressed by the following formula:

[0065] (10)

[0066] (11)

[0067] By combining the two equations, we can obtain:

[0068] (12)

[0069] This formula describes the movement of an image point within an image frame when a spatial point is stationary and the camera is translating. The image point moves from the previous frame... Begin, along the path from With the extreme point Defined linear motion, the range of which is related to the amount of translation. The value is proportional to the magnitude and the depth. Inversely proportional. If given three-dimensional points... By assuming a depth range, image points can be predicted. The location on the epipolar line. Let the upper and lower limits of the image point depth be respectively... and The corresponding minimum and maximum displacements of the image point on the epipolar line are: and If point The displacement is not If the points are between a certain value and a certain value, then the point is considered a dynamic point.

[0070] In steps S10-S12, the number of dynamic feature points within the bounding box of each object is recorded as follows: The total number of feature points is If the proportion of dynamic feature points If the threshold is exceeded, the object is determined to be a dynamic object.

[0071] The process of tracking, local mapping, and loop closure detection of the area outside the bounding box of a dynamic object.

[0072] When implementing this technical solution

[0073] As shown in Figure 1, a dynamic scene SLAM localization method based on the YOLOv4 object detection algorithm and geometric constraint algorithm includes four parts: dynamic object detection module, tracking, local mapping and loop closure detection.

[0074] This example uses the TUM RGB-D dataset released by the Technical University of Munich (TUM) laboratory. This dataset consists of 39 sequences recorded by a Kinect depth camera. Each sequence includes a color image, a depth image, and the camera's true pose at the same timestamp. The walking sequence in this dataset is often used to measure the accuracy of dynamic SLAM. This particular sequence is a 640*480 resolution image depicting a scene in an office environment, including a desk and computer, where two researchers are walking and talking. Using sequences such as fr3 / walking_xyz, halfsphere, rpy, and static from TUM as image input, the entire system is run through the following steps:

[0075] Step 1: After the camera acquires the image sequence, it first enters the dynamic object detection module. The YOLOv4 network model used for object detection is trained on the COCO dataset and has 80 different object labels. The network input is an image, and the output is the bounding box position and confidence of the trained objects contained in the image. In this method, the area outside the bounding box is considered as a static background area. If the objects detected in this frame are not significantly different from the objects in the previous frame, it directly enters step 2. However, the YOLOv4 network may fail. If no object is detected in the current frame, but the algorithm determines that there is a dynamic object in the previous frame, then optical flow tracing is performed on the feature points within the bounding box of the moving object detected in the previous frame, and the average displacement change of all points is calculated using equation (6). and The bounding box is moved by the corresponding pixel value and used as the current frame bounding box for step 2 calculation.

[0076] Step 2: Perform feature extraction and optical flow tracking on the background region outside the bounding box of the potential moving object, and then estimate the fundamental matrix F using the eight-point method. Calculate the epipolar line using Equation (8), and calculate the distance from the feature point to the epipolar line using Equation (9). Define threshold The value is 0.8. This can be achieved by setting a threshold. Determine the state of the feature points, if Then the feature points are considered to be For dynamic feature points, if , considering feature points It is a stationary point.

[0077] Step 3: When the camera and object undergo a pure translational motion parallel to each other, the boundary constraint method (12) is used to determine whether the feature points have moved by limiting the maximum and minimum values ​​of the feature point displacements. Set the depth. The range is 2-5m, and the predicted image points are... The displacement is considered a feature point if it is outside the range. For dynamic points.

[0078] Step 4: Record the number of dynamic feature points within the bounding box of each object. The total number of feature points is If the proportion of dynamic feature points If the value exceeds a threshold of 0.5, the object is determined to be a dynamic object. Then, tracking, local mapping, and loop closure detection are performed on the region outside the bounding box of the dynamic object, realizing a SLAM method that combines object detection and geometric constraints for dynamic scenes.

[0079] Common metrics for measuring the localization accuracy of SLAM systems are absolute trajectory error (ATE) and relative pose error (RPE). ATE aligns the true and estimated poses using timestamps and calculates the error for each pair of poses; this metric is suitable for evaluating the performance of visual SLAM systems. RPE similarly aligns timestamps, calculates the change in true and estimated poses at regular intervals, and then subtracts this change to obtain the relative pose error; this metric is suitable for estimating system drift.

[0080] This method is quantitatively compared with the results of ORB-SLAM2, a representative traditional visual SLAM system, running the TUM dynamic dataset. Four highly dynamic scene sequences, fr3 / walking_xyz, halfsphere, rpy, and static, are selected for quantitative comparison. Each dataset is tested five times and the average is taken. The root mean square error (rmse), median error, and mean error of the ATE and RPE indices are calculated respectively. As shown in Tables 1, 2, and 3.

[0081] Table 1 Comparison of ATE between ORB-SLAM2 and the algorithm of this invention

[0082]

[0083] Table 2 Comparison of displacement components in RPE of ORB-SLAM2 and the algorithm of this invention

[0084]

[0085] Table 3 Comparison of the rotational portion in RPE of ORB-SLAM2 and the algorithm of this invention.

[0086]

[0087] Define the percentage increase for:

[0088] (13)

[0089] This represents the error value of ORB-SLAM2. This indicates the error value of this method.

[0090] The percentage improvement of the algorithm of this invention, as calculated, is shown in Table 4 below.

[0091] Table 4. Average percentage improvement of the algorithm of this invention compared to ORB-SLAM2

[0092]

[0093] Quantitative comparison results show that the present invention significantly improves localization accuracy and robustness compared to traditional visual SLAM in highly dynamic scenes. In addition to the comparison of localization accuracy, the average processing time for each frame of the four sequences is 49ms, fully meeting real-time requirements.

[0094] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A dynamic scene SLAM localization method based on the YOLOv4 target detection algorithm and geometric constraints, characterized in that, Includes the following steps: Step S1: Read the image sequence through the visual sensor, and use the YOLOv4 object detection algorithm to identify all classified and trained objects and their bounding box positions in the image, proceed to step S4; if no object is detected, proceed to step S2; Step S2: If no object is detected in the current frame, the YOLOv4 object detection algorithm network fails, proceed to step S3; Step S3: Determine if there is a dynamic object in the previous frame; if so, match the two frames according to optical flow, and shift the bounding box of the dynamic object in the previous frame accordingly, proceed to step S4; if not, the current frame process ends; Step S4: Extract feature points and match them using optical flow for the part outside the bounding box region of the potential moving object, and calculate the basic transformation matrix between the two frames; Step S5: and For a pair of feature points matched using optical flow in two adjacent frames, the solution is obtained based on the transformation matrix. The epipolar line corresponding to the feature point in another frame image Step S6: Based on feature points to the poles Step S7: If degradation motion occurs, i.e., when the camera and the feature point undergo a pure translational motion parallel to each other, the image of that point moves along the epipolar line, and it is impossible to determine whether the feature point is moving based on the distance; Step S8: Use the boundary constraint method to determine whether the feature point is moving by limiting the maximum and minimum values ​​of the feature point displacement; Step S9: If and For point The image points on the two frames of the image, then and The homogeneous coordinates of the image points are respectively and Step S10: Obtain the boundary constraint method formula. If the displacement of a point is outside the range of the pixel coordinates, then the feature point has moved and is a dynamic feature point; Step S11: If the proportion of dynamic feature points within the bounding box of the object in step S1 exceeds the threshold, then the object is considered a dynamic object; Step S12: Perform subsequent tracking, local mapping and loop closure detection threads on the area outside the bounding box of the dynamic object.

2. The dynamic scene SLAM localization method based on the YOLOv4 target detection algorithm and geometric constraints according to claim 1, characterized in that: Specifically, step S7 involves using the boundary constraint method to determine whether an object's feature points are dynamic feature points when degenerative motion occurs, since the epipolar constraint can no longer determine whether the object's feature points are dynamic feature points. The boundary constraint method is then used to determine whether the object has moved by constraining the displacement of the feature points.

3. The dynamic scene SLAM localization method based on the YOLOv4 target detection algorithm and geometric constraints according to claim 1, characterized in that: The boundary constraint method formula for step S10 is as follows: ;in, For the camera intrinsic parameter matrix, Let be the camera translation vector. For point Relative to the camera's depth.

Images