SLAM method based on instance segmentation and dense mapping in dynamic environment

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) method based on instance segmentation and dense mapping in a dynamic environment. The SLAM method comprises the following steps: S1, respectively transmitting input data to a YOLOv5-seg module and an ORB feature extraction module in ORB-SLAM2; s2, acquiring image feature information by using an ORB feature extraction module, performing target detection and instance segmentation by using a YOLOv5-seg module, and transmitting a segmentation result to an ORB-SLAM2; s3, after the receiving end of the ORB-SLAM2 obtains the segmentation result, mapping the segmentation result to the ORB-SLAM2 to carry out dynamic feature point elimination, and carrying out tracking based on the data after the dynamic feature point elimination; s4, local mapping and closed-loop detection correction are carried out in the ORB-SLAM2; and S5, adding a dense mapping module on the basis of the ORB-SLAM2, and providing a point cloud filtering scheme and a global optimization scheme of the dynamic object. According to the method, the instance segmentation algorithm and the SLAM are effectively combined, dense mapping is carried out on static objects on the basis of removing dynamic objects, and a more detailed and accurate environment map can be provided.

Description

Technical Field

[0001] The present invention relates to the field of positioning and map construction, and in particular to a SLAM method based on instance segmentation and dense mapping in a dynamic environment. Background Art

[0002] In the current technological context, simultaneous localization and mapping (SLAM) technology plays a vital role in automation, robot navigation, driverless cars and other fields. Especially in dynamic environments, how to achieve efficient and accurate positioning and mapping has become a key challenge for research and application. The SLAM problem in a dynamic environment is more complicated. In addition to dealing with the mapping and positioning problems in a static environment, it is also necessary to identify and track dynamic objects in the environment in real time, and correctly process these dynamic information to ensure the accuracy and robustness of the SLAM system. Fast-moving dynamic objects will seriously interfere with the positioning and mapping process of the SLAM system through feature point matching errors, parallax errors, static environment occlusion, and irregular noise. However, traditional SLAM solutions are mainly designed for static environments and have limited processing capabilities for dynamic objects, which greatly limits their application scope and effect in complex dynamic environments. Traditional SLAM usually relies on algorithms such as RANSAC to eliminate some dynamic feature points, but this method has limited effect when the range of dynamic objects is large, and it is easy to cause positioning drift and map distortion. In recent years, some studies have introduced deep learning segmentation algorithms. For example, DynaSLAM uses Mask R-CNN for dynamic object segmentation. Although it improves the detection capability of dynamic objects, its inference speed is slow and it is difficult to meet real-time requirements on resource-constrained devices.

[0003] Some SLAM methods only have sparse three-dimensional point cloud mapping, and it is impossible to visually observe environmental information with the naked eye. The dense mapping capabilities of traditional SLAM methods are also relatively weak. In dynamic environments, it is extremely easy to have residual ghosting and noise, resulting in poor map readability and practicality. To address the problem of dense mapping in dynamic environments, DDL-SLAM uses semantic segmentation technology to exclude dynamic objects, but the resulting map still has dynamic targets remaining. Yao Erliang's team tried their best to reduce the impact of dynamic targets through the YOLOv3 algorithm and a series of optimization steps, but the readability of the resulting semantic map still has room for improvement. At present, SLAM technology still has obvious shortcomings in positioning accuracy, real-time performance and dense mapping quality in dynamic environments. Summary of the invention

[0004] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a SLAM method based on instance segmentation and dense mapping in a dynamic environment, which effectively combines the instance segmentation algorithm with SLAM, and densely maps static objects on the basis of removing dynamic objects, so as to provide a more detailed and accurate environmental map.

[0005] The object of the present invention is to achieve the following technical solution: a SLAM method based on instance segmentation and dense mapping in a dynamic environment, comprising the following steps: S1. Transmit the input data to the YOLOv5-seg module and the ORB feature extraction module in ORB-SLAM2 respectively; The input data is: for the target area (the target area is generally an indoor scene, such as an indoor factory area or an office area), the depth camera is used to collect RGBD image data. The data is passed to the ORB feature extraction module to obtain image feature information, and is also passed to the YOLOv5-seg module for target detection and instance segmentation.

[0006] S2. Use the ORB feature extraction module to obtain image feature information, and use the YOLOv5-seg module to perform target detection and instance segmentation, and then transmit the segmentation results to ORB-SLAM2; The YOLOv5-seg module includes a YOLOv5-seg network and a YOLOv5-seg transmitter; Target detection: Use the YOLOv5-seg network to perform target detection on the input image and identify dynamic objects in the image.

[0007] Instance segmentation: The YOLOv5-seg network can also perform instance segmentation, segment dynamic objects at the pixel level, and generate target detection boxes, categories, and segmentation point sets.

[0008] Since the YOLOv5-seg network is written in python code, and ORB-SLAM2 is written in C++ code, UNIX Domain Socket is used as the inter-process communication (IPC) mechanism, which is faster than deploying YOLOv5-seg on C++. Add an ORB-SLAM2 receiver in ORB-SLAM2; The YOLOv5-Seg sender sends the segmentation results (target detection box, category, segmentation point set) to the ORB-SLAM2 receiver in real time through the UNIX Domain Socket, and then the ORB-SLAM2 receiver maps it to ORB-SLAM2. The specific sending and receiving process is as follows: Sender (YOLOv5-Seg sender): The YOLOv5-Seg sender innovatively uses Unix domain sockets to achieve efficient and low-latency data transmission, and sends the dynamic object detection results to the ORB-SLAM2 receiver in real time. First, create Unix domain sockets (socket.AF_UNIX and socket.SOCK_STREAM), and wait for the ORB-SLAM2 receiver to connect through the listening mechanism (server.listen). After detecting a dynamic object, the YOLOv5-Seg sender efficiently encodes information such as the target detection box, category, and segmentation point set mask into a compact string format (for example: "left:26 top:227..."), and uses Socket communication to quickly transmit the detection results. The combination of this lightweight data format and real-time transmission mechanism significantly improves the interaction efficiency between the two ends, laying the foundation for dynamic object processing in real-time SLAM systems.

[0009] Receiver (ORB-SLAM2 receiver): The ORB-SLAM2 receiver seamlessly integrates the detection results of the YOLOv5-Seg transmitter into the dynamic object culling process of ORB-SLAM2 through a custom parsing and integration mechanism. The receiver uses efficient Socket communication to read the detection results and quickly extracts the target detection box, category, and segmentation point set through customized parsing logic (such as the LoadBoundingBoxFromPython function). The parsing results are encapsulated as structured objects containing geometric features and segmentation information (such as the detect_result vector), which are directly mapped to the ORB-SLAM2 data structure for constructing Frame objects and dynamically updating SLAM maps.

[0010] S3. After the ORB-SLAM2 receiver obtains the segmentation result, it is mapped to ORB-SLAM2 to remove dynamic feature points, and tracking is performed based on the data after dynamic feature point removal; After the ORB-SLAM2 end obtains the target detection frame, category, and segmentation point set information, it removes dynamic feature points. First, the dynamic object area is divided into precise polygons according to the segmentation point set. Then, in order to improve the detection speed, the algorithm is written to filter the feature points in the target detection frame at the same time. Only when the feature point is in the target detection frame, the cv2.pointPolygonTest() function is used to determine whether the feature point is in the polygonal area. If it is judged to be a dynamic feature point, it will be marked and removed during the distortion correction process according to the marking information, thereby ensuring that the system only uses feature points in the static area.

[0011] In step S3, tracking based on the data after dynamic feature points are eliminated includes: Map initialization: ORB-SLAM2 initializes the map based on the data after removing dynamic features, passes in continuous frames, and generates key frames from the two frames with the largest initial parallax; Constant speed model tracking: Visual tracking of consecutive frames in ORB-SLAM2 to maintain tracking stability; Reference keyframe tracking: Use keyframe information as a reference in ORB-SLAM2 to avoid lost tracking; Relocalization: In ORB-SLAM2, when tracking fails or becomes unstable, relocalization is achieved by referencing keyframes; Local map tracking: In ORB-SLAM2, local maps are referenced during tracking to improve positioning accuracy; Create a new keyframe: In ORB-SLAM2, keyframes are continuously selected from consecutive frames and retained in the keyframe sequence in chronological order, waiting to be called in subsequent steps.

[0012] S4. Perform local mapping and closed-loop detection correction in ORB-SLAM2; In step S4, performing local mapping in ORB-SLAM2 includes: Insert keyframe: Get the keyframe sequence and insert the new keyframe in the sequence into the map; Eliminate map points: clean up redundant or large-error map points; Generate map points: Generate new static map points based on inserted keyframes; Local BA optimization: perform Bundle Adjustment optimization on the local map, optimize the position of the key frames, and evaluate the redundancy of the key frames; Eliminate redundant keyframes: Remove keyframes in the sequence that contribute less to the overall map, and save the remaining optimized keyframes to the sequence in chronological order for use in the closed-loop step.

[0013] In step S4, performing closed-loop detection correction in ORB-SLAM2 includes: Query the database: input the current key frame, compare it with the key frame sequence in the database, and generate a closed loop when the same scene is detected, and determine the historical key frame corresponding to the current key frame; Calculate Sim3: After the closed loop is generated, calculate the similarity transformation Sim3 based on the trajectory between the current key frame and the historical key frame; Closed-loop fusion: Optimize and fuse the map based on the calculated transformation results, eliminate the accumulated trajectory drift, and correct the map; Essential map optimization: Further optimize the entire map structure according to the constraints of map optimization to ensure consistency.

[0014] Global BA and map update: Jointly optimize all keyframes and all map points to minimize the projection error of the entire system, optimize the keyframe poses and update the map point positions based on the calculation results.

[0015] S5. Add a dense mapping module based on ORB-SLAM2, and provide a point cloud filtering solution for dynamic objects and a global optimization solution.

[0016] (1) Get the depth map and color map of the current key frame Through the passed key frame (KeyFrame* kf), the corresponding color map (cv::Mat&color) and depth map (cv::Mat&depth) are used as the basic data required to prepare the point cloud.

[0017] (2) Traverse each pixel in the depth map By traversing the pixels of the depth map in a double loop and sampling with a step size of 3 pixels, the amount of calculation and the density of the final point cloud are reduced.

[0018] (3) Depth value filtering For each pixel, we first check whether its depth value is within the valid range, which is set to be greater than 0.01 meters and less than 30 meters in this article. This is basic depth filtering to exclude invalid or too far points.

[0019] (4) Point cloud filtering of dynamic objects ① For each valid depth point, determine whether the point belongs to a dynamic object through the dynamic object information recorded in the keyframe object.

[0020] ② Convert the pixel coordinates of the current point into cv::Point2f format, and then traverse each dynamic object detected in the current key frame.

[0021] ③ For each dynamic object, check whether the current point is in the extended area of ​​the object detection frame, and expand it by 50 pixels on the left, right, top and bottom; if so, use the cv::pointPolygonTest function to check whether the point is in the contour or on the edge of the dynamic object, and set the tolerance range to -50 pixels, that is, the dynamic contour is expanded, and the expanded area is used as the area of ​​the dynamic object.

[0022] ④ If the current point is within the area of ​​a dynamic object, it is marked as a point within the dynamic area and skipped without being added to the final point cloud.

[0023] (5) Point cloud construction For the points that pass the above filtering conditions, a point cloud is constructed based on its depth value and color image data. Here, the world coordinate position of each point is calculated and the corresponding RGB color value is assigned to the point in the point cloud.

[0024] (6) Point cloud stitching Each frame of point cloud generated needs to be stitched together to generate a more complete 3D map. Point cloud stitching aligns point cloud data from different frames to the same global coordinate system through pose transformation, thereby constructing a cumulative point cloud map.

[0025] (7) Point cloud filtering Use the statistical filter statistical_filter to process the global point cloud globalMap and remove noise points. The statistical filter calculates the average and standard deviation of the distance between each point and its neighboring points, and then removes those points that are too far from the average according to the set threshold. Then, use the voxel filter voxel to downsample the filtered point cloud tmp1 to reduce the density of the point cloud.

[0026] (8) Global point cloud update In the ORB-SLAM2 system, after global BA optimization, the pose of the keyframe is optimized, and the global point cloud needs to be updated and optimized; global BA corrects the camera pose drift problem caused by the accumulated error in the keyframe, and the optimized camera pose will also directly affect the position of the previously constructed three-dimensional map points. Therefore, the optimized keyframe pose is used to perform corresponding coordinate transformation and update on the point cloud data of the entire map to ensure the consistency of the map; in addition, independent point cloud data is repeated or unevenly distributed, and the global point cloud optimization solves this problem at the same time. During the optimization, the voxel filtering method is used to optimize the point cloud to improve the uniformity of the point cloud. The specific steps for updating the global point cloud are as follows: ① Check the point cloud update status and initialize Check the cloudbusy flag to ensure that no other process is updating the point cloud to avoid data conflicts. Set loopbusy = true to start the loop update process of the point cloud and output a prompt message. Initialize a temporary point cloud tmp1 to store all the transformed keyframe point cloud data.

[0027] ②Traverse the current keyframe and point cloud Double loop traverses the currently valid keyframe currentvpKFs and the associated point cloud data pointcloud. For each keyframe and point cloud pair, if the point cloud ID (pcID) matches the keyframe frame ID (mnFrameId), it means that this point cloud belongs to the keyframe.

[0028] ③Point cloud transformation GetPose() is used to obtain the pose of the key frame and converted to the Euclidean transformation matrix T through ORB_SLAM2::Converter::toSE3Quat. Then, use the pcl::transformPointCloud function to transform each matched point cloud into the global coordinate system according to the inverse pose of the key frame and accumulate it in tmp1.

[0029] ④Voxel filtering and point cloud update Create another temporary point cloud tmp2, use the voxel filter voxel to downsample tmp1, reduce the density of the point cloud, and improve storage and processing efficiency. Then, exchange the filtered point cloud tmp2 with the global map globalMap to update the point cloud data of the global map. After the update, set the loopbusy flag to false, indicating that the loop update process of the point cloud is over.

[0030] The beneficial effects of the present invention are as follows: 1. The present invention realizes the real-time integration of instance segmentation and SLAM system: by introducing the YOLOv5-Seg instance segmentation algorithm, the dynamic objects in the image are detected and segmented in real time, and the segmentation results are seamlessly integrated with the ORB-SLAM2 system in combination with Unix domain Socket communication. This design not only ensures the high efficiency of the algorithm, but also reduces the coupling degree of the system, which is convenient for expansion and maintenance.

[0031] 2. The present invention realizes the precise elimination of dynamic feature points: by utilizing the dynamic object boundary and mask information generated by instance segmentation, feature points in the dynamic area are eliminated through pixel-level analysis in the ORB-SLAM2 tracking thread, thereby eliminating the interference of dynamic objects on positioning and mapping. This dynamic feature point elimination method based on pixel accuracy significantly improves the positioning accuracy of the SLAM system in a dynamic environment.

[0032] 3. The present invention adopts a dense mapping method based on PCL. On the basis of traditional sparse point cloud mapping, it expands the dense mapping module and uses the PCL point cloud library to generate a dense point cloud map. During the dense mapping process, the point cloud data corresponding to dynamic objects is filtered by combining the color image and depth map information provided by the RGB-D camera, thereby ensuring that only high-quality point clouds of static objects are retained in the map.

[0033] 4. The present invention performs global point cloud optimization and dynamic interference elimination: optimizes the key frame pose through global BA, and optimizes the global point cloud using PCL statistical filtering and voxel filtering methods to remove noise points and unevenly distributed areas. Combined with the dynamic object segmentation results, the global point cloud optimization effectively eliminates the residual image and ghosting interference caused by dynamic objects, and improves the readability of dense maps.

[0034] 5. The present invention realizes real-time identification and processing of dynamic and static feature points: the concept of dynamically expanding the mask boundary (i.e., the boundary formed by the segmentation point set, which is a polygon) in the point cloud generation stage is proposed, and the dynamic point cloud is completely eliminated by expanding the contour range of the dynamic object. Combining the dynamic detection of feature points and the static optimization of point clouds, the present invention realizes efficient identification and processing of dynamic objects.

[0035] 6. The present invention takes into account both real-time performance and robustness: it fully considers the performance of resource-constrained devices in practical applications. By selecting a lightweight YOLOv5-Seg model and an optimized Socket communication solution, real-time operation can be guaranteed only on the CPU, while improving the robustness of the system in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 is a flow chart of the method of the present invention; Figure 2 It is a detailed schematic diagram of the structure of the present invention; Figure 3 It is a flow chart for dynamic feature point removal; Figure 4 To remove the effect of dynamic feature points; Figure 5 Schematic diagram of feature points detected by ORB-SLAM2 on the TUM dataset; Figure 6 This is a schematic diagram of the detection results of ORB-SLAM2 combined with YOLOv5-Seg on the TUM dataset; Figure 7 This is a trajectory comparison chart of ORB-SLAM2; Figure 8 This is a trajectory comparison diagram of this patent; Fig. 9 Dense mapping for ORB-SLAM2 extension; Fig.10 Schematic diagram of the dense mapping effect of this patent. DETAILED DESCRIPTION

[0037] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings, but the protection scope of the present invention is not limited to the following.

[0038] like Figure 1~2 As shown, a SLAM method based on instance segmentation and dense mapping in a dynamic environment includes the following steps: S1. Transmit the input data to the YOLOv5-seg module and the ORB feature extraction module in ORB-SLAM2 respectively; The input data is: for the target area, the depth camera is used to collect RGBD image data; in the embodiment of the present application, the RGBD data of the TUM data set is directly used. The data is not only passed to the ORB feature extraction module to obtain image feature information, but also passed to the YOLOv5-seg module for target detection and instance segmentation.

[0039] S2. Use the ORB feature extraction module to obtain image feature information, and use the YOLOv5-seg module to perform target detection and instance segmentation, and then transmit the segmentation results to ORB-SLAM2; The YOLOv5-seg module includes a YOLOv5-seg network and a YOLOv5-seg transmitter; Target detection: Use the YOLOv5-seg network to perform target detection on the input image and identify dynamic objects in the image.

[0040] Instance segmentation: The YOLOv5-seg network can also perform instance segmentation, segment dynamic objects at the pixel level, and generate target detection boxes, categories, and segmentation point sets.

[0041] Data transmission between YOLOv5-seg module and ORB-SLAM2: Since YOLOv5-seg is written in python code and ORB-SLAM2 is written in C++ code, UNIX Domain Socket is used as the inter-process communication (IPC) mechanism. This method is faster than deploying YOLOv5-seg on C++. The YOLOv5-Seg sender maps the segmentation results (target detection box, category, segmentation point set) to the ORB-SLAM2 receiver in real time through the UNIX Domain Socket. The specific sending and receiving process is as follows: Sender (YOLOv5-Seg sender): The YOLOv5Seg sender sends dynamic object detection results to the ORB-SLAM2 receiver through Socket communication. The sending process of the YOLOv5Seg sender is as follows: (1) Create a Unix domain socket Create a Unix domain socket using socket.AF_UNIX and socket.SOCK_STREAM. Store the socket file in the / home / kk2 / server_socket path.

[0042] (2) Listen and receive client connections In the run function, server.bind(serverAddr) is used to bind the socket address, and server.listen(5) is used to start listening for client connection requests. When a client (ORB-SLAM2 receiving end) connects, server.accept() is used to accept the connection and obtain the connection object conn and the client address addr.

[0043] (3) Constructing dynamic object detection result data After detecting a dynamic object, a string containing the coordinates of the target detection box, category, and segmentation point set mask information is constructed by concatenating strings. An example of the format of a single dynamic object string is: "left:26 top:227right:800 bottom:750 class:bus0.71seg:1,0,2,0,2,1,3,2,4,3,3,3,1,2,0,1*".

[0044] (4) Send data to the ORB-SLAM2 receiver After constructing the dynamic object detection result data, use conn.send(result_send.encode()) to send the string to the ORB-SLAM2 receiver.

[0045] Receiver (ORB-SLAM2 receiver): (1) Establishing a Socket Connection In the main function, a Unix domain socket is used to connect to the preset address / home / kk2 / server_socket to receive data from the YOLOv5Seg sender.

[0046] (2) Receiving data Before processing each frame of the image, call the MakeDetect_result function to read the string data sent by the YOLOv5Seg sender through the Socket.

[0047] (3) Analyze data Inside the MakeDetect_result function, the read function is used to receive data, and the information of each detected object is split using the * as the delimiter through the strtok function. For each split string, the LoadBoundingBoxFromPython function is called for further parsing.

[0048] (4) Extracting object information The LoadBoundingBoxFromPython function parses the target detection box coordinates, categories, and segmentation contour points of each object. The target detection box coordinates and categories are extracted by looking for specific patterns in the string ("left:", "top:", "right:", "bottom:", "class:" in this experiment). The segmentation contour points are extracted by looking for the data after "seg:" and parsing it into a series of cv::Point objects.

[0049] (5) Constructing test results The parsed information of each object is encapsulated as a triple (std::tuple), including the target detection box coordinates (std::vector <double>), category (int), segmentation contour points (std::vector <cv::point>). The information of all objects is added to the detect_result vector.

[0050] (6) Mapping to the internal data structure of ORB-SLAM2 The detect_result vector is passed through the relevant parameters in the tracking thread. When constructing the Frame object, the data in the detect_result is traversed, the valid information is extracted, and the corresponding Object objects are created using the constructor of the Object class. These Object objects contain various properties of dynamic objects and are stored in the objects_cur_ vector of the Frame for use in the subsequent ORB-SLAM2 processing flow.

[0051] S3. After the ORB-SLAM2 receiver obtains the segmentation result, it is mapped to the ORB-SLAM2 to remove dynamic feature points, including, and tracking based on the data after dynamic feature point removal; Dynamic feature point removal Figure 3 As shown: Segment (generated by instance segmentation) can be used as a complex polygon, and then the cv2.pointPolygonTest() function is used to determine whether the feature point (obtained by the ORB feature extraction module) is inside the polygon (the segmented dynamic object area). cv2.pointPolygonTest(contour, point, measureDist) is a function in OpenCV used to determine whether a point is inside the polygon area. It can return the sign and distance from the point to the polygon boundary. Therefore, Figure 3 As shown in the figure, we only need to use the target detection algorithm of YOLOv5-Seg to determine the feature points in the target frame, and then pass each feature point and Segment as parameters to the cv2.pointPolygonTest() function. According to the result, we can determine whether it is inside the dynamic object, and remove the feature points in the dynamic area during distortion correction. This removal method is combined with ORB-SLAM2, as shown in the figure. Figure 4 As shown, all feature points in the dynamic area "person" are removed, while feature points in other static areas are retained.

[0052] In step S3, tracking based on the data after dynamic feature points are eliminated includes: Map initialization: ORB-SLAM2 initializes the map based on the data after removing dynamic features, passes in continuous frames, and generates key frames from the two frames with the largest initial parallax; Constant speed model tracking: Visual tracking of consecutive frames in ORB-SLAM2 to maintain tracking stability; Reference keyframe tracking: Use keyframe information as a reference in ORB-SLAM2 to avoid lost tracking; Relocalization: In ORB-SLAM2, when tracking fails or becomes unstable, relocalization is achieved by referencing keyframes; Local map tracking: In ORB-SLAM2, local maps are referenced during tracking to improve positioning accuracy; Create a new keyframe: In ORB-SLAM2, keyframes are continuously selected from consecutive frames and retained in the keyframe sequence in chronological order, waiting to be called in subsequent steps.

[0053] S4. Perform local mapping and closed-loop detection correction in ORB-SLAM2; In step S4, performing local mapping in ORB-SLAM2 includes: Insert keyframe: Get the keyframe sequence and insert the new keyframe in the sequence into the map; Eliminate map points: clean up redundant or large-error map points; Generate map points: Generate new static map points based on inserted keyframes; Local BA optimization: perform Bundle Adjustment optimization on the local map, optimize the position of the key frames, and evaluate the redundancy of the key frames; Eliminate redundant keyframes: Remove keyframes in the sequence that contribute less to the overall map, and save the remaining optimized keyframes to the sequence in chronological order for use in the closed-loop step.

[0054] In step S4, performing closed-loop detection correction in ORB-SLAM2 includes: Query the database: input the current key frame, compare it with the key frame sequence in the database, and generate a closed loop when the same scene is detected, and determine the historical key frame corresponding to the current key frame; Calculate Sim3: After the closed loop is generated, calculate the similarity transformation Sim3 based on the trajectory between the current key frame and the historical key frame; Closed-loop fusion: Optimize and fuse the map based on the calculated transformation results, eliminate the accumulated trajectory drift, and correct the map; Essential map optimization: Further optimize the entire map structure according to the constraints of map optimization to ensure consistency.

[0055] Global BA and map update: Jointly optimize all keyframes and all map points to minimize the projection error of the entire system, optimize the keyframe poses and update the map point positions based on the calculation results.

[0056] S5. Add a dense mapping module based on ORB-SLAM2, and provide a point cloud filtering solution for dynamic objects and a global optimization solution.

[0057] (1) Get the depth map and color map of the current key frame Through the passed key frame (KeyFrame* kf), as well as the corresponding color map (cv::Mat&color) and depth map (cv::Mat&depth), first prepare the basic data required to generate the point cloud.

[0058] (2) Traverse each pixel in the depth map By traversing the pixels of the depth map in a double loop and sampling with a step size of 3 pixels, the amount of calculation and the density of the final point cloud are reduced.

[0059] (3) Depth value filtering For each pixel, we first check whether its depth value is within the valid range, which is set to be greater than 0.01 meters and less than 30 meters in this article. This is basic depth filtering to exclude invalid or too far points.

[0060] (4) Point cloud filtering of dynamic objects ① For each valid depth point, determine whether the point belongs to a dynamic object through the dynamic object information recorded in the keyframe object.

[0061] ② Convert the pixel coordinates of the current point into cv::Point2f format, and then traverse each dynamic object detected in the current key frame.

[0062] ③ For each dynamic object, check whether the current point is in the extended area of ​​the object detection box, and expand it by 50 pixels on the left, right, top and bottom. If so, use the cv::pointPolygonTest function to check whether the point is in the contour or on the edge of the dynamic object, and set the tolerance range to -50 pixels, that is, the dynamic contour is expanded.

[0063] ④ If the current point is within the area of ​​a dynamic object, it is marked as a point within the dynamic area and skipped without being added to the final point cloud.

[0064] (5) Point cloud construction For the points that pass the above filtering conditions, a point cloud is constructed based on its depth value and color image data. Here, the world coordinate position of each point is calculated and the corresponding RGB color value is assigned to the point in the point cloud.

[0065] (6) Point cloud stitching Each frame of point cloud generated needs to be stitched together to generate a more complete 3D map. Point cloud stitching aligns point cloud data from different frames to the same global coordinate system through pose transformation, thereby constructing a cumulative point cloud map.

[0066] (7) Point cloud filtering Use the statistical filter statistical_filter to process the global point cloud globalMap and remove noise points. The statistical filter calculates the average and standard deviation of the distance between each point and its neighboring points, and then removes those points that are too far from the average according to the set threshold. Then, use the voxel filter voxel to downsample the filtered point cloud tmp1 to reduce the density of the point cloud.

[0067] (8) Global point cloud update In the ORB-SLAM2 system, after global BA optimization, the pose of the keyframe is optimized, and the global point cloud needs to be updated and optimized; global BA corrects the camera pose drift problem caused by the accumulated error in the keyframe, and the optimized camera pose will also directly affect the position of the previously constructed three-dimensional map points. Therefore, the optimized keyframe pose is used to perform corresponding coordinate transformation and update on the point cloud data of the entire map to ensure the consistency of the map; in addition, independent point cloud data is repeated or unevenly distributed, and the global point cloud optimization solves this problem at the same time. During the optimization, the voxel filtering method is used to optimize the point cloud to improve the uniformity of the point cloud. The specific steps for updating the global point cloud are as follows: ① Check the point cloud update status and initialize Check the cloudbusy flag to ensure that no other process is updating the point cloud to avoid data conflicts. Set loopbusy = true to start the loop update process of the point cloud and output a prompt message. Initialize a temporary point cloud tmp1 to store all the transformed keyframe point cloud data.

[0068] ②Traverse the current keyframe and point cloud Double loop traverses the currently valid keyframe currentvpKFs and the associated point cloud data pointcloud. For each keyframe and point cloud pair, if the point cloud ID (pcID) matches the keyframe frame ID (mnFrameId), it means that this point cloud belongs to the keyframe.

[0069] ③Point cloud transformation GetPose() is used to obtain the pose of the key frame and converted to the Euclidean transformation matrix T through ORB_SLAM2::Converter::toSE3Quat. Then, use the pcl::transformPointCloud function to transform each matched point cloud into the global coordinate system according to the inverse pose of the key frame and accumulate it in tmp1.

[0070] ④Voxel filtering and point cloud update Create another temporary point cloud tmp2, use the voxel filter voxel to downsample tmp1, reduce the density of the point cloud, and improve storage and processing efficiency. Then, exchange the filtered point cloud tmp2 with the global map globalMap to update the point cloud data of the global map. After the update, set the loopbusy flag to false, indicating that the loop update process of the point cloud is over.

[0071] In the embodiments of the present application, tests were conducted on the TUM RGB-D dataset, and four datasets in the DynamicObjects task were selected for error evaluation. From the experimental results, the improvement rate of positioning accuracy of this patent on high-dynamic datasets can reach more than 85%, and the improvement rate on low-dynamic datasets is between 0-30%. In the comparison of dense mapping, this patent better eliminates dynamic objects and clearly presents static object information. In the original dense map of ORB-SLAM2, a large number of dynamic objects' ghosting obscured static objects, and the map was misplaced and distorted. These results show that this patent can greatly improve the positioning and mapping performance of the SLAM system in a dynamic environment. The specific effects achieved by this patent are as follows: 1. Feature point filtering effect Figure 5 The feature points detected by ORB-SLAM2 for the TUM dataset show that there are still many feature points in the area of ​​the dynamic object "person", which affect the accuracy of subsequent mapping and positioning. Figure 6 The detection results of ORB-SLAM2 combined with YOLOv5-Seg on the TUM dataset show that there are no feature points in the area of ​​the dynamic object "people", while the feature points are retained in the static area. This result shows that this patent successfully achieves pixel-level segmentation of dynamic objects and accurately filters out dynamic feature points, improving the accuracy of subsequent feature point matching.

[0072] 2. Significantly improved positioning accuracy The ATE absolute trajectory error was evaluated on the four selected TUM RGB-D datasets. The tests were conducted on the ORB-SLAM2 system and the improved system of this patent, and the results are shown in Table 1. Among them, walking_xyz and walking_static are high-dynamic datasets, and desk_with_person and sitting_halfsphere are low-dynamic datasets. In the low-dynamic dataset, the movement amplitude of some dynamic objects is small, which is not much different from the static environment, so there is only a small improvement. On the contrary, in the high-dynamic dataset, fast-moving dynamic objects will seriously interfere with the positioning and map construction process of the SLAM system through feature point matching errors, loop detection optimization errors, introduction of parallax errors, static environment occlusion, and irregular noise. In this scenario, effectively identifying and eliminating dynamic feature points can significantly improve the positioning accuracy. For example, in the "walking_xyz" dataset, such as Figure 7 , the camera pose trajectory CameraTrajectory estimated by the ORB-SLAM2 system has a significant deviation from the groundtruth and is completely out of touch with the real trajectory. Figure 8 The estimated trajectory and the actual trajectory of the improved system of this patent have a small deviation, which significantly improves the positioning accuracy.

[0073] 3. The readability and quality of dense maps have been significantly improved like Fig. 9 , using the dense mapping extended by ORB-SLAM2, dynamic objects are not eliminated. In this case, there are a lot of residual images of dynamic people in the dense map, and these residual images block some static objects, which seriously affects the mapping effect of static objects. The overall map presents a chaotic and incoherent appearance, the structure of the static environment cannot be clearly displayed, and the readability is extremely poor. Using the dense mapping method based on PCL combined with the dynamic point cloud elimination technology, such as Fig.10 The dense point cloud map generated by this patent significantly reduces the ghosting and noise interference caused by dynamic objects. Experimental results show that the dense map without dynamic objects can clearly present the details of the static environment, especially in scenes with large range of dynamic objects. The map quality generated by this patent method is much higher than the traditional ORB-SLAM2 extended dense mapping method.

[0074] 4. Real-time performance is guaranteed The present invention fully considers the real-time performance requirements of the system in a resource-constrained environment, selects a lightweight YOLOv5-Seg model and optimizes Socket communication, so that the computational efficiency of the instance segmentation and dynamic feature point removal modules is greatly improved. In a CPU environment, the processing frame rate of the method of the present invention can reach 10fps, which can meet the real-time requirements in a dynamic environment, while traditional deep learning segmentation algorithms such as Mask R-CNN cannot run in real time under the same conditions.

[0075] 5. Improve system robustness The present invention effectively avoids the interference of dynamic objects on the positioning and mapping of the SLAM system through real-time elimination of dynamic objects and global point cloud optimization, and improves the system's adaptability in complex dynamic scenes. This advantage is not only applicable to dynamic targets in indoor environments (such as pedestrians and mobile furniture), but also to complex dynamic target interference scenes such as vehicles and pedestrians in outdoor environments.

[0076] 6. Reduce usage complexity Through the modular integration of instance segmentation and SLAM system, combined with PCL-based automated point cloud optimization processing, the present invention simplifies the manual processing requirements of traditional SLAM system for dynamic scenes, making it more convenient in operation and deployment. In addition, the optimized system is easy to expand and apply, and is suitable for a variety of robot navigation and environmental perception scenarios.

[0077] Although exemplary embodiments of the present invention have been described for illustrative purposes, it will be understood by those skilled in the art that various modifications, additions, and substitutions may be made in form and detail without departing from the scope and spirit of the invention disclosed in the appended claims, and all such changes shall fall within the scope of protection of the appended claims of the present invention, and the various steps in the method claimed for protection by the present invention may be combined together in any combination. Therefore, the description of the embodiments disclosed in the present invention is not intended to limit the scope of the present invention, but is used to describe the present invention. Accordingly, the scope of the present invention is not limited by the above embodiments, but is defined by the claims or their equivalents.< / cv::point> < / double>

Claims

1. A SLAM method based on instance segmentation and dense mapping in a dynamic environment, characterized by: The following steps are involved: S1. Transmit the input data to the YOLOv5-seg module and the ORB feature extraction module in ORB-SLAM2 respectively; S2. Use the ORB feature extraction module to obtain image feature information, and use the YOLOv5-seg module to perform target detection and instance segmentation, and then transmit the segmentation results to ORB-SLAM2; S3. After the ORB-SLAM2 receiver obtains the segmentation result, it is mapped to ORB-SLAM2 to remove dynamic feature points, and tracking is performed based on the data after dynamic feature point removal; S4. Perform local mapping and closed-loop detection correction in ORB-SLAM2; S5. Add a dense mapping module based on ORB-SLAM2, and provide a point cloud filtering solution for dynamic objects and a global optimization solution.

2. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: The input data is: RGBD image data collected by a depth camera for the target area.

3. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: The YOLOv5-seg module includes a YOLOv5-seg network and a YOLOv5-seg transmitter; Among them, using the YOLOv5-seg module for target detection and instance segmentation includes: The YOLOv5-seg network in the YOLOv5-seg module is used to perform target detection on the input image and identify dynamic objects in the image; the YOLOv5-seg network also performs instance segmentation, segments dynamic objects at the pixel level, and generates target detection, category, and segmentation point sets.

4. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 3, characterized in that: When the segmentation result is transmitted to ORB-SLAM2 in step S2, since the YOLOv5-seg network is written in python code and ORB-SLAM2 is written in C++ code, it is necessary to use UNIX Domain Socket as the inter-process communication mechanism and add an ORB-SLAM2 receiver in ORB-SLAM2; The YOLOv5-seg sender in the YOLOv5-Seg module sends the segmentation results, i.e., the target detection box, category, and segmentation point set, to the ORB-SLAM2 receiver in real time through the UNIX Domain Socket, and then the ORB-SLAM2 receiver maps it to ORB-SLAM2, including: The YOLOv5-Seg transmitter uses Unix domain sockets to achieve efficient and low-latency data transmission, and sends the dynamic object detection results to the ORB-SLAM2 receiver in real time: First, a Unix domain socket is created and the listening mechanism is used to wait for the ORB-SLAM2 receiver to connect. After detecting a dynamic object, the YOLOv5-Seg sender encodes the target detection box, category, and segmentation point set mask information into a compact string format and uses Socket communication to quickly transmit the detection results. As the receiver, the ORB-SLAM2 receiver seamlessly integrates the detection results of the YOLOv5-Seg sender into the dynamic object removal process of ORB-SLAM2 through the parsing and integration mechanism: Socket communication is used to read the detection results, and the target detection box, category and segmentation point set are quickly extracted through parsing logic; the parsing results are encapsulated as structured objects containing geometric features and segmentation information, which are directly mapped to the data structure of ORB-SLAM2, and are used to construct continuous frame Frame objects and key frame KeyFrame objects and dynamically update the SLAM map.

5. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: In step S3, after the ORB-SLAM2 receiving end obtains the segmentation result, it transmits it to ORB-SLAM2 for dynamic feature point removal, including: The feature points in the target detection frame are screened. Only when the feature points are in the target detection frame, the dynamic object area is segmented into precise polygons according to the segmentation point set using the cv2.pointPolygonTest() function. When judging whether the feature points are in the polygonal area, if they are judged to be dynamic feature points, they are marked and removed during the distortion correction process according to the marking information, thereby ensuring that the system only uses feature points in the static area.

6. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: In step S3, tracking based on the data after dynamic feature points are eliminated includes: Map initialization: ORB-SLAM2 initializes the map based on the data after removing dynamic features, passes in continuous frames, and generates key frames from the two frames with the largest initial parallax; Constant speed model tracking: Visual tracking of consecutive frames in ORB-SLAM2 to maintain tracking stability; Reference keyframe tracking: Use keyframe information as a reference in ORB-SLAM2 to avoid lost tracking; Relocalization: In ORB-SLAM2, when tracking fails or becomes unstable, relocalization is achieved by referencing keyframes; Local map tracking: In ORB-SLAM2, local maps are referenced during tracking to improve positioning accuracy; Create a new keyframe: In ORB-SLAM2, keyframes are continuously selected from consecutive frames and retained in the keyframe sequence in chronological order, waiting to be called in subsequent steps.

7. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: In step S4, performing local mapping in ORB-SLAM2 includes: Insert keyframe: Get the keyframe sequence and insert the new keyframe in the sequence into the map; Eliminate map points: clean up redundant or large-error map points; Generate map points: Generate new static map points based on inserted keyframes; Local BA optimization: perform Bundle Adjustment optimization on the local map, optimize the position of the key frames, and evaluate the redundancy of the key frames; Eliminate redundant keyframes: Remove keyframes in the sequence that contribute less to the overall map, and save the remaining optimized keyframes to the sequence in chronological order for use in the closed-loop step.

8. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: In step S4, performing closed-loop detection correction in ORB-SLAM2 includes: Query the database: input the current key frame, compare it with the key frame sequence in the database, and generate a closed loop when the same scene is detected, and determine the historical key frame corresponding to the current key frame; Calculate Sim3: After the closed loop is generated, calculate the similarity transformation Sim3 based on the trajectory between the current key frame and the historical key frame; Closed-loop fusion: Optimize and fuse the map based on the calculated transformation results, eliminate the accumulated trajectory drift, and correct the map; Essential map optimization: further optimize the entire map structure according to the constraints of map optimization to ensure consistency; Global BA and map update: Jointly optimize all keyframes and all map points to minimize the projection error of the entire system, optimize the keyframe poses and update the map point positions based on the calculation results.

9. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 1, characterized in that: The step S5 comprises: S501. Obtain the depth map and color map of the key frame newly created during the tracking process: Through the key frame passed in, the color image and depth image corresponding to the key frame are used as the basic data required to prepare the point cloud; S502. Traverse each pixel in the depth map and perform sampling: By traversing the pixels of the depth map in a double loop and sampling with a step size of 3 pixels, the amount of calculation and the density of the final point cloud are reduced; S503. Depth value filtering: For each pixel, first check whether its depth value is within the set valid range, and filter out the pixels that are not within the set valid range; S504. Point cloud filtering of dynamic objects: For each valid depth point, determine whether the point belongs to a dynamic object through the dynamic object information recorded in the keyframe object; ② Convert the pixel coordinates of the current point into cv::Point2f format, and then traverse each dynamic object detected in the current key frame; ③ For each dynamic object, check whether the current point is within the extended area of ​​the object detection frame, and expand it by 50 pixels on the left, right, top and bottom; if so, use the cv::pointPolygonTest function to check whether the point is within the contour or on the edge of the dynamic object, and set the tolerance range to -50 pixels, that is, the dynamic contour is expanded, and the expanded area is used as the area of ​​the dynamic object; ④ If the current point is within the area of ​​a dynamic object, it is marked as a point within the dynamic area and skipped without being added to the final point cloud; S505. Generate point cloud: For the points filtered through the point cloud, a point cloud is constructed based on its depth value and color image data; the world coordinate position of each point is calculated here, and the corresponding RGB color value is assigned to the point in the point cloud; S506. Point cloud stitching: Each frame of point cloud generated needs to be stitched together to generate a more complete 3D map. Point cloud stitching aligns point cloud data from different frames to the same global coordinate system through pose transformation, thereby constructing a cumulative point cloud map. S507. Point cloud filtering: Use the statistical filter statistical_filter to process the global point cloud globalMap to remove noise points; the statistical filter calculates the average and standard deviation of the distance between each point and its neighboring points, and then removes those points whose distance to the average value exceeds the threshold according to the set threshold; then, use the voxel filter voxel to downsample the filtered point cloud tmp1 to reduce the density of the point cloud; S508. Global point cloud update: In the ORB-SLAM2 system, after global BA optimization, the pose of the keyframe is optimized, and the global point cloud needs to be updated and optimized; global BA corrects the camera pose drift problem caused by cumulative errors in the keyframe, and the optimized camera pose will also directly affect the position of the previously constructed three-dimensional map points. Therefore, the optimized keyframe pose is used to perform corresponding coordinate transformation and update on the point cloud data of the entire map to ensure the consistency of the map; in addition, independent point cloud data may be repeated or unevenly distributed, and this problem can be solved while optimizing the global point cloud. During the optimization, the voxel filtering method is used to optimize the point cloud to improve the uniformity of the point cloud.

10. The SLAM method based on instance segmentation and dense mapping in a dynamic environment according to claim 9, characterized in that: The step S508 includes: ① Check the point cloud update status and initialize: Check the cloudbusy flag to ensure that no other process is updating the point cloud to avoid data conflicts; set loopbusy = true to start the loop update process of the point cloud and output a prompt message; initialize a temporary point cloud tmp1 to store all the transformed keyframe point cloud data; ②Traverse the current keyframe and point cloud: Double loop traverses the currently valid keyframe currentvpKFs and the associated point cloud data pointcloud; for each keyframe and point cloud pair, if the point cloud ID matches the frame ID of the keyframe, it means that this point cloud belongs to the keyframe; ③Point cloud transformation: Use GetPose() to get the pose of the key frame and convert it to the Euclidean transformation matrix T through ORB_SLAM2::Converter::toSE3Quat; then, use the pcl::transformPointCloud function to transform each matched point cloud into the global coordinate system according to the inverse pose of the key frame and accumulate it into tmp1; ④Voxel filtering and point cloud update Create another temporary point cloud tmp2, use the voxel filter voxel to downsample tmp1, reduce the density of the point cloud, and improve storage and processing efficiency; then, exchange the filtered point cloud tmp2 with the global map globalMap to update the point cloud data of the global map; after the update, set the loopbusy flag to false, indicating that the loop update process of the point cloud is over.

Images