Visual SLAM method based on dynamic target tracking and feature point filtering

Abstract

The present invention discloses a visual SLAM method based on dynamic target tracking and feature point filtering, which belongs to the field of visual SLAM and includes the following steps: when performing pose estimation, using target detection to identify potential dynamic objects, and then using target tracking to reduce the impact of false detection; then filtering the feature points of potential dynamic objects according to geometric constraints and depth constraints, eliminating the impact of dynamic objects on positioning while retaining effective information. The present invention largely avoids the impact of false detection problems on positioning, optimizes the screening and utilization of dynamic information; retains partial information from previous frames, giving dynamic objects correlation between image frames; not only helps to match the feature points of objects between frames, but also can more robustly determine the state of the object; and can obtain accurate and real-time pose estimation in high-dynamic scenes indoors and outdoors, which is unattainable by ordinary visual SLAM.

Description

Technical Field

[0001] The present invention relates to the field of visual SLAM, and more specifically to a visual SLAM method based on dynamic target tracking and feature point filtering. Background Art

[0002] There is a technical solution that currently performs feature extraction on visual input to restore the position and posture of the camera itself and build a point cloud map. This method is called visual SLAM technology.

[0003] This technical framework processes and optimizes the input video, resulting in a pose estimation that is highly dependent on the input environment. Current visual positioning research primarily focuses on static environments and is difficult to apply to dynamic, highly dynamic environments. In highly dynamic environments, the extracted features vary significantly between frames, resulting in poor positioning results and limitations. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the existing technology and provide a visual SLAM method based on dynamic target tracking and feature point filtering, which can obtain accurate and real-time pose estimation in high-dynamic scenes indoors and outdoors, which is unattainable by ordinary visual SLAM.

[0005] The object of the present invention is achieved through the following solutions:

[0006] A visual SLAM method based on dynamic target tracking and feature point filtering includes the following steps:

[0007] When performing pose estimation, target detection is used to identify potential dynamic objects, and then target tracking is used to reduce the impact of false detections.

[0008] Then, the feature points of potential dynamic objects are filtered according to geometric constraints and depth constraints, eliminating the impact of dynamic objects on positioning while retaining effective information, and achieving pose estimation that meets both real-time and accurate requirements.

[0009] Furthermore, the pose estimation is performed in a high dynamic environment.

[0010] Furthermore, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0011] Use the YOLO target detection algorithm to identify potential dynamic objects, and design a standard Kalman filter with a constant velocity model and a linear observation model to predict the position and state of the detected object in the next frame, including: defining the state space To describe the state of the object and the motion information in the image coordinate system, which includes the bounding box center coordinates (u, v), aspect ratio (γ), height (h) and its speed in the image coordinates, and the bounding box coordinates (u, v, γ, h) are used as the direct observation value of the object state; then, the bounding box of the current frame is calculated with the bounding box in the prediction. The IOU is used to match the bounding boxes between the two frames, the speed information is updated and assigned a number.

[0012] Furthermore, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0013] Use the following method to optimize inter-frame prediction: i ,S i ) is defined as the projection of the ith tracking distribution in the measurement space, including the observation value y i and the covariance matrix S i ; d j Defined as the jth bounding box detection result, the following formula is used between the Kalman prediction value and the observation value:

[0014]

[0015] Optimize and improve observation reliability;

[0016] In the above formula, Mahalanobis distance d (Mahalanobis) Used to calculate the distance of the residual vector, used to judge the difference between the observed value and the predicted value, T is the matrix transpose, Indicates S i Inverse matrix.

[0017] Furthermore, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0018] The following method is used to optimize inter-frame prediction: When the object is occluded or the camera shakes, causing the Mahalanobis distance to be ineffective, the cosine similarity is used to process the bounding box information, that is, the bounding box information is input into the VGG16 network, and L2 normalization is performed after feature extraction to obtain the descriptor a j where |a j |=1; then measure the cosine similarity between the i-th track and the j-th detection result in appearance space:

[0019] Then according to d (cosine) (i,j)=min(1-s i,j ) Calculate and find the nearest neighbor cosine distance, which is the best match.

[0020] Furthermore, the pose estimation is specifically based on the ORB_SLAM2 framework, and also includes the following steps: when completing the detection and tracking of the target, the feature points are extracted and processed synchronously, that is, ORB_SLAM2 uses an image pyramid to divide each layer of the image into multiple regions, and ORB feature points are extracted and detected in each region. The scaling expression of each layer of the image pyramid is:

[0021] S i =α i (i=0,1,2,…,n-1)

[0022] Where: n is the number of layers in the image pyramid; α is the scale factor of each layer of the pyramid; S i is the scaling scale of the i-th layer image;

[0023] If the total number of feature points that can be extracted from the image is N, then the number of feature points that need to be extracted from each layer of the pyramid image is:

[0024]

[0025] Where: N i is the number of feature points required for the i-th layer; S i is the inverse of the scale factor;

[0026] Divide each layer of pyramid image into regions and define and is the number of row and column divisions of the i-th layer image, and the number of pre-extractions is set to:

[0027]

[0028] In the divided area, set the initialization threshold T for extracting feature points init ; Determine the number of extractions N t If it is less than the expected number of feature points N' i , it is considered that the feature points extracted from some areas are empty, and the threshold T is adjusted. min , complete the extraction of feature points in the area and ensure that the feature points are evenly distributed, thereby changing the phenomenon of decreased tracking accuracy caused by feature points being in distant or weak texture areas after dynamic objects are removed.

[0029] Furthermore, the pose estimation is specifically based on the ORB_SLAM2 framework, and when completing the detection and tracking of the target, the feature points are extracted and processed synchronously, that is, the field of view of the feature point extraction is adjusted to limit the range of the feature point extraction.

[0030] Furthermore, the pose estimation specifically includes the following sub-steps:

[0031] Assume that the set of all feature points of the i-th frame image is:

[0032]

[0033] in, is the mth feature point on the frame image; the set of static points in the current frame is defined as

[0034] Define the feature point set in the reference frame as Then in and Find matching points between them to calculate relative pose;

[0035] Define a threshold to filter out inappropriate matches: Let the minimum Hamming distance in the match be For any match, if the calculated Hamming distance is greater than the set value, it is considered a mismatch; at the same time, an empirical threshold is set. To limit The value size of ;

[0036] The speed information of the reference frame is used to limit the search range of feature points and improve the robustness of matching; the key points p of the reference frame are r Project to the current frame, set a set pixel size area Δ around the feature point after projection, Searching among the feature points of the Δ distribution can reduce the time complexity and the probability of mismatch;

[0037] After obtaining the matching points, the basic matrix F is used to describe the posture relationship between the frames. When the camera is only rotated or the feature points are coplanar, the homography matrix H is used. Two methods are selected according to the number of matching points that meet the constraints. The specific steps are as follows: First, the random sampling consensus algorithm RANSAC and the normalized eight-point method are used to calculate the F matrix of the current frame and the reference frame, and RANSAC and the direct linear transformation method DLT are used to calculate the H matrix of the current frame and the reference frame; then the point-to-point distance and the number of inner points n are calculated. F and n H Calculation; finally compare n F 、n H The size of the larger one is selected to perform pose estimation.

[0038] Furthermore, filtering feature points of potential dynamic objects according to geometric constraints and depth constraints includes the following sub-steps:

[0039] 1) Use the RANSAC algorithm to calculate the basic matrix F of the two frames of images;

[0040] 2) For each feature point p1 in the first frame image, use the fundamental matrix to calculate the parameters of the line where its projected epipolar line e2p2 lies;

[0041] 3) The position of p2 is not strictly on the epipolar line. Calculate the distance from the corresponding feature point p2 to the epipolar line. If it is too large, it is an outlier.

[0042] Furthermore, the filtering of feature points of potential dynamic objects based on geometric constraints and depth constraints includes the following sub-steps: when using RGBD images as input, the depth of the map points is obtained by triangulation; for the feature point pairs matched between the reference frame and the current frame, the depth projected from the reference frame to the current frame is converted according to the estimated inter-frame pose matrix, and then the difference with the true annotation is calculated; after that, a floating threshold th is specified. depth Perform depth screening; finally, design a program lock to ensure that pose estimation is suspended when updating the static point map, and restart after the update is completed, ensuring more accurate positioning.

[0043] The beneficial effects of the present invention include:

[0044] The embodiment of the present invention, under the pose estimation framework based on ORB_SLAM2, combines potential dynamic target detection, prior semantic information, dynamic target tracking, epipolar geometry and depth constraint algorithms to largely avoid the impact of false detection problems on positioning and optimize the screening and utilization of dynamic information. In addition, the partial information of the previous frame is retained, which gives the dynamic objects the correlation between image frames. The technical solution of the present invention not only helps to match the feature points of the object between frames, but also can more robustly determine the state of the object. Accurate and real-time pose estimation that cannot be achieved by ordinary visual SLAM can be obtained in high-dynamic scenes indoors and outdoors.

[0045] The embodiment of the present invention utilizes YOLO target detection and tracking prediction technology to reduce the impact of false detection on system optimization, and retains prior motion information to improve the robustness of positioning.

[0046] The solution of the embodiment of the present invention can be applied in indoor and outdoor places in a variety of dynamic environments, achieving the real-time requirement that is difficult to meet in instance segmentation.

[0047] The embodiment of the present invention proposes a novel visual SLAM solution based on semantic information, which does not eliminate dynamic targets as a whole, but retains effective information as much as possible through a feature filtering mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0049] Figure 1 It is a schematic diagram of the framework;

[0050] Figure 2 Comparison between YOLO conventional detection and potential dynamic target detection;

[0051] Figure 3 Schematic diagram of dynamic target prediction and tracking;

[0052] Figure 4 is a schematic diagram of epipolar geometry;

[0053] Figure 5 This is a schematic diagram that satisfies the epipolar geometry but is a mismatch;

[0054] Figure 6 Misjudgment using only geometric constraints for outdoor scenes;

[0055] Figure 7 Correct misjudgment for depth constraints;

[0056] Figure 8 Conventional solutions cannot be used normally in highly dynamic indoor environments;

[0057] Figure 9 For KITTI outdoor scene application;

[0058] Note: Figure 2 、 Figure 3 and Figure 8 The portraits of the people in the dataset are all from public datasets; Figure 6 、 Figure 7 、 Figure 8 and Figure 9 The use of colors can more clearly express and distinguish, which is conducive to understanding the present invention. DETAILED DESCRIPTION

[0059] All features disclosed in all embodiments in this specification, or steps in all methods or processes implicitly disclosed, except for mutually exclusive features and / or steps, can be combined and / or expanded or replaced in any manner.

[0060] The embodiments of the present invention relate to visual SLAM (simultaneous localization and mapping) and pose estimation technology in high-dynamic environments, and specifically provide a visual SLAM method based on dynamic target tracking and feature point filtering. In the inventive concept, when performing pose estimation in a high-dynamic environment, YOLO target detection is used to identify potential dynamic objects, and then a target tracking algorithm is used to reduce the impact of false detections. This solution solves the problem that most existing dynamic SLAM solutions cannot run in real time. Feature points of potential dynamic objects are filtered according to geometric constraints and depth constraints, while retaining effective information as much as possible, eliminating the impact of dynamic objects on positioning, and achieving pose estimation that meets both real-time and accuracy in a high-dynamic environment.

[0061] In the further invention conception, in view of the problem that traditional visual SLAM algorithms cannot handle dynamic scenes and the shortcomings of existing algorithms, based on the ORB-SLAM2 framework, the embodiment of the present invention combines prior semantic information with epipolar geometry and depth constraint algorithms to optimize the use of dynamic information. The system framework is as follows: Figure 1 As shown, the present invention works synchronously in two threads, with the conventional SLAM module below. In specific implementation, the technical solution of the present invention is designed with the following four parts:

[0062] 1. Object Detection and Tracking

[0063] The embodiment of the present invention uses YOLOv3 and YOLOv8n with excellent accuracy and real-time performance to detect objects with potential motion, such as people and cars. The detection effect of this stage on the public dataset TUM is as follows: Figure 2 As shown in the figure, the left picture shows common object detection, including person, TV, keyboard, and mouse, while the right picture shows the detection of potential dynamic targets.

[0064] In the embodiment of the present invention, a standard Kalman filter with a constant velocity model and a linear observation model is designed to predict the position and state of the detected object in the next frame. Specifically, the following contents are included: defining the state space To describe the state of the object and the motion information in the image coordinate system, which includes the bounding box center coordinates (u, v), aspect ratio (γ), height (h) and its speed in the image coordinates, and the bounding box coordinates (u, v, γ, h) are used as the direct observation value of the object state.

[0065] Then, the bounding box of the current frame is compared with the bounding box in the prediction to calculate the IOU, so as to match the bounding boxes between the two frames, update the speed information and assign a number.

[0066] The embodiment of the present invention uses two algorithms to optimize inter-frame prediction: one is to convert (y i ,S i ) is defined as the projection of the ith track distribution in the measurement space, including the observation value y i and the covariance matrix S i ;d j Defined as the j-th bounding box detection result, the formula between the Kalman prediction value and the observation value is:

[0067]

[0068] Optimize and improve the reliability of observation. The Mahalanobis distance d (Mahalanobis) It is usually used to calculate the distance of the residual vector to determine the difference between the observed value and the predicted value.

[0069] Another is to use cosine similarity to process the bounding box information when the object is occluded or the camera is shaking, which makes the Mahalanobis distance difficult to work. In this algorithm, we input the bounding box information into the VGG16 network, extract the features and perform L2 normalization to obtain the descriptor a j where |a j |= 1. The cosine similarity between the i-th track and the j-th detection result is then measured in appearance space:

[0070] Then according to d (cosine) (i,j)=min(1-s i,j ) Calculate and find the nearest neighbor cosine distance, which is the best match. The prediction is as follows Figure 3 As shown, the left picture is the current frame detection, Detection is the detection box; the right picture is the next frame detection, Detection is the detection box, and Prediction is the prediction box of the algorithm of the present invention.

[0071] 2. Visual front-end processing

[0072] After completing target detection and tracking, the SLAM system will simultaneously extract and process feature points. This section makes two design decisions to achieve better results in feature point selection and extraction, thereby improving the robustness of the algorithm.

[0073] 1) ORB_SLAM2 uses the image pyramid to divide each layer of the image into many regions, and performs ORB feature point extraction and detection in each region. The scaling expression of each layer of the image pyramid is:

[0074] S i =α i (i=0,1,2,…,n-1)

[0075] Where: n is the number of layers in the image pyramid; α is the scale factor of each layer of the pyramid; S i is the scaling scale of the i-th layer image.

[0076] If the total number of feature points that can be extracted from the image is N, then the number of feature points that need to be extracted from each layer of the pyramid image is:

[0077]

[0078] Where: N i is the number of feature points required for the i-th layer; S i is the inverse of the scale factor.

[0079] We divide each layer of the pyramid image into regions and define and is the number of row and column divisions of the i-th layer image, and the number of pre-extractions is set to:

[0080]

[0081] In the divided area, set the initialization threshold T for extracting feature points init . Determine the number of extractions N t If it is less than the expected number of feature points N i ′ , it is considered that the feature points extracted from some areas are empty, and the threshold T is adjusted. min , complete the extraction of feature points in the area and ensure that the feature points are evenly distributed, thereby changing the phenomenon of decreased tracking accuracy caused by feature points being in distant or weak texture areas after dynamic objects are removed.

[0082] 2) Adjust the field of view for feature point extraction. When the camera moves past an object, changes in the surrounding objects or scene become more noticeable as the field of view widens. Pixel changes are more pronounced near the edges. The input image size in the KITTI dataset is 1241*376. This wide field of view makes it easy to be affected by large pixel changes at the edges when passing vehicles. Therefore, when processing images with an aspect ratio greater than 2:1, we restrict the range of feature point extraction to 15% of each long edge.

[0083] 3. Estimate the initial pose between frames

[0084] This section addresses the impact of dynamic objects on pose estimation, as dynamic objects may be mistaken for stable features in the environment, leading to errors in localization and mapping.

[0085] First, classify dynamic and static points and remove dynamic points. The set of all feature points of the i-th frame image is:

[0086]

[0087] in, is the mth feature point on the frame image. The set of static points in the current frame is defined as Used to estimate pose.

[0088] The specific method is: define the feature point set in the reference frame as Then in and Find matching points between them to calculate the relative pose. This step uses the idea of ​​using Hamming distance to measure the similarity of two BRIEF descriptors in the original ORB-SLAM2. However, in this process, the problem of brute force search is encountered: it is easy to introduce false matches. In order to solve this problem, the embodiment of the present invention defines a reasonable threshold to filter out inappropriate matches: let the minimum Hamming distance in the match be For any match, if the calculated Hamming distance is greater than twice, it is considered a mismatch. In very small cases, a large number of correct matches will be filtered out, so an empirical threshold is set. to limit this situation.

[0089] The velocity information of the reference frame (i.e., the posture transformation from the reference frame to the previous frame) is used to limit the search range of feature points and improve the robustness of matching. r Project to the current frame and set a 9*5 or 7*7 pixel area Δ around the feature point after projection (adjusted according to the camera motion mode). By searching among the feature points of the Δ distribution, the time complexity and the probability of mismatch can be reduced.

[0090] After obtaining matching points using the proposed scheme, the fundamental matrix F can be used to describe the pose relationship between frames. When the camera is only rotated or the feature points are coplanar, the homography matrix H is used. In this scheme, two methods are selected based on the number of matching points that meet the constraints.

[0091] The main steps are as follows: First, use the random sampling consensus algorithm (RANSAC) and the normalized eight-point method to calculate the F matrix of the current frame and the reference frame, and use RANSAC and the direct linear transformation method (DLT) to calculate the H matrix of the current frame and the reference frame; then perform point-to-point spacing and the number of inner points n F and n H Calculation; finally compare n F 、n H The size of the matrix (n F Corresponding to the F matrix, n HThe principle of this step is further explained in the following.

[0092] 4. Filtering of feature points

[0093] This part of the algorithm filters the feature points contained in the bounding box of potential dynamic targets, separating the dynamic and static points contained therein, thereby reducing the situation where the entire object is deleted due to the movement of a small part, and achieving the goal of removing dynamic feature points while retaining static information as much as possible.

[0094] In traditional SLAM algorithms, the sliding window search algorithm is widely used for feature point matching. This method is used to find feature point matches in consecutive frames. It selects a fixed-size window containing some feature points in the current frame and other information such as image coordinates and descriptors. It then searches for points in the previous frame that match the feature points in the window. The camera motion is calculated based on the matching points and used to update the current pose estimate and map. Repeating these steps until completion is called a sliding window.

[0095] However, in the presence of dynamic objects, the motion of the dynamic object may differ from the camera's motion, so using a window search may result in a large number of feature point matching errors. Therefore, when establishing a match, the only option is brute force matching. In the previous section, a threshold was designed to address some of the drawbacks of this solution.

[0096] We applied the epipolar constraint in the process of calculating the inliers, and demonstrated this principle in this section. The cameras of the two frames are located at O1 and O2, and the projections e1 and e2 of the spatial point P are the poles. l1 and l2 are the epipolar lines, which are the intersection lines of the epipolar plane OPP and the image plane. We cannot know the specific position of the spatial point P only through the first frame of the image, because the spatial points on the ray O1p1 will be projected onto the point p1. Therefore, the spatial point P is on the ray O1p1, and when projected to the second frame, p2 must be on the line e2p2. That is, the pixels on the first frame of the image are projected as a straight line on the second frame of the image. The spatial coordinates of P are specified as P = [x, y, z], and the positions of pixels p1 and p2 satisfy:

[0097] s1p1=KP

[0098] s2p2=K(RP+t)

[0099] K is the camera intrinsic parameter, R,t is the camera rotation and translation matrix. The derivation is the epipolar constraint:

[0100]

[0101]

[0102] In dynamic scenes, epipolar constraints must be used to detect and remove outliers on moving objects. The process is as follows:

[0103] 1) Use the RANSAC algorithm to calculate the basic matrix F of the two frames of images;

[0104] 2) For each feature point p1 in the first frame image, use the fundamental matrix to calculate the parameters of the line where its projected epipolar line e2p2 lies;

[0105] 3) Because of errors in the actual process, the position of p2 is not completely strictly on the epipolar line. Calculate the distance from the corresponding feature point p2 to the epipolar line. If it is too large, it is an outlier.

[0106] Figure 4 The epipolar constraint also has its limitations. When the object moves along the epipolar line, the distance between p2 and the epipolar line is always 0, which cannot reflect the true error. The same situation also occurs when the object moves exactly in the epipolar plane (when P moves to the position of P′ or P″), as shown in the following example. Figure 5 This drawback is particularly prominent when detecting cars on highways. The same problem also occurred during the dataset testing process. Figure 6 , the vehicle in the yellow frame in the middle of the road ahead in the image is misjudged as static when only epipolar constraints are used, and is involved in the pose estimation, which is bound to cause errors.

[0107] Taking advantage of the fact that RGBD and Stereo data contain or can calculate depth, the embodiment of the present invention adds depth information filtering. When Stereo is used as input, the depth of the map point in the current frame can be easily obtained. When using RGBD images as input, the depth ground truth is known in the data set, and the depth of the map point needs to be known through triangulation. For the feature point pairs matched between the reference frame and the current frame, the depth projected from the reference frame to the current frame can be obtained by conversion according to the inter-frame pose matrix estimated in Part 2, and then the difference with the ground truth needs to be calculated. After that, a floating threshold th is specified. depth Do in-depth screening.

[0108] By designing and combining the above algorithm with epipolar geometry constraints and depth constraints, the embodiment of the present invention completes the feature point screening and separates some dynamic and static points. We designed a program lock to ensure that the pose estimation is suspended when the static point map is updated, and restarted after the update is completed, ensuring more accurate positioning. In the dataset test, the effect of adding depth constraints is shown in Figure 7 As can be seen, by adding the depth constraint, the incorrect judgment of setting only the epipolar constraint is corrected, proving the effectiveness of the algorithm. In addition, the calculated information is also used in the back-end optimization without introducing redundant computational burden.

[0109] Within the ORB_SLAM2-based pose estimation framework, by combining potential dynamic target detection, prior semantic information, dynamic target tracking, epipolar geometry, and depth-constrained algorithms, the impact of false detection on positioning is largely avoided, optimizing the screening and utilization of dynamic information. Furthermore, by retaining partial information from previous frames, dynamic objects are correlated between image frames. This approach not only facilitates matching object feature points between frames but also enables more robust determination of object state. It can achieve accurate, real-time pose estimation in highly dynamic indoor and outdoor scenes, a level unattainable by conventional visual SLAM.

[0110] On one hand, the present invention is used for pose estimation in indoor, high-dynamic environments. Indoor scenes have limited environmental information and a small scene area. Common targets that may be in high motion are often people in motion. When a person passes in front of the camera, pose tracking often becomes disrupted for a period of time, which can be fatal. Using high-real-time object detection to process dynamic targets can effectively combat this situation.

[0111] To obtain the most accurate test accuracy comparison, this example uses the TUM dataset, which contains relatively accurate ground truth and is widely used in the SLAM testing field.

[0112] Using the conventional visual solution ORB_SLAM2 in application scenarios such as Figure 8 As shown, the left image compares the trajectory of ORB_SLAM2 and the real-world annotation. The X, Y, and Z coordinate units are all in meters. The right image is a screenshot of the running process. It can be seen that in this highly dynamic indoor environment, the positioning results have huge errors, and conventional solutions are no longer effective, especially for unmanned robot navigation and planning.

[0113] In this implementation example, we detect and track people who are moving, and then perform geometric and depth filtering on the feature points within the dynamic frame. We calculate the root mean square error (RMSE) and standard deviation (STD) of the absolute trajectory error (ATE) between the motion trajectory and the ground truth, which can be quantified. The actual pose estimation in this environment is obtained and plotted on Figure 9 In the table shown, xyz, static, rpy, and halfsphere in the first column represent four types of camera ego motion. For example, xyz represents camera movement along the xyz axis. W and S represent the walking and sitting states of a dynamic object. RMSE and STD can better illustrate the camera's motion trajectory.

[0114] The improvement of the pose estimation results in the examples of the present invention is very significant, generally reaching an improvement level of more than 80%, and has robust and excellent practical application capabilities.

[0115] Another aspect of the present invention is to estimate pose in outdoor high-dynamic environments. Outdoor scenes contain more effective information and may contain more dynamic objects, such as various vehicles and pedestrians, which occupy a relatively small proportion of the entire image.

[0116] Similarly, we tested our outdoor performance using the highly accurate KITTI dataset, the most widely used dataset in autonomous driving. This dataset is primarily recorded using cameras mounted on vehicles traveling through villages, city streets, and urban highways. The examples used also feature clips with a high number of dynamic objects and intensive motion (denoted by s), which better meet the validation requirements.

[0117] The scene is shown as Figure 9 ,The test results of the embodiment of the present invention are shown in Table 1 below, ,which shows the improvement of RMSE and STD, the descriptive indicators of ,the absolute trajectory error ATE in the KITTI outdoor scene.

[0118] Table 1

[0119]

[0120] When considering the error across the entire dataset, the RMSE of the absolute trajectory is similar, except for periods with a high concentration of dynamic objects, as no dynamic points occur during these periods that could affect pose estimation. For a complete sequence, where the scene is low-dynamic or even static for extended periods, the proposed algorithm is virtually indistinguishable from ORBSLAM2. Therefore, after averaging the errors, the improvement is minimal. However, significant improvements are seen in segments with high dynamics.

[0121] Furthermore, existing excellent dynamic SLAM algorithms, such as DynaSLAM and DS-SALM, take at least 200-350ms to process each frame, making them incapable of meeting real-time requirements for practical use. However, this solution, through rapid detection using YOLO and feature filtering, can achieve real-time operation at a minimum of 15 frames per second.

[0122] Quantitative testing in both indoor and outdoor scenarios clearly demonstrates that, compared to the most widely used conventional vision solution, ORB_SLAM2, the proposed pose estimation solution based on dynamic target tracking and feature point filtering can achieve accurate and reliable estimation results in both indoor and outdoor highly dynamic environments, with excellent real-time performance. Running this algorithm on unmanned equipment can achieve safe, real-time, and precise positioning, demonstrating excellent real-world application potential.

[0123] It should be noted that within the scope of protection defined in the claims of the present invention, the following embodiments can be combined and / or expanded or replaced in any logical way from the above specific implementation methods, such as disclosed technical principles, disclosed technical features or implicitly disclosed technical features.

[0124] Example 1

[0125] A visual SLAM method based on dynamic target tracking and feature point filtering includes the following steps:

[0126] When performing pose estimation, target detection is used to identify potential dynamic objects, and then target tracking is used to reduce the impact of false detections.

[0127] Then, the feature points of potential dynamic objects are filtered according to geometric constraints and depth constraints, eliminating the impact of dynamic objects on positioning while retaining effective information, and achieving pose estimation that meets both real-time and accurate requirements.

[0128] Example 2

[0129] Based on Example 1, the pose estimation is performed in a high dynamic environment.

[0130] Example 3

[0131] Based on Example 1, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0132] Use the YOLO target detection algorithm to identify potential dynamic objects, and design a standard Kalman filter with a constant velocity model and a linear observation model to predict the position and state of the detected object in the next frame, including: defining the state space To describe the state of the object and the motion information in the image coordinate system, which includes the bounding box center coordinates (u, v), aspect ratio (γ), height (h) and its speed in the image coordinates, and the bounding box coordinates (u, v, γ, h) are used as the direct observation value of the object state; then, the bounding box of the current frame is calculated with the bounding box in the prediction. The IOU is used to match the bounding boxes between the two frames, the speed information is updated and assigned a number.

[0133] Example 4

[0134] Based on Example 1, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0135] Use the following method to optimize inter-frame prediction: i ,S i ) is defined as the projection of the ith tracking distribution in the measurement space, including the observation value y i and the covariance matrix S i ; d j Defined as the jth bounding box detection result, the following formula is used between the Kalman prediction value and the observation value:

[0136]

[0137] Optimize and improve observation reliability;

[0138] In the above formula, Mahalanobis distance d (Mahalanobis) Used to calculate the distance of the residual vector, used to judge the difference between the observed value and the predicted value, T is the matrix transpose, Indicates S i Inverse matrix.

[0139] Example 5

[0140] Based on Example 1, the method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps:

[0141] The following method is used to optimize inter-frame prediction: When the object is occluded or the camera shakes, causing the Mahalanobis distance to be ineffective, the cosine similarity is used to process the bounding box information, that is, the bounding box information is input into the VGG16 network, and L2 normalization is performed after feature extraction to obtain the descriptor a j where |a j |=1; then measure the cosine similarity between the i-th track and the j-th detection result in appearance space:

[0142] Then according to d (cosine) (i,j)=min(1-s i,j ) Calculate and find the nearest neighbor cosine distance, which is the best match.

[0143] Example 6

[0144] On the basis of Examples 1 to 5, the pose estimation is specifically based on the ORB_SLAM2 framework, and further includes the following steps: when completing the detection and tracking of the target, the feature points are extracted and processed synchronously, that is, ORB_SLAM2 uses an image pyramid to divide each layer of the image into multiple regions, and performs ORB feature point extraction and detection in each region. The scaling expression of each layer of the image pyramid is:

[0145] S i =α i (i=0,1,2,…,n-1)

[0146] Where: n is the number of layers in the image pyramid; α is the scale factor of each layer of the pyramid; S i is the scaling scale of the i-th layer image;

[0147] If the total number of feature points that can be extracted from the image is N, then the number of feature points that need to be extracted from each layer of the pyramid image is:

[0148]

[0149] Where: N i is the number of feature points required for the i-th layer; S i is the inverse of the scale factor;

[0150] Divide each layer of pyramid image into regions and define and is the number of row and column divisions of the i-th layer image, and the number of pre-extractions is set to:

[0151]

[0152] In the divided area, set the initialization threshold T for extracting feature points init ; Determine the number of extractions N t If it is less than the expected number of feature points N i ′ , it is considered that the feature points extracted from some areas are empty, and the threshold T is adjusted. min , complete the extraction of feature points in the area and ensure that the feature points are evenly distributed, thereby changing the phenomenon of decreased tracking accuracy caused by feature points being in distant or weak texture areas after dynamic objects are removed.

[0153] Example 7

[0154] On the basis of Example 1, the pose estimation is specifically based on the ORB_SLAM2 framework, and when completing the detection and tracking of the target, the feature points are also extracted and processed synchronously, that is, the field of view of the feature point extraction is adjusted to limit the range of the feature point extraction.

[0155] Example 8

[0156] Based on Example 1, the pose estimation specifically includes the following sub-steps:

[0157] Assume that the set of all feature points of the i-th frame image is:

[0158]

[0159] in, is the mth feature point on the frame image; the set of static points in the current frame is defined as

[0160] Define the feature point set in the reference frame as Then in and Find matching points between them to calculate relative pose;

[0161] Define a threshold to filter out inappropriate matches: Let the minimum Hamming distance in the match be For any match, if the calculated Hamming distance is greater than the set value, it is considered a mismatch; at the same time, an empirical threshold is set. To limit The value size of ;

[0162] The speed information of the reference frame is used to limit the search range of feature points and improve the robustness of matching; the key points p of the reference frame are r Project to the current frame, set a set pixel size area Δ around the feature point after projection, Searching among the feature points of the Δ distribution can reduce the time complexity and the probability of mismatch;

[0163] After obtaining the matching points, the basic matrix F is used to describe the posture relationship between the frames. When the camera is only rotated or the feature points are coplanar, the homography matrix H is used. Two methods are selected according to the number of matching points that meet the constraints. The specific steps are as follows: First, the random sampling consensus algorithm RANSAC and the normalized eight-point method are used to calculate the F matrix of the current frame and the reference frame, and RANSAC and the direct linear transformation method DLT are used to calculate the H matrix of the current frame and the reference frame; then the point-to-point distance and the number of inner points n are calculated. F and n H Calculation; finally compare n F 、n H The size of the larger one is selected to perform pose estimation.

[0164] Example 9

[0165] Based on Example 1, the filtering of feature points of potential dynamic objects according to geometric constraints and depth constraints includes the following sub-steps:

[0166] 1) Use the RANSAC algorithm to calculate the basic matrix F of the two frames of images;

[0167] 2) For each feature point p1 in the first frame image, use the fundamental matrix to calculate the parameters of the line where its projected epipolar line e2p2 lies;

[0168] 3) The position of p2 is not strictly on the epipolar line. Calculate the distance from the corresponding feature point p2 to the epipolar line. If it is too large, it is an outlier.

[0169] Example 10

[0170] Based on Example 1, the filtering of feature points of potential dynamic objects based on geometric constraints and depth constraints includes the following sub-steps: when using RGBD images as input, the depth of the map points is obtained by triangulation; for the feature point pairs matched between the reference frame and the current frame, the depth projected from the reference frame to the current frame is converted according to the estimated inter-frame pose matrix, and then the difference with the true annotation is calculated; after that, a floating threshold th is specified. depth Perform depth screening; finally, design a program lock to ensure that pose estimation is suspended when updating the static point map, and restart after the update is completed, ensuring more accurate positioning.

[0171] The units involved in the embodiments of the present invention may be implemented in software or hardware, and the units described may also be provided in a processor. In some cases, the names of these units do not limit the units themselves.

[0172] According to one aspect of an embodiment of the present invention, a computer program product or computer program is provided, comprising computer instructions stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform the methods provided in the various optional implementations described above.

[0173] As another aspect, embodiments of the present invention further provide a computer-readable medium, which may be included in the electronic device described in the above embodiments, or may exist independently and not incorporated into the electronic device. The computer-readable medium carries one or more programs, and when executed by the electronic device, the electronic device implements the methods described in the above embodiments.

[0174] In addition to the above examples, those skilled in the art may obtain other embodiments based on the above disclosure or by utilizing knowledge or technology in related fields to make modifications. The features of each embodiment may be interchangeable or replaced. The modifications and changes made by those skilled in the art do not depart from the spirit and scope of the present invention and should be within the scope of protection of the claims attached to the present invention.

Claims

1. A visual SLAM method based on dynamic target tracking and feature point filtering, characterized in that: The following steps are involved: When performing pose estimation, target detection is used to identify potential dynamic objects, and then target tracking is used to reduce the impact of false detections. Then, the feature points of potential dynamic objects are filtered according to geometric and depth constraints, eliminating the impact of dynamic objects on positioning while retaining effective information, achieving pose estimation that meets both real-time and accurate requirements. The pose estimation is specifically based on the ORB_SLAM2 framework and also includes the following steps: when completing the detection and tracking of the target, the feature points are extracted and processed synchronously, that is, ORB_SLAM2 uses the image pyramid to divide each layer of the image into multiple regions, and ORB feature points are extracted and detected in each region. The scaling expression of each layer of the image pyramid is: Where: is the number of layers of the image pyramid; is the scale factor of each layer of the pyramid; It is The scaling of the layer image; If the total number of feature points that can be extracted from the image is , then the number of feature points that need to be extracted from each layer of the pyramid image is: Where: It is The number of feature points required for the layer; is the inverse of the scale factor; Divide each layer of pyramid image into regions and define and For the The number of rows and columns of the layer image is divided into, and the number of pre-extractions is set to: In the divided area, set the initialization threshold for extracting feature points ; Determine the number of extractions If it is less than the expected number of feature points to be extracted , it is considered that the feature points extracted from some areas are empty, and the threshold is adjusted , complete the extraction of feature points in the area and ensure that the feature points are evenly distributed, thereby changing the phenomenon of reduced tracking accuracy caused by feature points being in distant or weak texture areas after dynamic objects are removed; The filtering of feature points of potential dynamic objects according to geometric constraints and depth constraints includes the following sub-steps: 1) Use the RANSAC algorithm to calculate the basic matrix F of the two frames of images; 2) For each feature point of the first frame image , use the basic matrix to calculate the epipolar line after projection Parameters of the line; 3) The position of is not strictly on the epipolar line, so calculate the corresponding feature points If the distance to the extreme line is too large, it is an outlier; The filtering of feature points of potential dynamic objects based on geometric constraints and depth constraints includes the following sub-steps: when using RGBD images as input, the depth of the map points is obtained by triangulation; for the feature point pairs matched between the reference frame and the current frame, the depth projected from the reference frame to the current frame is converted according to the estimated inter-frame pose matrix, and then the difference with the true annotation is calculated; after that, a floating threshold is specified. Perform depth screening; finally, design a program lock to ensure that pose estimation is suspended when updating the static point map, and restart after the update is completed, ensuring more accurate positioning.

2. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The pose estimation is performed in a high dynamic environment.

3. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps: Use the YOLO target detection algorithm to identify potential dynamic objects, and design a standard Kalman filter with a constant velocity model and a linear observation model to predict the position and state of the detected object in the next frame, including: defining the state space To describe the state of the object and the motion information in the image coordinate system, which includes the coordinates of the center of the bounding box , aspect ratio ,high and its velocity in image coordinates, and the bounding box coordinates As a direct observation value of the object state; then, the bounding box of the current frame is calculated with the bounding box in the prediction, so as to match the bounding boxes between the two frames, update the speed information and assign a number.

4. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps: Use the following method to optimize inter-frame prediction: Defined as The projection of the tracking distribution in the measurement space, including the observation and the covariance matrix ;Will Defined as The bounding box detection results are calculated using the following formula between the Kalman prediction value and the observation value: Optimize and improve observation reliability; In the above formula, Mahalanobis distance Used to calculate the distance of the residual vector, used to judge the difference between the observed value and the predicted value, T is the matrix transpose, express Inverse matrix.

5. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The method of using target detection to identify potential dynamic objects and then using target tracking to reduce the impact of false detections specifically includes the following sub-steps: The following method is used to optimize inter-frame prediction: When the object is blocked or the camera shakes, causing the Mahalanobis distance to be ineffective, the cosine similarity is used to process the bounding box information, that is, the bounding box information is input into the VGG16 network, and the feature extraction is performed. Normalize and obtain descriptors in ; Then measure the first Track and The cosine similarity between the detection results: ; Then according to Calculate and find the nearest neighbor cosine distance, which is the best match.

6. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The pose estimation is specifically based on the ORB_SLAM2 framework, and when completing the detection and tracking of the target, the feature points are also extracted and processed synchronously, that is, the field of view of the feature point extraction is adjusted to limit the range of the feature point extraction.

7. The visual SLAM method based on dynamic target tracking and feature point filtering according to claim 1, wherein The pose estimation specifically includes the following sub-steps: Set up the first The set of all feature points of the frame image is: in, The first feature points; the set of static points in the current frame is defined as ; Define the feature point set in the reference frame as , then and Find matching points between them to calculate relative pose; Define a threshold to filter out inappropriate matches: Let the minimum Hamming distance in the match be For any match, if the calculated Hamming distance is greater than the set value, it is considered a mismatch; at the same time, an empirical threshold is set To limit The value size of ; Use the speed information of the reference frame to limit the search range of feature points and improve the robustness of matching; Project to the current frame, set a set pixel size area Δ around the feature point after projection, Searching among the feature points of the Δ distribution can reduce the time complexity and the probability of mismatch; After obtaining the matching points, use the basic matrix To describe the posture relationship between frames, when the camera is only rotated or the feature points are coplanar, the homography matrix is ​​used ; According to the number of matching points that meet the constraints, the two methods are selected. The specific steps are as follows: First, the random sampling consensus algorithm RANSAC and the normalized eight-point method are used to calculate the F matrix of the current frame and the reference frame, and RANSAC and the direct linear transformation method DLT are used to calculate the Matrix; then the point-to-point distance and the number of inner points and Calculation; final comparison 、 The size of the larger one is selected to perform pose estimation.

Images