Synchronous localization and mapping method and device in dynamic environment

Abstract

The invention provides a synchronous positioning and mapping method and device in a dynamic environment, and belongs to the technical field of robot mapping and localization, and the method comprises the steps: carrying out the target detection of a to-be-detected image, and obtaining a detection frame and the type of a target object; judging the state of the target object according to the corresponding relation between the object category and the state in a preset rule; dividing each pixel in the to-be-detected image into a neighborhood window with a preset size, calculating a local threshold value of each pixel point, and extracting feature points of the to-be-detected image based on the local threshold values; traversing the feature points, and removing the feature points in the detection frame where the target object in the motion state is located; performing camera pose estimation according to the target feature point to obtain camera pose data; and constructing a map based on the camera pose data. According to the method, the feature points of the to-be-detected image in the dynamic environment can be accurately identified, and the dynamic feature points in the feature points are removed, so that more accurate positioning and map construction are realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot mapping and positioning, and in particular relates to a method and device for synchronous positioning and map construction in a dynamic environment. Background Art

[0002] Visual SLAM (Simultaneous Location And Mapping, SLAM) technology is a technology that uses a camera as a sensor to simultaneously locate and build maps in an unknown environment. It estimates the camera's motion trajectory (camera pose estimation) by processing continuous images captured by the camera and builds a three-dimensional map of the environment based on the camera pose.

[0003] At present, most existing algorithms of visual SLAM technology in static scenes have achieved high-precision positioning and demonstrated stable performance. However, in the real world, the environment is dynamically changing, including moving objects and changing lighting. The appearance of dynamic objects and changes in lighting will have a significant impact on the extraction and matching of feature points in the image, which in turn affects the estimation of the camera pose, resulting in a decrease in the accuracy of positioning and mapping. Summary of the invention

[0004] In order to solve the problem of low positioning and mapping accuracy in the prior art, the present invention provides a method and device for synchronous positioning and mapping in a dynamic environment.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A method for synchronous positioning and map construction in a dynamic environment, comprising:

[0007] Acquire an image to be detected captured by a camera, wherein the image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing light;

[0008] Perform target detection on the image to be detected to obtain the detection frame and category of the target object in the image to be detected; determine the state of the target object according to the correspondence between the object category and the state in the preset rules; the state includes the moving state and the static state;

[0009] Divide each pixel in the image to be detected into a neighborhood window of a preset size, calculate the local threshold of each pixel point according to the grayscale value of the pixel in the neighborhood window, and extract the feature points of the image to be detected based on the local threshold;

[0010] Traverse the extracted feature points, remove the feature points in the detection frame where the target object in motion is located, obtain the target feature points, estimate the camera pose based on the target feature points, and obtain the camera pose data;

[0011] Build a map based on camera pose data.

[0012] Optionally, target detection is performed on the image to be detected by an improved YOLOv5 network to obtain a detection frame and category of the target object in the image to be detected, wherein the improved YOLOv5 network includes an improved backbone network, a neck network and a head network, and an improved backbone network is formed by integrating multiple ShuffleNetv2 units after the maximum pooling layer of the original backbone network and adding an EMA attention mechanism between the last ShuffleNetv2 unit and the SPPF module;

[0013] The improved YOLOv5 network is used to perform target detection on the image to be detected, and the detection frame and category of the target object in the image to be detected are obtained, including:

[0014] The image to be detected is input into the improved backbone network, and the features of the image to be detected are extracted through the CBRM convolutional layer sequence;

[0015] The features extracted by the CBRM convolutional layer sequence are processed by convolution operations through multiple ShuffleNetv2 units to generate multi-scale feature maps, and the multi-scale feature maps are weighted by the EMA attention mechanism to obtain enhanced feature maps;

[0016] The enhanced feature map is input into the SPPF module for feature fusion, and the fused features are predicted through the head network to output the detection box and category of the target object in the image to be detected.

[0017] Optionally, a local threshold of each pixel is calculated according to the grayscale value of the pixel in the neighborhood window by using an improved Sauvola algorithm, including:

[0018] For each pixel in the image to be detected, calculate the mean grayscale value of all pixels in its neighborhood window. The formula is:

[0019]

[0020] Among them, m(x,y) represents the mean gray value of the pixel points in the neighborhood window of the pixel point with coordinates (x,y), r represents the neighborhood radius, and g(i,j) represents the gray value of the pixel point with coordinates (i,j) in the neighborhood window;

[0021] The variance is calculated based on the mean, and the formula is:

[0022]

[0023] Among them, s(x,y) represents the standard deviation of the grayscale values ​​of the pixels in the neighborhood window of the pixel with coordinates (x,y);

[0024] The local threshold is calculated based on the standard deviation, and the formula is:

[0025]

[0026] Where T represents the local threshold and R is a fixed parameter representing the dynamic range of the standard deviation.

[0027] Optionally, extracting feature points of the image to be detected based on a local threshold includes:

[0028] Calculate the grayscale value difference between the current pixel and all pixels in its neighborhood window, compare the grayscale value difference with the local threshold, and if the grayscale value difference between a preset number of pixels and the current pixel is greater than the local threshold, determine the current pixel as a feature point; or,

[0029] According to the preset screening rules, some pixels are selected in the neighborhood window of the current pixel, the gray value difference between some pixels and the current pixel is calculated, the gray value difference is compared with the local threshold, and feature points are extracted.

[0030] Optionally, after obtaining the target feature points, the following steps are further included:

[0031] Divide the image to be detected into four regions of equal size as the root nodes of the quadtree;

[0032] Count the number of feature points in each area. If the number of feature points in a target area exceeds the preset threshold, continue to divide the target area into four sub-areas;

[0033] The segmentation step is repeated until the number of feature points in all regions does not exceed a preset threshold or reaches a preset maximum depth.

[0034] The present invention also provides a synchronous positioning and map building device in a dynamic environment, comprising:

[0035] An acquisition module is used to acquire an image to be detected captured by a camera, wherein the image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing light;

[0036] The detection module is used to perform target detection on the image to be detected, obtain the detection frame and category of the target object in the image to be detected; determine the state of the target object according to the correspondence between the object category and the state in the preset rules; the state includes the moving state and the static state;

[0037] A feature extraction module is used to divide each pixel in the image to be detected into a neighborhood window of a preset size, calculate the local threshold of each pixel point according to the gray value of the pixel in the neighborhood window, and extract feature points of the image to be detected based on the local threshold;

[0038] The pose estimation module is used to traverse the extracted feature points, remove the feature points in the detection frame where the target object in motion is located, obtain the target feature points, and perform camera pose estimation based on the target feature points to obtain camera pose data;

[0039] Map construction module, used to build maps based on camera pose data.

[0040] The present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned synchronous positioning and map building method in a dynamic environment when executing the program.

[0041] The present invention also provides a computer-readable storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the method for synchronous positioning and map construction in a dynamic environment is implemented.

[0042] The method for synchronous positioning and map construction in a dynamic environment provided by the present invention has the following beneficial effects:

[0043] When extracting feature points, the present invention divides each pixel in the image to be detected into a neighborhood window of preset size, and calculates the local threshold of each pixel according to the grayscale value of the pixel in the neighborhood window. Since the present invention is aimed at the image to be detected in a dynamic environment, the local threshold of each pixel is dynamically calculated according to the grayscale value of the local area of ​​the image. Compared with the fixed threshold, the local threshold can adapt to the contrast changes in different areas of the image, thereby more accurately extracting the feature points in the image. Furthermore, in the target detection stage, the motion category of the object is judged based on the object category, and the feature points in the detection box where the object of the motion state category is located are removed according to the motion category before the pose estimation. This can avoid the interference of dynamic feature points in the image to be detected on the camera pose estimation, thereby reducing the error of the camera pose estimation, obtaining more accurate positioning data, and thus achieving more accurate map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] In order to more clearly illustrate the embodiment of the present invention and its design scheme, the following briefly introduces the drawings required for this embodiment. 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 creative work.

[0045] Figure 1 A schematic flow chart of a method for synchronous positioning and map building in a dynamic environment provided by an embodiment of the present invention;

[0046] Figure 2A schematic diagram of the structure of an improved YOLOv5 network provided in an embodiment of the present invention;

[0047] Figure 3 A schematic flow chart of extracting feature points provided by an embodiment of the present invention;

[0048] Figure 4 A schematic flow chart of another method for synchronous positioning and map building in a dynamic environment provided by an embodiment of the present invention;

[0049] Figure 5 A comparison diagram of the number of feature point extractions at various brightnesses provided by an embodiment of the present invention; wherein: Figure 5 Figure (a) is a comparison of the number of feature points extracted at 30% brightness; Figure 5 Figure (b) is a comparison of the number of feature points extracted at 60% brightness; Figure 5 Figure (c) is a comparison of the number of feature points extracted at 100% brightness; Figure 5 Figure (d) is a comparison of the number of feature points extracted at 130% brightness;

[0050] Figure 6 The actual road test results of each network on the KITTI and BDD100K data sets provided by the embodiment of the present invention; wherein, Figure 6 Figure (a) shows the actual test results of the original YOLO network on the road. Figure 6 Figure (b) shows the actual road test results of the improved YOLOv5 network of the present invention;

[0051] Figure 7 A test result diagram of a Sitting_halfsphere sequence provided in an embodiment of the present invention; wherein, Figure 7 Figure (a) is the XYZ axis view. Figure 7 Figure (b) is the Euler angle view;

[0052] Figure 8 A schematic diagram of trajectories corresponding to various algorithms under different sequence data provided by an embodiment of the present invention; wherein, Figure 8 Figure (a) is a schematic diagram of the trajectory corresponding to the Sitting_halfsphere sequence; Figure 8 Figure (b) is a schematic diagram of the trajectory corresponding to the Sitting_rpy sequence; Figure 8 Figure (c) is a schematic diagram of the trajectory corresponding to the Sitting_static sequence; Figure 8 Figure (d) is a schematic diagram of the trajectory corresponding to the Sitting_xyz sequence;

[0053] Fig. 9A schematic diagram of trajectories corresponding to various algorithms under different sequence data provided by an embodiment of the present invention, wherein: Fig. 9 Figure (a) is a schematic diagram of the trajectory corresponding to the Walking_xyz sequence; Fig. 9 Figure (b) is a schematic diagram of the trajectory corresponding to the Walking_rpy sequence; Fig. 9 Figure (c) is a schematic diagram of the trajectory corresponding to the Walking_static sequence; Fig. 9 Figure (d) is a schematic diagram of the trajectory corresponding to the Walking_halfsphere sequence;

[0054] Fig.10 A schematic diagram of an ORB algorithm and a feature point extraction result of the algorithm of the present invention provided in an embodiment of the present invention; wherein, Fig.10 Figure (a) shows the ORB feature point extraction result; Fig.10 Figure (b) shows the feature point extraction result of the algorithm of the present invention;

[0055] Fig.11 The present invention provides a sequence trajectory data in a KITTI data set, wherein: Fig.11 Figure (a) shows the trajectory data generated by the 07 sequence; Fig.11 Figure (b) shows the trajectory data generated by the 09 sequence;

[0056] Fig.12 A schematic diagram of feature point extraction results on a measured road provided by an embodiment of the present invention; Fig.12 Figure (a) shows the feature point extraction result of the original algorithm on the measured road. Fig.12 Figure (b) shows the feature point extraction result of the algorithm of the present invention on the measured road;

[0057] Fig.13 A point cloud map is provided in an embodiment of the present invention; wherein, Fig.13 Figure (a) shows the point cloud map constructed from some sequences of the TUM dataset. Fig.13 Figure (b) is a point cloud map constructed in a real indoor environment; Fig.14 A schematic diagram of the structure of a synchronous positioning and map building device in a dynamic environment provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0058] In order to enable those skilled in the art to better understand the technical solution of the present invention and implement it, the present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the scope of protection of the present invention.

[0059] In the description of the present invention, it is to be understood that the terms “center”, “longitudinal”, “lateral”, “length”, “width”, “thickness”, “up”, “down”, “front”, “back”, “left”, “right”, “vertical”, “horizontal”, “top”, “bottom”, “inside”, “outside”, “axial”, “radial”, “circumferential”, etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing the technical solutions of the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0060] In addition, the terms "first", "second", etc. are used for descriptive purposes only and are not to be understood as indicating or implying relative importance. In the description of the present invention, it should be noted that, unless otherwise clearly specified or limited, the terms "connected" and "connected" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to the specific circumstances. In the description of the present invention, unless otherwise specified, "plurality" means two or more, which will not be described in detail here.

[0061] Example 1

[0062] Figure 1 A schematic flow chart of a method for synchronous positioning and map building in a dynamic environment provided by an embodiment of the present invention. Optionally, the execution subject of the method can be a robot equipped with a specific sensor.

[0063] Exemplarily, the method is performed as follows:

[0064] S1, obtaining the image to be detected captured by the camera.

[0065] The image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing illumination.

[0066] It should be understood that the robot is equipped with sensors (for example, RGBD cameras), which sense the surrounding environment in real time and collect images to be detected.

[0067] S2, performing target detection on the image to be detected to obtain a detection frame and category of the target object in the image to be detected; and determining the state of the target object according to the correspondence between the object category and the state in the preset rules.

[0068] In an embodiment of the present invention, target detection is performed on the image to be detected through an improved YOLOv5 network.

[0069] Figure 2 A schematic diagram of the structure of an improved YOLOv5 network provided in an embodiment of the present invention.

[0070] For example, Figure 2 As shown in the figure, the improved YOLOv5 network structure is mainly divided into three parts: the improved backbone network, the neck network and the head network. Among them, the improved backbone network integrates multiple ShuffleNetv2 units after the maximum pooling layer in the original backbone network, and adds the EMA (Efficient Multi-Scale Attention) attention mechanism between the last ShuffleNetv2 unit and the fast spatial pyramid pooling (Spatial Pyramid Pooling Fast, SPPF) module. The backbone network is the core part of image feature extraction. Its function is to convert the original input image into a feature map. By dividing the input image into different grid units and predicting the category and bounding box of the target for each unit, feature maps of different scales are obtained for subsequent target detection tasks. The neck network fuses feature maps of different scales to better detect targets of different sizes. The head network is located at the end of the model and is responsible for predicting the category and bounding box position of the target.

[0071] EMA is an attention mechanism for natural language processing tasks. It aims to improve the efficiency and effectiveness of the standard multi-head attention mechanism. Its core idea is to improve efficiency by reducing computational complexity and the number of parameters while maintaining or improving model performance.

[0072] The EMA attention mechanism redesigns part of the channel dimension into multiple sub-feature groups to ensure that the spatial semantic features are evenly distributed in each feature group. At the same time, the encoded global information is used to recalibrate the weights of the channels in each parallel branch, and the cross-dimensional interaction is used to capture the pairwise relationship at the pixel level. By normalizing the global information tensor output by the 1×1 branch, encoding the branch and the third branch respectively, and then fitting them through Softmax, the features of the output minimum branch are directly transformed in the channel dimension to achieve cross-dimensional integration of spatial information. Subsequently, the cross-spatial interaction module is used to fuse the two channel attention weight values ​​to capture the corresponding relationship at the pixel level. Finally, the activation function is used for nonlinear transformation and the feature map is output, thereby effectively improving the original input features.

[0073] It should be understood that the improved YOLOv5 network was trained with different data sets in the early stage to adapt to different environmental requirements. In outdoor scenes, the widely recognized BDD100K data set is used, which contains a wealth of traffic scene images, which helps to improve the model's detection capabilities for outdoor target objects such as vehicles and pedestrians; while in indoor environments, the COCO data set is used, which covers a variety of indoor object images, providing the model with recognition training for indoor target objects such as furniture and daily necessities.

[0074] After training with different data sets, the improved YOLOv5 network can not only identify target objects and classify them accurately, but also save the location information of each detection box.

[0075] Based on the above improved YOLOv5 network, the process of performing target recognition on the image to be detected through the improved YOLOv5 network to obtain the detection frame and category of the target object in the image to be detected is as follows:

[0076] First, the image to be detected is input into the improved backbone network, and the features of the image to be detected are extracted through the CBRM convolutional layer sequence, which includes the convolutional layer (Conv 2d), batch normalization layer (BN), activation function (ReLu) and maximum pooling layer (MaxPool).

[0077] Then, multiple ShuffleNetv2 units are used to perform convolution operations on the features extracted by the CBRM convolutional layer sequence to generate multi-scale feature maps, and the multi-scale feature maps are weighted through the EMA attention mechanism to obtain enhanced feature maps.

[0078] Finally, the enhanced feature map is input into the SPPF module for feature fusion, and the fused features are predicted through the head network to output the detection box and category of the object in the image to be detected.

[0079] Exemplarily, in the backbone network, the convolution layer is first used to extract image features, and the output of the convolution layer is normalized through the batch normalization layer; then it is processed through the ReLu activation function, and the output of the convolution layer is downsampled through the MaxPool maximum pooling layer to perform feature extraction and dimensionality reduction to reduce the amount of calculation.

[0080] The ShuffleNetv2 unit then processes the output of the CBRM, including further convolution operations, depth convolution, and channel shuffling, to increase the depth and width of the network, which helps improve the generalization ability of the model. Multi-scale feature maps are generated through different pooling or convolution operations. The EMA attention mechanism enhances the model's perception of features of different scales by weighting the features, better captures the target information, and outputs enhanced feature maps. The feature map after attention weight adjustment is used as the network output and connected to the SPPF module to connect the feature representations of the same feature map at different scales.

[0081] The neck network further processes and fuses the features from the backbone network, and finally predicts through the head network to output the detection box and category of the target object in the image to be detected.

[0082] In the above embodiment, ShuffleNetv2 units are introduced on the basis of the original framework of the YOLOv5 network to perform model lightweight processing. These ShuffleNetv2 units perform efficient channel mixing lightweight operations, which can extract rich features while maintaining high computational efficiency; in addition, the EMA mechanism based on cross-space multi-scale attention is introduced. The EMA attention mechanism enhances the representation ability of the feature map by weighting the features, better captures the target information, and improves the target detection accuracy of the model while the parameter size and other settings remain basically unchanged. The feature information extracted by the backbone network is fused and processed in the neck network to generate a more accurate feature map.

[0083] Compared with the original YOLOv5 network model, the improved model reduces the amount of computation consumed when visual SLAM uses the target detection network in a dynamic environment. It also strikes a balance between detection accuracy and model complexity, significantly reducing model parameters and scale while keeping detection accuracy basically unchanged. It is more efficient and has stronger expressive power when extracting features.

[0084] After obtaining the detection frame and category of the target object in the image to be detected through the YOLOv5 network model, the preset rules are obtained, and the state of the target object is judged according to the correspondence between the object category and the state in the preset rules.

[0085] The states include motion state and static state, and the motion state can be divided into high dynamics and low dynamics.

[0086] Among them, the preset rules include the correspondence between object categories and states. For example, objects with active movement capabilities, such as people and pets, are classified as high-dynamic objects. The movements of these objects are often autonomous and have the greatest impact on the environment; objects such as cups and chairs that may be forced to move by high-dynamic objects are classified as low-dynamic objects; and objects such as tables and bookshelves that are not prone to changing positions are classified as static objects.

[0087] It should be understood that, unlike the characteristics of outdoor traffic environments, the dynamics of objects in indoor environments are more diverse. The embodiment of the present invention subdivides common indoor objects into three categories: high-dynamic objects, low-dynamic objects, and static objects. This classification helps the model understand the dynamic changes of indoor environments more finely, thereby making more reasonable decisions.

[0088] S3, dividing each pixel in the image to be detected into a neighborhood window of a preset size, calculating a local threshold of each pixel point according to the grayscale value of the pixel in the neighborhood window, and extracting feature points of the image to be detected based on the local threshold.

[0089] It should be understood that the local threshold can also be understood as a local adaptive threshold. The method for extracting feature points of an image to be detected based on a local threshold provided in an embodiment of the present invention (hereinafter referred to as an adaptive threshold feature point extraction method) is implemented based on an ORB (Oriented FAST and Rotated BRIEF) feature point algorithm. The ORB feature point algorithm uses a FAST corner point detector and has a relatively fast running speed.

[0090] The logic of FAST corner detector in judging corners is as follows: when a target pixel needs to be judged as a corner, the brightness, i.e., grayscale value, of the pixel is compared with that of the surrounding pixels. If the difference is large and exceeds a certain threshold, then the pixel is very likely to be a corner.

[0091] The ORB feature point algorithm uses the BRIEF descriptor as the feature vector of each feature point. The descriptor is a binary vector composed of 0 and 1, and its generation process is based on the grayscale value relationship of the pixel pairs (such as p and q) selected near the feature point. If the grayscale value of p is greater than q, the corresponding position is 1, and if it is less than q, it is 0. Through 128 groups of p and q, a 128-dimensional binary vector is generated.

[0092] BRIEF adopts binary expression and uses Hamming distance, which is convenient for storage and fast for calculation.

[0093] In the process of corner point judgment, the size of the threshold is very important for the extraction of feature points, which determines the number and quality of feature points. If the threshold is too large, there may be too few feature points, making it difficult to match and track; if the threshold is too small, there may be too many feature points, increasing the amount of calculation and the risk of mismatching. In visual SLAM application scenarios, when there are dynamic changes in the environment such as changes in lighting, a fixed threshold will inevitably lead to the above two situations.

[0094] To solve the above problem, the present invention is based on the local threshold concept of Sauvola algorithm and dynamically determines the local threshold of each pixel.

[0095] The input of the Sauvola algorithm is a grayscale image. The current pixel is taken as the center pixel, and the threshold of the pixel is dynamically calculated based on the grayscale mean and standard deviation in the neighborhood of the current pixel.

[0096] Assume that the coordinates of the current pixel are (x, y), the neighborhood centered on the point is r*r, g(x, y) represents the grayscale value at (x, y), and the steps of the original Sauvola algorithm are:

[0097] First, calculate the grayscale mean m(x,y) and standard deviation s(x,y) in the neighborhood. The formula is as follows:

[0098]

[0099] Secondly, calculate the threshold T of the pixel point (x, y), the formula is as follows:

[0100]

[0101] In the above formula, m(x,y) represents the mean grayscale value of the pixel in the neighborhood size of the pixel with coordinates (x,y); r represents the neighborhood radius; g(i,j) represents the grayscale value of the pixel with coordinates (i,j) in the neighborhood range; s(x,y) represents the standard deviation of the grayscale value of the pixel in the neighborhood size of the pixel with coordinates (x,y); T is the threshold; k is a modified adjustable parameter used to adjust the sensitivity of the threshold to local contrast, generally between 0 and 1; R is a fixed parameter, which represents the dynamic range of the standard deviation and is used to control the local standard deviation and normalization. If the current input image is an 8-bit grayscale image, then R = 128.

[0102] Based on this, the present invention improves the formula for calculating the pixel threshold in the last step of the original Sauvola algorithm, as follows:

[0103]

[0104] The improved formula is more representative of local changes, and emphasizes the difference between the local standard deviation s(x,y) and the dynamic range R, which helps to produce a more obvious binarization effect in high-contrast areas of the image. The original formula may cause the threshold T to have a negative value, which is meaningless in practical applications. The improved formula ensures that the threshold is always in a non-negative state.

[0105] Exemplarily, a neighborhood window of 16 (4×4) pixels is divided with each pixel in the image to be detected as the center, the mean and standard deviation of the grayscale values ​​of the pixels in the neighborhood window are calculated by the above original formula, and then the local adaptive threshold is calculated by the improved formula.

[0106] Furthermore, after obtaining the local threshold of each pixel, the grayscale value difference between the current pixel and the pixels in its neighborhood window is calculated, and the grayscale value difference is compared with the local threshold. If the grayscale value difference between a preset number of consecutive pixels and the current pixel is greater than the local threshold, the current pixel is determined to be a feature point.

[0107] Exemplarily, taking the above-mentioned neighborhood window of 4×4 size as an example, the grayscale values ​​of the current pixel are compared with the grayscale values ​​of 15 other pixels in the neighborhood window. If the difference between n consecutive pixels (for example, 12) and the current pixel is greater than the local threshold, the current pixel is determined to be a feature point.

[0108] It should be understood that since the grayscale values ​​of adjacent pixels may not differ much, in order to improve the efficiency of feature point determination, some pixels can be selected in the neighborhood window of the current pixel according to the preset screening rules; the grayscale value difference between some pixels and the current pixel is calculated, and the grayscale value difference is compared with the local threshold to extract feature points. For specific methods, please refer to Figure 3 As shown, Figure 3 A schematic flowchart of extracting feature points provided by an embodiment of the present invention.

[0109] For example, Figure 3 As shown in the figure, the process of feature point extraction is as follows:

[0110] First, construct a Gaussian pyramid.

[0111] Apply a Gaussian filter to the original image for blurring, then downsample the blurred image, repeat the above steps with the reduced image as input, and build the next layer of the pyramid until the pyramid level or image size threshold is reached. The downsampled images at each level are used as a layer of the pyramid and are arranged in order to form a pyramid structure.

[0112] Then, read the gray value of the current pixel, calculate the local threshold T in the current neighborhood window (4×4), number the 16 pixels in the neighborhood window, first compare the gray values ​​of pixels numbered 1 and 9 with the current pixel (center pixel), if any one of them does not meet the condition (that is, the gray value difference between any pixel of 1 and 9 and the current pixel is less than or equal to the local threshold), the current pixel is eliminated; if the condition is met (that is, the gray value difference between any pixel of 1 and 9 and the current pixel is greater than the local threshold), four points 3, 5, 11 and 13 are extracted from the remaining numbers for judgment; if at most two of these four points do not meet the condition, the current pixel is determined to be a candidate point, and 8, 15 and 16 are continued to be extracted from the remaining numbers for judgment; if at most one point of the result does not meet the condition, the current pixel is determined to be a feature point.

[0113] Traverse all the pixels. If all the pixels have been traversed, the extraction process ends. If the pixels have not been traversed, continue to select the next pixel to read the grayscale value and repeat the above judgment process.

[0114] S4, traverse the extracted feature points, remove the feature points in the detection frame where the moving object is located, obtain the target feature points, estimate the camera pose based on the target feature points, and obtain the camera pose data.

[0115] Exemplarily, traverse each feature point to determine whether its position is only located in the detection frame of the high-dynamic object. If so, remove it; if not, determine whether it is in the intersection area of ​​the low-dynamic and high-dynamic object detection frames. If so, determine it as a dynamic feature point and remove it; if it exists alone and has no intersection with the detection frame of the high-dynamic object, determine it as a static feature point and retain the feature point (i.e., the target feature point).

[0116] In the above embodiment, removing dynamic feature points and retaining static feature points can eliminate the influence of dynamic feature points on subsequent camera pose estimation, thereby achieving more accurate pose estimation. It should be understood that since the indoor environment space is relatively limited and most objects are stationary or only move under specific circumstances, the dynamic feature point removal method designed by the present invention is mainly aimed at indoor environments.

[0117] Furthermore, the camera pose is estimated based on the target feature points to obtain camera pose data.

[0118] Specifically, feature point matching is performed based on the target feature points. After the correct feature point matching, the camera pose estimation can be performed. Different types of cameras use different pose calculation methods. The main difference lies in whether depth information can be obtained. Depth cameras generally use feature point method or iterative closest point ICP method to solve their pose.

[0119] The monocular camera is usually initialized using epipolar geometry to solve its pose. The pixel positions of the pixels p1 and p2 generated by the three-dimensional space point P are:

[0120]

[0121] In the above formula, the coordinates of P are assumed to be [X, Y, Z] T ; K is the camera intrinsic parameter; s1 and s2 are scale factors; R and t are the rotation matrix and translation matrix of the camera center O1 to O2 respectively.

[0122] Eliminating the scale factors s1 and s2, the scale meanings are equal under homogeneous coordinates, which can be expressed as follows:

[0123]

[0124] Let x1 = K -1 p1,x2=K -1 p2, x1, and x2 are the normalized plane coordinates of the pixel points, expressed as follows:

[0125]

[0126] Multiply both sides by the antisymmetric matrix t^ of t, and then multiply by x2 T Since t^x2 is perpendicular to both t and x2, its value is equal to 0 when the inner product is made with x2, and we can get:

[0127]

[0128] Substituting p1 and p2 again, we can get:

[0129]

[0130] The middle part of the above formula is transformed into matrices E and F respectively, expressed as follows:

[0131]

[0132] Among them, E is the essential matrix and F is the basic matrix.

[0133] From the above, E or F is solved with the help of the correctly matched feature points, and the camera pose can be estimated based on the result. The least squares optimization algorithm is used to minimize the reprojection error of the matching point pairs.

[0134] Optionally, to further improve the accuracy of camera pose estimation, after obtaining the target feature points, quadtree homogenization is performed to adjust the distribution of feature points in each layer of the image to be more uniform. The process is as follows: divide the image to be detected into four equal-sized regions, namely the root nodes of the quadtree; count the number of feature points in each region. If the number of feature points in a region exceeds the preset threshold, continue to divide the region into four sub-regions and count the number of feature points in each sub-region; repeat the above region separation steps until the number of feature points in all regions does not exceed the threshold or reaches the preset maximum depth. After quadtree homogenization, the distribution of feature points will be more uniform. In visual SLAM, these uniformly distributed feature points can be used for more accurate pose estimation and map construction.

[0135] S5, build a map based on the camera pose data.

[0136] It should be understood that the process of building a map using camera pose data can be implemented with reference to existing technologies.

[0137] Example 2

[0138] Figure 4 Another method for synchronous positioning and map construction in a dynamic environment is provided by an embodiment of the present invention.

[0139] For example, Figure 4 As shown, the method comprises the following steps:

[0140] S40, obtaining an image to be detected captured by a camera, inputting the image to be detected into an improved YOLOv5 network for target detection, obtaining a detection frame and category of the target object, calculating an adaptive local threshold through an improved Sauvola algorithm, and extracting feature points based on the local threshold.

[0141] Exemplarily, this process can refer to the relevant description of the S1-S3 process in the above embodiment, which will not be repeated here.

[0142] S41, feature point matching and eliminating false matches.

[0143] In the embodiment of the present invention, the Hamming distance is used as the image I t and I t+1 When the Hamming distance between descriptors exceeds twice the preset minimum distance, it is judged as a wrong match. In this case, matches with high dissimilarity can be filtered out to improve the accuracy of matching.

[0144] When there are fewer feature points, the distance between the feature point descriptors in the two images can be directly calculated and sorted; when there are more feature points, matching will consume a lot of time and affect the real-time performance of SLAM. In this case, the bag-of-words model is used to accelerate feature point matching, and the RANSAC method is combined to eliminate mismatched points to improve the matching accuracy of feature points.

[0145] The steps of RANSAC method to remove mismatched points are as follows:

[0146] Step 1: Randomly sample K points from sample n. Where n represents the total number of points in the data set, which is often the minimum number of data sets used for fitting. In feature matching, it is the number of all possible matching point pairs. K represents the number of points randomly selected in each iteration.

[0147] Step 2: Fit the model to K points.

[0148] Step 3: Calculate the distance from other points to the fitting model and set the target threshold. If the distance is greater than the target threshold, it is considered an outlier and discarded; if the distance is less than the target threshold, it is considered an inlier and the number of inliers is counted.

[0149] Step 4: Based on the new inliers, proceed to step 2 to obtain a new fitting model, iterate M times, and select the model with the largest number of inliers as the optimal model. The number of iterations M is estimated in the following way:

[0150]

[0151] Among them, z is the probability that the model is correct, which can be set according to actual requirements and is usually above 0.99; p is the probability that the K selected points are all inliers (i.e., correct matching points).

[0152] Step 5: On this basis, select the model with the largest number of inliers for re-estimation and fit it using the least squares method.

[0153] S42, removing dynamic feature points.

[0154] Exemplarily, this process can refer to the relevant description of the S4 process in the above embodiment, which will not be repeated here.

[0155] S43, pose estimation, keyframe selection and pose optimization.

[0156] The implementation process of posture estimation can refer to the relevant description of the process S4 in the above embodiment, which will not be repeated here.

[0157] It should be understood that the ORBSLAM algorithm can more accurately determine the appropriate time to insert a key frame. Once it is determined that a key frame needs to be inserted, the algorithm will perform special processing on the ordinary frame, upgrade it to a key frame, and establish a connection with its adjacent key frames.

[0158] It should be understood that in the selection of key frames, it is necessary to screen according to the quality of the key frames themselves and the degree of correlation with other key frames; select unambiguous images that can extract a large number of feature points as key frames. In addition, key frames must be connected with other key frames, but the degree of correlation cannot be too high to cause information redundancy. The ORBSLAM algorithm takes most factors into consideration, and the present invention mainly refers to its key frame selection strategy to select key frames.

[0159] Furthermore, to ensure the accuracy of the SLAM system, the camera pose needs to be optimized.

[0160] Minimize the reprojection error, that is, use the camera model to project the 3D points in the world coordinate system onto the camera imaging plane, compare the obtained pixel coordinates with the observed pixel point coordinates, and obtain more accurate poses and landmarks by minimizing the difference.

[0161] The specific description is as follows:

[0162] Assume that there are j points P in real 3D space, whose coordinates are P m =[X m ,Y m ,Z m ] T , and the corresponding projection is u m , where m = 1, 2,…, j.

[0163] The camera's pose is composed of the rotation matrix R and the displacement vector t, that is, T = [R, t] T Based on this, the position relationship between the pixel point and the spatial point can be obtained. The specific relationship is expressed as follows:

[0164] s m u m =KTP m .

[0165] Among them, s m represents the scale factor associated with the projection of the mth measurement vector; K is the intrinsic parameter matrix of the camera.

[0166] Due to the unknown camera pose and the influence of noise at the observation point, there is a certain error in the above formula. Usually, there are a large number of feature points extracted from the image. In order to find a suitable result to minimize the error of the estimated camera pose, the least squares method is used to define the above optimization problem:

[0167]

[0168] Among them, T* represents the solution to the problem to be optimized, that is, the optimal transformation matrix that minimizes the objective function.

[0169] The present invention improves the BA (Bundle Adjustment) optimization process in the local mapping thread based on the ORBSLAM algorithm, as follows:

[0170] (1) The current key frame and the key frames with co-viewing relationship with it are added to the local key frame, and then the first-level connected key frames in the local key frame are traversed to add the map point to the local map point.

[0171] (2) Optimizing the secondary connected keyframes (i.e., "fixed keyframes") as constraint conditions. After the above steps, the pose of the newly inserted keyframe can be optimized with the pose of the co-viewing keyframe of the keyframe and the map point.

[0172] Local BA only optimizes the poses of local keyframes and the 3D coordinates of map points that can be observed in these local keyframes.

[0173] Specifically, all key points and map points in the global map are used as data to optimize the pose of the key frames and the coordinates of the 3D map points. The call time is generally after the camera is initialized and the closed loop is detected.

[0174] After the above BA optimization, the trajectory accuracy can be further improved.

[0175] S44, loop detection position recognition and geometric verification.

[0176] The goal of loop detection is to determine whether the robot passes through a previously visited location again. Once a loop is successfully detected, the system further optimizes the positioning trajectory, thereby improving the accuracy of map construction and trajectory estimation, and effectively reducing the accumulated errors in system operation.

[0177] Optionally, location recognition may represent image content by constructing a bag-of-words model of the image, and determine the similarity between images by calculating the similarity between the models.

[0178] The collected image information can be regarded as the text of the bag-of-words model, and the words in the text are the local information such as the features in the image. The bag-of-words model is mainly used to accelerate feature matching and closed-loop detection.

[0179] Matching feature points in an image is time-consuming and prone to mismatching, which not only affects the subsequent camera pose calculation, but also increases the calculation delay. The introduction of the bag-of-words model can effectively solve this problem. This method constructs an efficient feature point index structure so that only feature points under the same node need to be compared during feature matching, thereby improving the calculation speed. Since the feature points within the node have a high similarity, this method also greatly reduces the probability of mismatching.

[0180] The specific implementation method is to cluster the BRIEF descriptors when extracting ORB feature points in the image dataset into i words [n1, n2, ..., n i ], build K The image can be converted to:

[0181] A=1·n1+1·n2+0·n3+...+1·n i =[n1,n2,n3,...,n i ][1,1,0,...1] T .

[0182] In the above formula, 1 and 0 respectively represent whether the word exists. Since visual words are fixed values, any image can be represented by a bag-of-words vector; the bag-of-words vector is used to calculate the similarity between images.

[0183] For the bag-of-words model, the relationship between key frames and ordinary frames in an image is similar to the relationship between keywords and ordinary words in a text, and the amount of information in the image and text systems is different.

[0184] Use TF-IDF (Term Frequency-Inverse Document Frequency) to calculate word weights. The weights are divided according to the frequency of the image in the word bag. When the frequency of the word is high, the amount of information it contains is small, and the corresponding weight should be reduced; when the frequency of the image is low, the amount of information it contains is large, and the weight assigned should also be large. The formula is as follows:

[0185]

[0186] In the above formula, n i is the number of features of the image at node i; n is the total number of feature points; N is the total number of images; N i is the number of images containing feature point i; w i is the weight, the larger the value is, the more information it contains, and vice versa.

[0187] It should be understood that once an incorrect loop occurs, the trajectory estimated by SLAM will have a larger error, so further geometric verification is required after the above position recognition to determine whether a loop needs to be called. The geometric verification process is as follows:

[0188] (1) Traverse all potential loop candidate frames, and then match the feature points of the current frame with each candidate frame to determine the number of matching points and the corresponding map points.

[0189] (2) Use the RANSAC method to verify the match, thereby estimating the relative pose between frames and excluding those abnormal loop candidate frames that do not conform to the model. At this time, the pose will be used for reprojection matching to determine the number of matches.

[0190] (3) Determine whether to enter a loop based on the number of matches and the distance of the loop key frame.

[0191] S45, point cloud mapping.

[0192] It should be understood that the map constructed by visual SLAM based on feature point recognition and matching is a sparse point cloud map. In practical applications, sparse maps cannot well meet the interaction between robots and the external environment, while dense maps can provide more detailed external information.

[0193] Therefore, the present invention adds a dense point cloud mapping module to the algorithm framework and uses an RGBD camera that can actively measure depth information to build a dense point cloud map. Due to range limitations, when using it as a dense mapping sensor, it is usually used only indoors by default, and only sparse point cloud mapping is generally performed outdoors.

[0194] The dense mapping process is as follows:

[0195] The pixel points in the key frames are stitched into point clouds. At the same time, isolated point clouds and point clouds that exceed the range of the RGBD camera (i.e., "invalid depth information") are filtered and optimized. The optimized point clouds can be used to construct a dense point cloud map.

[0196] To verify the feasibility of the present invention, the positioning accuracy of the method provided by the present invention is quantitatively tested and analyzed using the indoor public dataset TUM and the outdoor public dataset KITTI. The proposed method is tested for indoor dynamic feature point removal and qualitatively analyzed for outdoor positioning accuracy using the measured dataset. Dense mapping of indoor environments is achieved in the RGBD camera mode.

[0197] Before testing, the camera is calibrated to obtain the intrinsic parameters.

[0198] The upper left corner of the pixel plane is taken as the coordinate origin, the positive direction of the horizontal coordinate axis (u) is parallel to the x-axis to the right, and the positive direction of the vertical coordinate axis (v) is parallel to the y-axis downward.

[0199] Assume the origin is translated by [c x ,c y ] T , where c x is the moving distance in the x-axis direction, c y is the moving distance in the y-axis direction. Since there is a scaling relationship between the pixel coordinate system and the imaging plane, it is assumed that the u-axis in the pixel coordinate system has an α-fold scaling relationship with the x-axis and a β-fold scaling relationship with the v-axis.

[0200] For a spatial point P in the real world, the coordinates are [X, Y, Z] T , whose P' coordinates are [X', Y', Z'] T , whose coordinates on the pixel plane are [u,v] T Among them, the spatial point P in the real world falls on the physical imaging plane O'-x'-y' after being projected through the pinhole, and P' is the imaging point.

[0201] The model of the monocular camera is as follows:

[0202]

[0203]

[0204] In the above formula, f is the focal length, and the meanings of other parameters are consistent with the assumptions.

[0205] The mathematical expression of the monocular camera model mapping is as follows:

[0206]

[0207] In the above formula, f x With f y is defined as follows:

[0208]

[0209] For the convenience of calculation and concise expression, the above formula is converted into matrix form as follows:

[0210]

[0211] In the above formula, the variables in the intermediate matrix are all fixed values. The parameters are mainly determined by the internal structure of the camera and will not change due to changes in usage scenarios or different installation locations. These parameter values ​​are fixed when the camera is produced, so the above matrix is ​​also called the camera's intrinsic parameter matrix K.

[0212] In order to solve the image blur problem caused by camera aperture size and light fluctuation, it is necessary to add a lens to the camera system to make the image clearer. However, due to problems in actual production and manufacturing, the lens will be distorted. There are two major distortion modes: radial distortion and tangential distortion.

[0213] The mathematical model of radial distortion can be corrected by using the first few terms of the Taylor series expansion around the principal point. The mathematical model for correction is as follows:

[0214]

[0215] In the above formula, (x, y) is the position of the pixel without distortion; (x dis ,y dis ) is the pixel position after radial distortion occurs; k1, k2 and k3 are radial distortion coefficients; r 2 =x 2 +y 2 , r is the distance between the pixel and the center of the image.

[0216] The mathematical model for correcting tangential distortion is as follows:

[0217]

[0218] In the above formula, (x dis ,y dis ) is the pixel position after tangential distortion occurs; p1 and p2 are tangential distortion coefficients; the meanings of other parameters are the same as those in the radial distortion mathematical model.

[0219] The camera model can be determined by its intrinsic parameter matrix and extrinsic parameter matrix. The extrinsic parameter is also called the camera pose, which changes with the movement of the camera. The camera extrinsic parameter is the target to be estimated in the visual SLAM problem.

[0220] The embodiment of the present invention calibrates the intrinsic parameters of the camera to obtain the intrinsic parameter matrix; before testing, the Zhang Zhengyou calibration method is used to calibrate the intrinsic parameters of the camera.

[0221] The present invention uses the toolbox of Matlab software to calibrate the camera intrinsic parameters, and the specific steps are as follows:

[0222] Step 1: Get a black and white checkerboard calibration board.

[0223] Step 2: Take several pictures of the calibration plate from different angles.

[0224] Step 3: Import the calibration plate image into Matlab software, use the toolkit to detect the corner points of the calibration plate, set the calibration plate size and select the distortion model.

[0225] Step 4: Perform calibration and the software calculates the camera intrinsic parameters.

[0226] Step 5: Evaluate the calibration results by looking at the reprojection error and calibration error. If the error is large, you need to retake the image or adjust the position of the calibration plate to find the camera intrinsic parameters under ideal distortion-free conditions.

[0227] Step 6: Save the camera calibration parameters.

[0228] Similarly, the intrinsic calibration method of the RGBD camera is similar to the above steps.

[0229] In order to compare the performance of the adaptive threshold feature point extraction method in the present invention with the original ORB feature point extraction method, 2774 pictures were collected under normal indoor brightness environment, and the brightness of the collected pictures was adjusted to 130% to simulate the strong light environment; at the same time, the picture brightness was reduced to 30% and 60% to simulate the insufficient light situation.

[0230] Since the original ORBSLAM feature point extraction threshold is a double threshold set to 20 and 7 respectively, the present invention sets the FAST threshold to its average value, i.e. 13, to quantify the number of feature points in the image. The average number of feature points under different light intensities is shown in Table 1, and the number of feature points extracted from the image under each brightness is shown in Table 1. Figure 5 shown.

[0231] Table 1 Average number of feature points under different light intensities

[0232]

[0233] In order to intuitively show the difference between wrong matches and correct matches, and to verify whether the adaptive threshold feature point extraction method will cause the corner point features to be insignificant due to the lowered threshold, which will in turn cause the problem of increased wrong matches. Two frames of images were selected from the collected data set and the data set after the light intensity was reduced by 40% for feature extraction and matching, and the correct matches and wrong matches were screened out using the RANSAC method. The results are shown in Table 2.

[0234] Table 2 Comparison of feature point matching numbers

[0235]

[0236] It can be seen from the above table that the feature point extraction method of the original method is greatly affected by light intensity. The feature point extraction method of the present invention can still extract more feature points in a weak light environment, and has a certain degree of insensitivity to light changes on the premise of extracting enough feature points.

[0237] In terms of indicator evaluation, the present invention evaluates the target detection performance of the improved YOLOv5 network through four aspects: detection accuracy, detection speed, model scale, and network computation. The evaluation indicators of detection accuracy include precision P, recall R, and average precision AP.

[0238] P represents the proportion of truly positive samples among the results predicted by the model as positive samples. The formula is as follows:

[0239]

[0240] R represents the proportion of all true positive samples that the model successfully predicts as positive samples. It reflects the model's ability to find all positive samples. The formula is as follows:

[0241]

[0242] In the above formula, TP stands for true positive, which refers to the number of dynamic points successfully predicted; FP stands for false positive, which refers to false alarms; FN stands for false negative, which refers to the number of dynamic points that have not been detected.

[0243] AP is the area enclosed by the PR curve and the coordinate axis. In target detection, it is often necessary to identify objects of multiple categories, so it is necessary to calculate the AP value of each category and take the average value to obtain the mean average precision (mAP).

[0244] Among them, mAP_0.5 represents the mAP value calculated when the intersection over union (IoU) threshold is 0.5.

[0245] The intersection over union (IoU) threshold is used to measure the degree of overlap between the detection box and the true annotation box. The higher the value, the higher the degree of overlap between the detection box and the true annotation box, which means the model prediction is more accurate. When IoU is greater than or equal to 0.5, it is considered to be a correct detection.

[0246] mAP_0.5:0.95 considers the range of IoU from 0.5 to 0.95 when calculating mAP, which is used to evaluate the target detection performance under different IoU thresholds corresponding to different scenarios.

[0247] Optionally, model complexity and computational requirements can be further measured by image processing frames per second (FPS) and model size.

[0248] Since the public data sets are equipped with real camera pose information, the error between the camera pose estimated by the SLAM system and the real camera pose can be quantitatively analyzed through indicators.

[0249] In the embodiment of the present invention, the absolute trajectory error ATE and the relative posture error RPE are used as evaluation indicators. ATE is used as the main trajectory accuracy evaluation indicator, and RPE is used as an auxiliary evaluation indicator to further analyze the changes in trajectory data.

[0250] ATE directly calculates the difference between the true value of the camera pose and the value estimated by the SLAM system.

[0251] First, the real value and the estimated value are aligned according to the pose timestamp, and then the difference is calculated. Finally, it is output in the form of a chart, which intuitively reflects the accuracy of the algorithm and the global consistency of the trajectory; RPE evaluates the system performance by calculating the difference in the pose change at the same two timestamps, which expresses the accuracy of the pose difference between two frames separated by a fixed time difference Δ. The relevant formula is as follows:

[0252] Formula symbol definition:

[0253] Estimated pose:

[0254] P1,…,Pn ∈SE(3).

[0255] True pose:

[0256] Q1,…,Q n ∈SE(3).

[0257] Subscript: represents time t.

[0258] Δ: indicates the interval time.

[0259] The transformation matrix S∈SE(3) from the estimated pose to the true pose is calculated by the least squares method.

[0260] The ATE of the i-th frame is expressed as follows:

[0261]

[0262] Using RMSE to calculate ATE, the formula is as follows:

[0263]

[0264] The RPE of the i-th frame is expressed as follows:

[0265]

[0266] The total number n and Δ are known, and m = n-Δ RPEs are obtained. The RMSE is used to calculate the error, and the formula is as follows:

[0267]

[0268] In the above formula, trans(E i ) represents the translation part of the relative posture error.

[0269] At the same time, the root mean square error RMSE and standard deviation Std of the pose estimation are calculated for error analysis to test the performance of the SLAM algorithm.

[0270] Among them, RMSE describes the deviation between the observed value and the true value. The RMSE value range is 0 to ∞. The smaller the value, the smaller the prediction error of the model and the stronger the prediction ability of the model. Std evaluates the degree of dispersion between the estimated trajectory and the true trajectory. The relevant formula is as follows:

[0271]

[0272] In the above formula, n is the number of samples, y i is the true value, is the predicted value.

[0273]

[0274] In the above formula, n is the number of samples, x i is the ith data value, is the average value of the group of data.

[0275] The present invention utilizes the EVO tool for trajectory evaluation.

[0276] The trajectories are aligned according to the timestamps, and the trajectories of different scales are stretched and aligned according to the specified standard trajectory, and the evaluation parameters such as the mean square error are calculated.

[0277] The feature point extraction method of the present invention is added to the ORBSLAM algorithm for performance testing to evaluate its impact on the overall algorithm.

[0278] The monocular system was tested using the TUM dataset, and the desk sequence in Fr2 was used for comparison. To simulate weak and strong light conditions, the desk sequence was segmented into 150 pictures, that is, the 1-150 sequence was 30% brightness, the 151-300 sequence was normal brightness, and so on. The new sequence after segmentation was used to compare the SLAM with adaptive threshold (expressed by AORB) and the SLAM with fixed threshold (expressed by ORB), and ATE was used as the evaluation indicator. The comparison of algorithm errors is shown in Table 3.

[0279] Table 3 Algorithm error comparison

[0280] algorithm RMSE Mean Median Std Min Max Sse AORB 0.010 0.009 0.009 0.003 0.001 0.017 0.013 ORB 0.012 0.010 0.009 0.005 0.001 0.035 0.020

[0281] The target detection performance of the improved YOLOv5 network is further tested as follows:

[0282] Data preprocessing: The target annotation information in the KITTI dataset is modified as follows: Car, Truck, etc. are merged into the Car class, Pedestrian and Person_sitting are uniformly classified as Pedestrian, Misc and Dontcare are ignored, and the rest of the target annotation information remains unchanged. Based on this, the annotation information of the KITTI dataset is reclassified into three categories: Car, Pedestrian, and Cyclist. The data is divided into training set, validation set, and test set in a ratio of 7:2:1, and 5236, 1496, and 748 actual road images are obtained respectively. Similarly, the BDD100K dataset is divided into training set, validation set, and test set in a ratio of 7:2:1, and 69863, about 20,000, and about 10,000 actual road images are obtained respectively.

[0283] An early stopping mechanism was added to monitor mAP and stop training before the model overfitted; the parameter was set to 50 rounds. In order to eliminate the impact of the dataset size on the model performance, the model was verified on the KITTI dataset, BDD100K dataset and actual roads.

[0284] The test results of each network detection result on KITTI and BDD100K datasets are shown in Table 4. The actual road test results are shown in Figure 6 As shown, Figure 6 Figure (a) shows the actual test results of the original YOLOv5 network on the road. Figure 6 Figure (b) shows the actual road test results of the improved YOLOv5 network of the present invention.

[0285] Table 4. Detection results of each network on KITTI and BDD100K datasets

[0286]

[0287] In the above table, 5s-Shu is the backbone network replaced by ShuffleNetv2; 5s-Shu-EMA is the network after adding the EMA attention mechanism, that is, the improved YOLOv5 network of the present invention; 5s-Shu-LSKA is the network after adding the LSKA attention mechanism; 5s-Shu-SEA is the network after adding the SEA attention mechanism.

[0288] The present invention adopts TUM data set testing in the SLAM indoor test of dynamic adaptive threshold.

[0289] The TUM dataset contains several types for different tasks, such as test, handheld SLAM, robot SLAM, dynamic objects and verification sets, each of which contains multiple data. The dataset subdivides the data into 39 sequences, covering a variety of application scenarios, which can be summarized into three categories: static scenes, low dynamic scenes and high dynamic scenes. It contains not only depth images and RGB images, but also real trajectory data.

[0290] In the dynamic adaptive threshold SLAM indoor test of the present invention, some sequences under the dynamic object category of the above data set are selected for indoor testing, and 4 sequences are selected from low dynamic scenes and high dynamic scenes for SLAM testing; the data set types include four camera motion postures (xyz, rpy, halfsphere and static) and two dynamic levels (sitting and walking).

[0291] The original algorithm ORBSLAM3, ORBSLAM3 after adding COCO dataset training YSE, and ORBSLAM3 after adding adaptive threshold feature point extraction method and COCO dataset training YSE are tested. The present invention refers to them as ORB, YSE_ORB and AYSE_ORB respectively. The test results of Sitting_halfsphere sequence are shown in Figure 2. Figure 7 As shown, Figure 7 Figure (a) is the XYZ axis view. Figure 7 Figure (b) is the Euler angle view.

[0292] Figure 7 The XYZ axis views in (a) represent the changes in the X, Y, and Z directions respectively.

[0293] For low-dynamic scenes, the Sitting_xyz, Sitting_rpy, Sitting_halfsphere, and Sitting_static sequences in the TUM dataset Fr3 are selected for comparison, and the trajectories are visualized to intuitively compare the performance of the above algorithms, such as Figure 8 As shown, Figure 8 Figure (a) is a schematic diagram of the trajectory corresponding to the Sitting_halfsphere sequence; Figure 8 Figure (b) is a schematic diagram of the trajectory corresponding to the Sitting_rpy sequence; Figure 8 Figure (c) is a schematic diagram of the trajectory corresponding to the Sitting_static sequence; Figure 8 Figure (d) is a schematic diagram of the trajectory corresponding to the Sitting_xyz sequence.

[0294] It can be seen that although there are dynamic objects in the environment, the movement amplitude is relatively small, and the dynamic feature points account for a small proportion of the total number. In low-dynamic scenes, the gap between the three algorithms is not large.

[0295] For high-dynamic scenes, the Walking_xyz, Walking_rpy, Walking_static, and Walking_halfsphere sequences in the TUM dataset Fr3 are selected for comparison, and the existing algorithms DS-SLAM and DRM-SLAM are added to better evaluate the algorithm of the present invention. The trajectories are visualized to intuitively compare the performance of the above algorithms, such as Fig. 9 As shown, Fig. 9 Figure (a) is a schematic diagram of the trajectory corresponding to the Walking_xyz sequence; Fig. 9 Figure (b) is a schematic diagram of the trajectory corresponding to the Walking_rpy sequence; Fig. 9 Figure (c) is a schematic diagram of the trajectory corresponding to the Walking_static sequence; Fig. 9 Figure (d) is a schematic diagram of the trajectory corresponding to the Walking_halfsphere sequence.

[0296] By analyzing the trajectory data of multiple groups of sequences in different indoor scenes, it is found that in low-dynamic scenes, the positioning accuracy of the algorithm of the present invention is not much different from that of the original algorithm; however, in high-dynamic scenes, the accuracy of the algorithm of the present invention is greatly improved, and the effect is better than the DS-SLAM algorithm under large-scale translation and rotation.

[0297] Since general RGBD cameras have limited detection distance and are easily affected by other light, there are few outdoor public datasets using this device.

[0298] To address this problem, the present invention adds a binocular camera mode based on RGBD, uses the BDD100K dataset to train the model, and uses the KITTI dataset for outdoor testing.

[0299] According to the complexity of the environment, the number of dynamic objects, and whether there are loops on the path, four sequences, 02, 07, 09, and 10, were selected from the experimental sections of the 00-10 sequences of the KITTI dataset for testing. The feature point extraction results of the original ORBSLAM3 algorithm and the algorithm of the present invention are shown in the figure. Fig.10 shown.

[0300] Fig.10 Figure (a) shows the original ORB feature point extraction result; Fig.10 Figure (b) shows the feature point extraction result of the algorithm of the present invention.

[0301] Fig.10 In figure (b), pedestrians and cars are marked with circle areas.

[0302] It can be seen that compared with the ORBSLAM3 algorithm, the feature extraction algorithm of the present invention can correctly and efficiently identify objects and remove their dynamic feature points. Under the same environment, the adaptive threshold feature point extraction can also obtain more feature points than the ORB feature point extraction.

[0303] The EVO tool was used for trajectory evaluation, and RMSE and Std were used for performance evaluation. The error analysis is shown in Table 5.

[0304] Table 5 Algorithm calculation error based on KITTI dataset sequence data

[0305]

[0306] It can be seen that the present invention can not only remove the influence of dynamic objects, but also retain enough feature points for matching due to the large number of feature points extracted itself, despite the removal of feature points of roadside vehicles.

[0307] Take the 07 and 09 sequences in the KITTI dataset to generate trajectory data and compare them with the real trajectory contained in the sequence. The results are as follows Fig.11 Although the RMSE of the three algorithms after quantification are different to varying degrees, the generated trajectories can almost fit the real trajectories well.

[0308] In order to test the performance of the method provided by the present invention on the actual road, the depth camera device is installed on a vehicle that can be legally driven on the road for experimental testing. The feature extraction results are as follows: Fig.12 As shown, Fig.12 Figure (a) shows the feature point extraction result of the original algorithm on the measured road. Fig.12 Figure (b) shows the feature point extraction result of the algorithm of the present invention on the measured road.

[0309] contrast Fig.12 Figure (a) and Fig.12 As can be seen from Figure (b), dynamic objects such as vehicles and pedestrians can be well identified and their surface feature points can be removed to avoid the influence of dynamic feature points on pose estimation.

[0310] The point cloud map is as follows:

[0311] In the algorithm framework of the present invention, a point cloud mapping module is added, and some sequences are selected from the TUM dataset and mapped in a real indoor environment. The mapping effect is as follows: Fig.13 As shown, Fig.13 Figure (a) shows a dense point cloud map constructed from some sequences of the TUM dataset. Fig.13 Figure (b) shows a dense point cloud map constructed in a real indoor environment.

[0312] In summary, in the high-dynamic sequence of the TUM data set, the positioning accuracy of the algorithm of the present invention is greatly improved compared with the original algorithm; although there are some ghosting in the high-dynamic environment sequence, the point cloud of dynamic objects can be removed well; in the low-dynamic scene, the positioning accuracy is comparable to that of the original algorithm.

[0313] The algorithm of the present invention improves the robustness of dense mapping of visual SLAM in indoor dynamic environments.

[0314] Based on the same inventive concept, the embodiment of the present invention also provides a synchronous positioning and map building device in a dynamic environment, such as Fig.14 shown.

[0315] Exemplarily, the device comprises:

[0316] The acquisition module 14 - 1 is used to acquire the image to be detected captured by the camera, where the image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing lighting.

[0317] The detection module 14-2 is used to perform target detection on the image to be detected, obtain the detection frame and category of the target object in the image to be detected; judge the state of the target object according to the correspondence between the object category and the state in the preset rules; the state includes the motion state and the static state.

[0318] The feature extraction module 14-3 is used to divide each pixel in the image to be detected into a neighborhood window of preset size, calculate the local threshold of each pixel point according to the pixel grayscale value in the neighborhood window, and extract the feature points of the image to be detected based on the local threshold.

[0319] The pose estimation module 14 - 4 is used to traverse the extracted feature points, remove the feature points in the detection frame where the target object in motion is located, obtain the target feature points, and perform camera pose estimation based on the target feature points to obtain camera pose data.

[0320] The map construction module 14-5 is used to construct a map based on the camera posture data.

[0321] Each module in the above-mentioned synchronous positioning and map building device in a dynamic environment can be implemented in whole or in part by software, hardware, or a combination thereof. Each of the above-mentioned modules can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute the operations corresponding to each of the above modules.

[0322] The present invention also provides a computer device, including a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps in the embodiment of the method for synchronous positioning and map construction in a dynamic environment. The specific implementation method can be found in the method embodiment, which will not be repeated here.

[0323] Furthermore, the present invention also provides a non-temporary computer-readable storage medium containing instructions, and a computer program is stored on the storage medium. For example, a memory containing instructions, the above instructions can be executed by a processor of a computer device to complete the above method. For example, a non-temporary computer-readable storage medium can be a ROM, a random access memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, and an optical data storage device. When the computer program is executed by the processor, the steps in the embodiment of the method for synchronous positioning and mapping in a dynamic environment can be implemented. The specific implementation method can be found in the method embodiment, which will not be repeated here.

[0324] It will be appreciated by those skilled in the art that embodiments of the present invention may provide methods, systems or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0325] The present invention is described with reference to flowcharts and / or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as a combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0326] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0327] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0328] It should be pointed out that the specific implementation methods described above can enable those skilled in the art to understand the invention more comprehensively, but do not limit the invention in any way. Therefore, although the invention has been described in detail in this specification and embodiments, those skilled in the art should understand that the invention can still be modified or replaced by equivalents; and all technical solutions and improvements that do not deviate from the spirit and scope of the invention are included in the protection scope of the patent for the invention. Any figure mark in the claims should not be regarded as limiting the claims involved. Any simple change or equivalent replacement of the technical solution that can be obviously obtained by any technician familiar with the field within the technical scope disclosed in the present invention belongs to the protection scope of the present invention.

Claims

1. A method for simultaneous positioning and mapping in a dynamic environment, characterized in that: include: Acquire an image to be detected captured by a camera, wherein the image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing light; Perform target detection on the image to be detected to obtain the detection frame and category of the target object in the image to be detected; determine the state of the target object according to the correspondence between the object category and the state in the preset rules; the state includes the moving state and the static state; Divide each pixel in the image to be detected into a neighborhood window of a preset size, calculate the local threshold of each pixel point according to the grayscale value of the pixel in the neighborhood window, and extract the feature points of the image to be detected based on the local threshold; Traverse the extracted feature points, remove the feature points in the detection frame where the target object in motion is located, obtain the target feature points, estimate the camera pose based on the target feature points, and obtain the camera pose data; Build a map based on camera pose data.

2. The method for simultaneous positioning and mapping in a dynamic environment according to claim 1, characterized in that: Performing target detection on the image to be detected by an improved YOLOv5 network to obtain a detection frame and a category of the target object in the image to be detected, wherein the improved YOLOv5 network includes an improved backbone network, a neck network and a head network, and an improved backbone network is formed by integrating multiple ShuffleNetv2 units after the maximum pooling layer of the original backbone network and adding an EMA attention mechanism between the last ShuffleNetv2 unit and the SPPF module; The improved YOLOv5 network is used to perform target detection on the image to be detected, and the detection frame and category of the target object in the image to be detected are obtained, including: The image to be detected is input into the improved backbone network, and the features of the image to be detected are extracted through the CBRM convolutional layer sequence; The features extracted by the CBRM convolutional layer sequence are processed by convolution operations through multiple ShuffleNetv2 units to generate multi-scale feature maps, and the multi-scale feature maps are weighted by the EMA attention mechanism to obtain enhanced feature maps; The enhanced feature map is input into the SPPF module for feature fusion, and the fused features are predicted through the head network to output the detection box and category of the target object in the image to be detected.

3. The method for synchronous positioning and mapping in a dynamic environment according to claim 1 or 2, characterized in that: The local threshold of each pixel is calculated according to the pixel gray value in the neighborhood window through the improved Sauvola algorithm, including: For each pixel in the image to be detected, calculate the mean grayscale value of all pixels in its neighborhood window. The formula is: Among them, m(x,y) represents the mean gray value of the pixel points in the neighborhood window of the pixel point with coordinates (x,y), r represents the neighborhood radius, and g(i,j) represents the gray value of the pixel point with coordinates (i,j) in the neighborhood window; The variance is calculated based on the mean, and the formula is: Among them, s(x,y) represents the standard deviation of the grayscale values ​​of the pixels in the neighborhood window of the pixel with coordinates (x,y); The local threshold is calculated based on the standard deviation, and the formula is: Where T represents the local threshold and R is a fixed parameter representing the dynamic range of the standard deviation.

4. The method for synchronous positioning and mapping in a dynamic environment according to claim 1 or 2, characterized in that: Extract feature points of the image to be detected based on local thresholds, including: Calculate the grayscale value difference between the current pixel and all pixels in its neighborhood window, compare the grayscale value difference with the local threshold, and if the grayscale value difference between a preset number of pixels and the current pixel is greater than the local threshold, determine the current pixel as a feature point; or, According to the preset screening rules, some pixels are selected in the neighborhood window of the current pixel, the gray value difference between some pixels and the current pixel is calculated, the gray value difference is compared with the local threshold, and feature points are extracted.

5. The method for synchronous positioning and mapping in a dynamic environment according to claim 1 or 2, characterized in that: After obtaining the target feature points, it also includes: Divide the image to be detected into four regions of equal size as the root nodes of the quadtree; Count the number of feature points in each area. If the number of feature points in a target area exceeds the preset threshold, continue to divide the target area into four sub-areas; The segmentation step is repeated until the number of feature points in all regions does not exceed a preset threshold or reaches a preset maximum depth.

6. A synchronous positioning and map building device in a dynamic environment, characterized in that: include: An acquisition module is used to acquire an image to be detected captured by a camera, wherein the image to be detected is an image to be detected in a dynamic environment, and the dynamic environment includes moving objects and changing light; The detection module is used to perform target detection on the image to be detected, obtain the detection frame and category of the target object in the image to be detected; determine the state of the target object according to the correspondence between the object category and the state in the preset rules; the state includes the moving state and the static state; A feature extraction module is used to divide each pixel in the image to be detected into a neighborhood window of a preset size, calculate the local threshold of each pixel point according to the gray value of the pixel in the neighborhood window, and extract feature points of the image to be detected based on the local threshold; The pose estimation module is used to traverse the extracted feature points, remove the feature points in the detection frame where the target object in motion is located, obtain the target feature points, and perform camera pose estimation based on the target feature points to obtain camera pose data; Map construction module, used to build maps based on camera pose data.

7. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the method for synchronous positioning and mapping in a dynamic environment according to any one of claims 1 to 5 is implemented.

8. A computer device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the method for synchronous positioning and map building in a dynamic environment as described in any one of claims 1 to 5 is implemented.

Images