Visual slam method suitable for dynamic environments

Abstract

The application discloses a visual SLAM method suitable for a dynamic environment, comprising the following steps: extracting feature points from an input image, and dividing the input image into an in-frame region and an out-of-frame region; filtering out dynamic points in the out-of-frame region from feature points in the out-of-frame region through geometric constraints to obtain static points in the out-of-frame region; performing speed calculation on feature points in the in-frame region to divide the feature points into dynamic points in the in-frame region and static points in the in-frame region; and integrating the static points in the in-frame region and the static points in the out-of-frame region into pose solving to obtain an optimal pose. The visual SLAM method suitable for the dynamic environment can identify dynamic targets and filter out the dynamic targets to realize positioning and mapping in the dynamic environment.

Description

Technical Field

[0001] This application relates to the field of image processing, and more specifically, to a visual SLAM method suitable for dynamic environments. Background Technology

[0002] SLAM (simultaneous localization and mapping) is a prerequisite for robot navigation and is widely used in autonomous driving, mobile robots, drones, augmented reality, and other fields. While visual SLAM technology frameworks are relatively mature, typical solutions often only consider static environments. In highly dynamic environments, cameras cannot distinguish between dynamic objects and static backgrounds, leading to localization failures and distorted map construction.

[0003] To reduce the impact of dynamic objects on system robustness, dynamic targets need to be identified at the front end, and dynamic information needs to be filtered out. Summary of the Invention

[0004] The summary section of this application is intended to provide a brief overview of the concepts, which will be described in detail in the detailed description section below. This summary section is not intended to identify key or essential features of the claimed technical solutions, nor is it intended to limit the scope of the claimed technical solutions.

[0005] To address the technical problems mentioned in the background section, some embodiments of this application provide a visual SLAM method suitable for dynamic environments, including: extracting feature points from an input image and dividing the input image into an in-frame region and an out-of-frame region; filtering out moving points outside the frame using geometric constraints to obtain stationary points outside the frame; calculating the velocity of feature points in the in-frame region to divide them into moving points inside the frame and stationary points inside the frame; and incorporating both stationary points inside and outside the frame into pose calculation to obtain the optimal pose.

[0006] Furthermore, feature points are extracted from the input image using ORB-SLAM2, and the input image is divided into in-frame and out-of-frame regions using YOLOv4.

[0007] Furthermore, filtering out moving points outside the bounding box to obtain stationary points outside the bounding box using geometric constraints for feature points in the region outside the bounding box includes:

[0008] Let I1 and I2 be two adjacent frames of images during camera motion, corresponding to two distinct moments. O1 and O2 are the optical centers of the camera at the two distinct moments. Point P is a spatial point, and its projections onto I1 and I2 are P1 and P2, respectively. With line segment O1O2 as the baseline, the intersection points of the baseline with I1 and I2 are e1 and e2, respectively. The intersection lines l1 and l2 of surface O1O2P with I1 and I2 are denoted as epipolar lines.

[0009] Establish a pixel coordinate system. Let [u1,v1] and [u2,v2] be the pixel coordinates of the projection point at times I1 and I2, respectively. The epipolar line l2 is represented in the uv rectangular coordinate system as au2 + bv2 + c = 0. Then the distance from the projection point to the epipolar line l2 is... Among them [abc] T =FK[u1 v1 1], where F represents the fundamental matrix from I1 to I2, and K represents the camera parameter matrix;

[0010] Set threshold D n If d < D n If d ≥ D, then the feature point is determined to be a static point; n If the feature point is determined to be a moving point, it will be eliminated; where D n The expression is n represents the number of pyramid layers in the ORB-SLAM2 image pyramid representing the feature points.

[0011] Furthermore, filtering out moving points outside the bounding box using geometric constraints to obtain static points outside the bounding box also includes: for features satisfying d < D n The feature point is used to calculate its projection coordinates at time I2 based on its projection coordinates at time I1 and the camera poses at times I1 and I2. The calculated projection coordinates are:

[0012] The calculation formula is:

[0013]

[0014] Among them, T cw_1 and T cw_2 These are the transformation matrices from the world coordinate system to the camera coordinate system at times I1 and I2, respectively, where z is the corresponding depth coordinate value. [u1 v1 z1] T Let P be the projected coordinates at time I1; the actual projected coordinates are represented as P = [u² v² z²]. T ;

[0015] If the feature points satisfy the constraints: The feature point is defined as a static point. If the condition is not met, the feature point is removed as a dynamic point. Here, n represents the number of layers in the image pyramid.

[0016] Furthermore, calculating the velocity of feature points within the bounding box to classify them into moving points and stationary points within the box includes: obtaining the coordinates [X] of the feature points within the bounding box in the world coordinate system at time t. t Y t Z t ] T =T wc_t KP t , among which, Twc_t denotes the transformation matrix from camera coordinate system to world coordinate system at t time, P t denotes the projection coordinate at t time;

[0017] The velocity V of the feature point in the frame region at t time is calculated t t The expression of V is:

[0018] V t = ([X t+1 Y t+1 Z t+1 ] T - [X t Y t Z t ] T ) / dt = [V x_t V y_t V z_t ] T ; wherein, [X t+1 Y t+1 Z t+1 ] T denotes the coordinates of the feature point in the frame region at adjacent t+1 time in the world coordinate system, and dt denotes the time interval;

[0019] The feature point satisfying the constraint condition is removed as the frame-in dynamic point, and the remaining feature point is removed as the frame-in static point; wherein the constraint condition is: ||V t+1 -V t || < ||V t+1 +V t ||.

[0020] Further, the optimal pose is solved by minimizing the error function; wherein the error function is minimized:

[0021]

[0022] ∑2 = ε × ∑1 = ε × n × E;

[0023] wherein, T cw_t denotes the transformation matrix from the world coordinate system to the camera coordinate system, S i and S j denote the coordinates of the frame-out static point and the frame-in static point, respectively, P i and P j denote the projection coordinates of the frame-out static point and the frame-in static point, respectively, Σ1 and Σ2 denote the third-order weight matrix of the corresponding points, that is, the information matrix, E denotes the third-order unit matrix, and ε denotes the weight coefficient.

[0024] ​Further, the step of incorporating the inlier static points and the outlier static points together into the pose solving to solve the optimal pose further comprises: solving the re-projection error of the inlier static points and the outlier static points, and assigning weights using information matrix.

[0025] Further, the step of solving the re-projection error of the inlier static points and the outlier static points, and assigning weights using information matrix comprises: setting the information matrix in the Bundle Adjustment of ORB-SLAM2, where the selection is a unit matrix E, and the default is that each coordinate value error is shared;

[0026] ORB-SLAM2 uses an image pyramid, and then extracts feature points at different scales. For the feature points extracted from the upper layer image, the coordinates of the feature points are multiplied by the layer number n to restore the coordinates to the scale of the first layer.

[0027] For the coordinates of the inlier static points, the original information matrix is multiplied by ε, where the value range of ε is 0.6 to 0.7.

[0028] Returning to the Gauss-Newton iteration method for solving the re-projection error, the projection error term is denoted as e T , and the error square term is e T Σe, where Σ is the corresponding information matrix.

[0029] Further, the visual SLAM method suitable for a dynamic environment further comprises: adding a dense point cloud mapping thread to ORB-SLAM2.

[0030] Further, the step of adding a dense point cloud mapping thread to ORB-SLAM2 comprises:

[0031] The tracking thread tracking is modified, the key frame of the tracking thread is inserted into the point cloud map, the point cloud id associated with the key frame image is obtained, and the transformation of the current frame relative to the world coordinate system is obtained;

[0032] The information of the RGB-D image is combined, the spatial position of the corresponding point is found, and a spatial point cloud is constructed.

[0033] In the viewer thread of ORB-SLAM2, the point cloud in each key frame is converted to the world coordinate system through a rigid body transformation;

[0034] The point cloud is processed by an outlier removal filter and a voxel filter through a PCL library function to remove outliers and downsample the point cloud, and the filtered point cloud is output to the global map.

[0035] The PCL point cloud display is added to the viewer thread of ORB-SLAM2, and the point cloud map is displayed in the display window of the PCL.

[0036] The application has the beneficial effect of providing a visual SLAM method suitable for dynamic environment, which can identify dynamic targets and filter out to realize positioning and mapping in dynamic environment. BRIEF DESCRIPTION OF DRAWINGS

[0037] The accompanying drawings, which form a part of this application, are intended to provide further understanding of the application and are incorporated herein in

[0038] In addition, throughout the drawings, same or similar reference numerals designate same or similar elements. It should be understood that the drawings are schematic, and elements and elements are not necessarily drawn according to scale.

[0039] In the drawings:

[0040] Figure 1 is a schematic diagram of main steps of a visual SLAM method suitable for dynamic environment according to an embodiment of the application;

[0041] Figure 2 is a schematic diagram of a framework of an ORB-SLAM2 system according to an embodiment of the application;

[0042] Figure 3 is a schematic diagram of a framework according to an embodiment of the application;

[0043] Figure 4 is a schematic diagram of a detection result of a data set by YOLOv4 according to an embodiment of the application;

[0044] Figure 5 is a schematic diagram of the effect of filtering out feature points on dynamic objects according to an embodiment of the application;

[0045] Figure 6 is a schematic diagram of the principle of static point geometric constraint according to an embodiment of the application;

[0046] Figure 7 is a comparison diagram of trajectory error of ORB-SLAM2 and the method of the application on walking_rpy, wherein Figure 7 (a) represents ORB-SLAM2, Figure 7 (b) represents the application;

[0047] Figure 8 is a comparison diagram of pose error of ORB-SLAM2 and the method of the application on walking_rpy, wherein Figure 8 (a) represents ORB-SLAM2, Figure 8 (b) represents the application;

[0048] Figure 9Trajectory error comparison chart of ORB-SLAM2 and the method of the application on walking_xyz, wherein Figure 9 (a) represents ORB-SLAM2, Figure 9 (b) represents the application;

[0049] Figure 10 Pose error comparison chart of ORB-SLAM2 and the method of the application on walking_xyz, wherein Figure 10 (a) represents ORB-SLAM2, Figure 10 (b) represents the application;

[0050] Figure 11 Point cloud mapping effect of ORB-SLAM2 on fr3_walking static dataset.

[0051] Figure 12 Point cloud mapping effect of the application on fr3_walking static dataset. DETAILED DESCRIPTION

[0052] Embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although some embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms, and should not be interpreted as being limited to the embodiments set forth herein. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete. It should be understood that the drawings and embodiments of the present disclosure are only for exemplary purposes, and are not intended to limit the scope of protection of the present disclosure.

[0053] In addition, it should be noted that only parts related to the present application are shown in the drawings for ease of description. The embodiments in the present disclosure and the features in the embodiments can be combined with each other without conflict.

[0054] It should be noted that the modification of "one" or "multiple" mentioned in the present application is illustrative rather than limiting, and those skilled in the art should understand that, unless otherwise explicitly indicated in the context, it should be understood as "one or more".

[0055] The present disclosure will be described in detail below with reference to the accompanying drawings and in conjunction with the embodiments.

[0056] As Figure 1 shown, the visual SLAM method for dynamic environment of the application mainly includes the following steps:

[0057] S1: extracting feature points from the input image, and dividing the input image into in-frame region and out-of-frame region.

[0058] S2: filtering out the out-of-frame dynamic points through geometric constraint to obtain out-of-frame static points.

[0059] S3: Calculate the velocity of feature points within the bounding box to classify them into moving points and stationary points within the bounding box.

[0060] S4: Include both the static points inside and outside the frame in the pose calculation to find the optimal pose.

[0061] S5: Add a dense point cloud mapping thread to ORB-SLAM2.

[0062] Specifically, in step S1, feature points are extracted from the input image using ORB-SLAM2, and the input image is divided into in-frame and out-of-frame regions using YOLOv4.

[0063] ORB-SLAM2 is a classic feature point algorithm framework, mainly composed of three threads: tracking, local mapping, and loop closure. Figure 2 As shown, it can operate in monocular, binocular, and RGB-D modes. This application improves upon its RGB-D mode. The overall algorithm framework is as follows: Figure 3 .

[0064] YOLOv4 is an advanced real-time object detection system that runs a single convolutional network to predict the coordinates of bounding boxes, the confidence score of objects contained within the boxes, and the object category directly from an entire image. This application uses the default YOLOv4 model and various parameters, and runs the YOLOv4 detection thread as a multi-threaded operation alongside the ORB-SLAM2 tracking thread. First, a 640×480 TUM-RGBD dataset image is input, and the returned image includes the bounding boxes, such as... Figure 4 In the office environment of the TUM dataset, only people are typically present as dynamic objects. Therefore, by using only people as the prior target, the filtering effect is as follows: Figure 5 .

[0065] In ORB-SLAM2, feature point extraction and matching are processed in pixel coordinates. Therefore, it's necessary to configure `dectect.py` to output the coordinates of the detection box (pixel coordinates), including the coordinates of the four vertices and the center, and return them to the tracking thread. In the tracking thread, the image is divided into in-box and out-of-box regions based on the detection box coordinates, for further processing.

[0066] After semantic detection by YOLOv4, the feature points on the person, the prior target, are mostly within the detection box. However, the detection box also contains feature points of stationary objects such as chairs and monitors. To address this issue, this application, based on actual results, found that most dynamic feature points in the image frame are already included in the detection box. Therefore, strict static geometric constraints are first applied to feature points outside the detection box, while retaining most of their feature points, i.e., static points. The static geometric constraints mainly include epipolar constraints and projection constraints.

[0067] This approach involves adding the real-time detection thread of YOLOv4 to the ORB-SLAM2 front end. After inputting the image frame, it returns the result with the prior object detection box, dividing the image region into the in-box region and the out-of-box region, which is beneficial for further geometric processing.

[0068] As a specific solution, step S2 includes the following steps:

[0069] S21: As Figure 6 As shown, let I1 and I2 be two adjacent frames of images during camera motion, corresponding to two distinct moments. O1 and O2 are the optical centers of the camera at the two distinct moments. Point P is a spatial point whose projections onto I1 and I2 are P1 and P2, respectively. Line segment O1O2 is taken as the baseline, and the intersection points of the baseline with I1 and I2 are e1 and e2, respectively. The intersection lines l1 and l2 of plane O1O2P with I1 and I2 are denoted as epipolar lines. P, P′, and P″ are the same point, where P is the position at moment I1, and P′ and P″ are the positions of this point at moment I2 under different motion conditions.

[0070] S22: Establish a pixel coordinate system. Let [u1, v1] and [u2, v2] be the pixel coordinates of the projection point at times I1 and I2, respectively. The epipolar line l2 is represented in the uv rectangular coordinate system as au2 + bv2 + c = 0. Then the distance from the projection point to the epipolar line l2 is... Among them [abc] T =FK[u1 v1 1], where F represents the fundamental matrix from I1 to I2, and K represents the camera parameter matrix.

[0071] d 2 The chi-square distribution is satisfied, and a significance level of 0.05, or 95% confidence level, is used here. Based on the fact that the projection degrees of freedom for the RGB-D mode are 1, the chi-square statistic threshold is determined to be 3.84. In ORB-SLAM2, ORB feature points utilize an image pyramid, which is scaled sequentially at a ratio of 1 / 1.2 to obtain 8 layers of images, and feature extraction is performed on each image.

[0072] S23: Set threshold D n If d < D n If d ≥ D, then the feature point is determined to be a static point; n If the feature point is determined to be a moving point, it will be eliminated; where D n The expression is n represents the number of pyramid layers in the ORB-SLAM2 image pyramid representing the feature points.

[0073] With such a scheme, by epipolar geometric constraint condition, the feature points whose distance d from the projection point to the epipolar line l2 does not exceed the threshold value are determined as static points, and more feature points can be obtained for pose calculation, thereby providing calculation accuracy.

[0074] As shown in Figure 6 , the projection point P2' of P' is not on the epipolar line l2, and the distance of the projection point P2' to the epipolar line l2 exceeds the threshold value, which does not satisfy the epipolar constraint and is determined as a dynamic point.

[0075] S24: For the feature points (such as point P", whose projection point P2" is on l2) satisfying d < D n , the projection coordinates of the feature points at I2 are calculated according to the projection coordinates at I1 and the camera poses at I1 and I2, and if the calculated projection coordinates deviate too much from the actual projection coordinates, the feature points are also regarded as dynamic points. The calculation of the projection coordinates is represented as

[0076] The calculation formula is:

[0077]

[0078] where T cw_1 and T cw_2 are the transformation matrices of the world coordinate system to the camera coordinate system at I1 and I2, respectively, z is the corresponding depth coordinate value, and [u1 v1 z1] T is the projection coordinate at I1.

[0079] The actual projection coordinates are represented as P = [u2 v2 z2] T .

[0080] According to the chi-square distribution of the sum of pixel coordinates, the degree of freedom is 2, and the significance level is 0.05, that is, the confidence level is 95%.

[0081] S25: If the feature points satisfy the constraint condition: , the feature points are defined as static points, and if not, the feature points are removed as dynamic points; where n represents the number of image pyramids, and 5.99 is the chi-square statistical threshold value.

[0082] As shown in Figure 6 , the projection point P2" of P" satisfies the epipolar constraint, but according to the calculation, it does not satisfy the projection threshold condition, and it is regarded as a dynamic point.

[0083] With such a scheme, combined with the epipolar geometric constraint and the projection constraint, and the image pyramid for processing feature points by ORB-SLAM2, the threshold value is set for the feature points of adjacent two images, and the dynamic information can be effectively filtered out.

[0084] Since there are still some static points in the detection box of YOLOv4, if all the static points are removed when the detection box is too large, the number of feature points for matching will be greatly reduced, which may lead to system loss and reduced robustness. Therefore, the application makes a motion constraint on the static and dynamic points in the detection box to distinguish between static and dynamic points, and takes a retention strategy for static points.

[0085] As a specific solution, step S3 specifically includes the following steps:

[0086] S31: Obtain the coordinates of the feature points in the frame region at time t in the world coordinate system as

[0087] [X t Y t Z t ] T wc_t =T t KP wc_t ;

[0088] Where T t represents the transformation matrix from the camera coordinate system to the world coordinate system at time t, and P t represents the projection coordinates at time t.

[0089] S32: Calculate the speed V t of the feature points in the frame region at time t, and the expression of V t is:

[0090] V t+1 =([X t+1 Y t+1 Z T ] t -[X t Y t Z T ] / dt=[V x_t V y_t V z_t ] T ;

[0091] Where [X t+1 Y t+1 Z t+1 ] T represents the coordinates of the feature points in the frame region at time t+1 in the world coordinate system, and dt represents the time interval.

[0092] According to the absolute value of the speed change difference of the dynamic point in a short time is much smaller than the absolute value of the sum of the speeds at the previous and next two times.

[0093] S33: Remove the feature points that meet the constraint condition as the dynamic points in the frame, and the remaining feature points as the static points (also known as: suspicious static points); wherein the constraint condition is: ||V t+1 ​-V t ||1<<||V t+1 +V t ||1。

[0094] With such a scheme, the speed of the feature points in a certain time is calculated, and the corresponding threshold conditions are set according to the different motion laws of moving points and static points, so as to divide the moving and static points and make different treatments.

[0095] In order to make the system pose optimization use as many feature points as possible, the suspicious static points in the frame are reserved, and the static points outside the frame are included in the calculation.

[0096] As a specific scheme, the minimum error function is used to solve the optimal pose in step S4; wherein the minimum error function is:

[0097]

[0098] ∑2=ε×∑1=ε×n×E;

[0099] Wherein, T cw_t represents the transformation matrix from the world coordinate system to the camera coordinate system, S i and S j represent the coordinates of the static points outside the frame and the static points inside the frame, respectively, P i and P j represent the projection coordinates of the static points outside the frame and the static points inside the frame, respectively, ∑1 and ∑2 represent the third-order weight matrix of the corresponding points, that is, the information matrix, E represents the third-order unit matrix, and ε represents the weight coefficient.

[0100] With such a scheme, the suspicious static points in the image are included in the pose solution, and appropriate weight coefficients are assigned, the optimal pose is solved using the minimum error function, the utilization of image feature points is increased, and the robustness of the system is improved.

[0101] The minimum error function is also called the least squares method, which is a parameter estimation method, and there are two parts of data, the re-projection error of the static points inside the frame and the static points outside the frame.

[0102] As a preferred scheme, step S4 specifically includes the following steps:

[0103] Solve the re-projection error of the static points inside the frame and the static points outside the frame, and assign weights using the information matrix.

[0104] 1) Set the information matrix in the Bundle Adjustment of ORB-SLAM2, here the unit matrix E is selected, and the default is that each coordinate value error is shared.

[0105] 2) ORB-SLAM2 uses image pyramid, then extracts feature points at different scales, for the upper image, the coordinates of the feature points are multiplied by the number of layers n to restore the scale of the first layer.

[0106] 3) For the coordinates of the static points in the frame, because of its low reliability, multiply ε on the basis of the original information matrix, where the value of ε ranges from 0.6 to 0.7.

[0107] 4) Return to the Gauss-Newton iterative method to solve the re-projection error, the projection error term is denoted as e T , the error square term is e T , and ∑ is the corresponding information matrix.

[0108] The present application is tested and compared with ORB-SLAM2 on TUM dataset, using the evaluation program provided by TUM dataset, which are relative pose error (RPE) and absolute trajectory error (ATE) respectively. The fr3 sequence of TUM dataset is a typical dynamic environment, in which the fr3_sitting series is a low dynamic sitting person, and the fr3_walking series contains a high dynamic walking person. Figures 7 to 10 The absolute trajectory error (ATE) and relative pose error (RPE) of ORB-SLAM2 and the present application on walking_rpy and walking_xyz dataset. The black line is the ground truth, the blue line is the estimated trajectory, and the red line (difference) is the absolute trajectory error (ATE).

[0109] As shown in Figure 7 (a) Figure 9 (a), ORB-SLAM2 in the high dynamic scene of the walking series, because it includes the dynamic feature points on the walking person in the calculation when optimizing the pose, the absolute trajectory obtained is completely offset, and even the tracking fails. The coincidence degree of the present application and the ground truth is higher, as shown in Figure 7 (b) Figure 9 (b).

[0110] As shown in Figure 8 (a) Figure 10 (a), the maximum offset of the visual odometry of ORB-SLAM2 is 1.4m and 1.2m respectively. The maximum offset of the present application Figure 8 (b) Figure 10The maximum offset of (b) is 0.175m, 0.06m, and the pose accuracy is increased by 87.5% and 95%, respectively.

[0111] As shown in Table 1, 8 sequences of the TUM dataset fr3 are tested, and the root mean square value (RMS) of ATE of the present application and ORB-SLAM2 is compared.

[0112] Table 1 is an absolute trajectory error table (unit: m) of the present application and ORB-SLAM2 in the fr3 sequence.

[0113]

[0114] In the low dynamic scene of the sitting series, the person in the scene basically does not produce obvious movement, the ORB-SLAM2 system retains more static points, and the system can judge a small amount of dynamic points as false matching, so the result of pose calculation is slightly better than that of the present system.

[0115] In the high dynamic scene of the walking series, there are moving feature points in the image for a long time, ORB-SLAM2 cannot distinguish static and dynamic points, and when calculating the pose, the dynamic points are treated as static points, so the calculation result is greatly offset from the true value. The present application algorithm filters out the dynamic points on the person, reduces the interference of dynamic points on the system, and the accuracy is basically improved by more than 90%. The walking_rpy sequence is blurred due to the fast movement of the camera, the number of static points decreases, and the feature points are difficult to match, so the accuracy is improved by 69%.

[0116] The original ORB-SLAM2 open source code lacks a mapping part, so the present application adds a dense point cloud mapping thread based on ORB-SLAM2. First, the PCL library is configured in the environment.

[0117] As a specific scheme, step S5 includes the following steps:

[0118] S51: modifying the tracking thread tracking, inserting the key frame of the tracking thread into the point cloud map, obtaining the point cloud id associated with the key frame image and the transformation of the current frame relative to the world coordinate system.

[0119] S52: combining the information of the RGB-D image, finding the spatial position of the corresponding point, and constructing a spatial point cloud.

[0120] S53: in the viewer thread of ORB-SLAM2, the point cloud in each key frame is converted to the world coordinate system through rigid body transformation.

[0121] S54: the point cloud is processed by the statistical filter and the voxel filter to remove outliers and downsample, and the filtered point cloud is output to the global map.

[0122] S55: Add PCL point cloud display in the viewer thread of ORB-SLAM2, display the point cloud map in the display window of PCL.

[0123] Figure 11 The static point cloud map generated by ORB-SLAM2 in the fr3_walking static sequence has a large number of ghost shadows of dynamic objects, low flatness, and lost contours. The application filters out the dynamic information of the key frame, as shown in Figure 12 The generated static point cloud map has a clear background and scene object contour, high straightness and flatness, and basically eliminates ghosting.

[0124] With such a scheme, a dense mapping thread is added to ORB-SLAM2, the key frame with filtered dynamic information is input into the thread, the point cloud is generated according to the depth information of the image, and the static point cloud map is further generated.

[0125] In summary, the target detection network is applied to the field of visual SLAM, the dynamic information is filtered out, and the effective static points in the image are included in the pose calculation, which is suitable for actual complex scenes, including high dynamic, low dynamic and static environments.

[0126] The above description is only some of the preferred embodiments of the present disclosure and the explanation of the applied technical principles. Those skilled in the art should understand that the scope of the application involved in the embodiments of the present disclosure is not limited to the technical solutions formed by the specific combinations of the above technical features, and should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the above inventive concept. For example, the above features are replaced with the technical features disclosed in the embodiments of the present disclosure (but not limited to) having similar functions to form technical solutions.

Claims

1. A visual SLAM method suitable for dynamic environments, characterized in that: The method comprises the following steps: extracting feature points from the input image, and dividing the input image into an in-frame region and an out-of-frame region; filtering out dynamic points in the out-of-frame region through geometric constraints to obtain static points in the out-of-frame region; calculating the speed of the feature points in the in-frame region to divide the feature points into dynamic points in the in-frame region and static points in the in-frame region; including the static points in the in-frame region and the static points in the out-of-frame region in pose solving to obtain an optimal pose; the extraction of the feature points from the input image is realized by ORB-SLAM2, and the division of the input image into the in-frame region and the out-of-frame region is realized by YOLOv4; the filtering out of the dynamic points in the out-of-frame region through the geometric constraints to obtain the static points in the out-of-frame region comprises the following steps: Let I1 and I2 be two adjacent frames of images in camera motion, respectively corresponding to two different time points, O1 and O2 be the camera optical centers at the two different time points, and P be a space point, the projections of which on I1 and I2 be P1 and P2 respectively; taking the line segment O1O2 as a base line, the intersection points of the base line with I1 and I2 be e1 and e2 respectively; the intersection lines of the plane O1O2P with I1 and I2 be l1 and l2, and the intersection lines are called epipolar lines; A pixel coordinate system is established, and [u1, v1] and [u2, v2] are respectively pixel coordinate system coordinates of the projection point at the I1 and I2 moments, and the polar line l2 is expressed as in the uv rectangular coordinate system, and the distance from the projection point to the polar line l2 is , wherein , F represents a basis matrix from I1 to I2, and K represents a camera parameter matrix; Setting threshold D n , if D n , the feature point is determined as a static point; if D n , the feature point is determined as a dynamic point and is removed; wherein the expression of D n is ; n represents the pyramid layer number of the feature point in the image pyramid of ORB-SLAM2; and the feature points in the out-of-frame region are filtered by geometric constraints to remove the out-of-frame dynamic points to obtain the out-of-frame static points. For feature points satisfying D n , the projection coordinates of the feature points at I2 are calculated according to the projection coordinates of the feature points at I1 and the camera poses at I1 and I2, and the calculated projection coordinates are , the calculation formula is: where T cw_1 and T cw_2 are the transformation matrices from the world coordinate system to the camera coordinate system at I1 and I2 respectively, and z is the corresponding depth coordinate value, is the projected coordinate at I1; the actual projected coordinate is represented as . If the feature point satisfies a constraint condition: the feature point is defined as a static point, and if the constraint condition is not satisfied, the feature point is removed as a dynamic point; wherein n represents a number of layers of the image pyramid; the speed calculation on the feature points in the region in the frame to divide the dynamic points in the frame and the static points in the frame comprises: Coordinates of feature points in the world coordinate system of the frame region at time t wherein T wc_t denotes the transformation matrix of the camera coordinate system to the world coordinate system at time t, P t denotes the projection coordinates at time t; calculating the speed of the feature point in the region of the frame at time t , the expression is: ; wherein, denotes the coordinates of the feature point in the world coordinate system of the adjacent in-frame region at time t+1, and dt denotes the time interval; The feature points satisfying the constraint condition are taken as in-frame moving points for elimination, and the remaining feature points are taken as in-frame static points; wherein the constraint condition is: .

2. The visual SLAM method suitable for a dynamic environment according to claim 1, characterized in that: an error function is minimized to solve the optimal pose; wherein the error function is minimized as follows: ； ； where T cw_t is the transformation matrix from the world coordinate system to the camera coordinate system, S i and S j are the coordinates of the out-of-frame static points and in-frame static points, respectively, P i and P j are the projection coordinates of the out-of-frame static points and in-frame static points, respectively, and Σ1 and Σ2 are the third-order weight matrices of the corresponding points, i.e., information matrices, E is a third-order identity matrix, and ε is a weight coefficient.

3. The visual SLAM method suitable for dynamic environments of claim 2, wherein: the re-projection error of the static points in the in-frame region and the static points in the out-of-frame region is solved, and an information matrix is used to assign weights. the re-projection error of the static points in the in-frame region and the static points in the out-of-frame region is solved, and an information matrix is used to assign weights, which comprises the following steps:

4. The visual SLAM method suitable for dynamic environments of claim 3, wherein: an information matrix is set in the Bundle Adjustment of ORB-SLAM2, and here a unit matrix E is selected by default, and each coordinate value error is shared; ORB-SLAM2 uses an image pyramid, and then feature points are extracted at different scales; for the feature points extracted from the upper layer image, the coordinates of the feature points are multiplied by the layer number n so that the coordinates are restored to the scale on the first layer; for the coordinates of the static points in the in-frame region, the coordinates are multiplied by ε on the basis of the original information matrix, wherein the value range of ε is 0.6 to 0.7; the visual SLAM method suitable for a dynamic environment further comprises: Returning to the Gauss-Newton iteration for solving the re-projection error, let the projection error term be e T with the error square term e T ∑e, where ∑ is the corresponding information matrix.

5. The visual SLAM method suitable for dynamic environments of claim 1, wherein: a dense point cloud mapping thread is added to ORB-SLAM2.

6. The visual SLAM method suitable for a dynamic environment according to claim 5, characterized in that: the dense point cloud mapping thread added to ORB-SLAM2 comprises the following steps: the tracking thread is modified, the key frame of the tracking thread is inserted into the point cloud map, the point cloud id associated with the key frame image is obtained, and the transformation of the current frame relative to the world coordinate system is obtained; the spatial positions of the corresponding points are found by combining the information of the RGB-D image, and a spatial point cloud is constructed; in the viewer thread of ORB-SLAM2, the point cloud in each key frame is converted to the world coordinate system through rigid body transformation; the point cloud is processed by an out-point removal filter and a voxel filter through a PCL library function to remove outliers and downsample the point cloud, and the filtered point cloud is output to the global map; ​ The PCL point cloud display is added in the viewer of ORB-SLAM2, and the point cloud map is displayed in the display window of PCL.

Images