A 3D semantic-based visual SLAM method in a basement scene

Abstract

The application discloses a kind of 3D semantic information-based SLAM methods under the garage scene.The application extracts ground and above-ground semantic information from the garage scene based on the method of deep learning, and these semantics jointly constitute 3D semantic information.Using these extracted 3D semantic information, a semantic-based data association and local mapping algorithm is designed to obtain inter-frame pose estimation results.The application proposes a 3D semantic-based loop detection and global optimization method to eliminate cumulative errors, enabling real-time positioning and semantic map construction in the garage.Compared to similar feature point-based or 2D semantic-based SLAM methods, the application has significant advantages, with improved positioning accuracy and stability.

Description

Technical Field

[0001] This invention relates to a method for real-time localization and mapping in unknown environments in the field of autonomous driving, and in particular to a visual real-time localization and mapping method based on 3D semantics in a parking garage scenario. Background Technology

[0002] In recent years, autonomous driving technology has been developing rapidly, and can be divided into two main areas based on different usage scenarios: road driving and autonomous parking. Autonomous parking (AVP) is mainly aimed at garage scenarios, where the driving speed is relatively slow. However, in underground garage scenarios, there is often no GPS signal, and the roads are narrow and congested. Achieving accurate navigation and autonomous parking in such scenarios places extremely high demands on the system's positioning and mapping accuracy.

[0003] Simultaneous Localization and Mapping (SLAM) is a crucial technique for localizing oneself and simultaneously reconstructing the 3D structure of a scene to create a map in an unknown environment based on observations from onboard sensors. Camera-based visual SLAM typically consists of three modules: a front-end, a back-end, and a loop closure mechanism. The front-end extracts features and performs inter-frame matching to estimate inter-frame pose. The back-end constructs and maintains scene structure information based on the front-end's estimation results. The loop closure mechanism checks if the current observation position has been previously visited; upon detection, global optimization is performed to eliminate accumulated errors. Therefore, feature extraction and matching are critical factors affecting the accuracy of visual SLAM throughout the entire algorithm.

[0004] However, traditional visual feature point-based SLAM solutions face significant challenges in underground parking garage scenarios. First, indoor and underground parking lots are mostly composed of walls and floors with weak textures, making it easy for traditional feature point methods to lose tracking. Second, underground parking garages often experience insufficient lighting and motion blur, making effective feature point matching impossible for feature point-based localization, resulting in severe motion drift and poor localization accuracy.

[0005] Therefore, solutions based on visual feature points struggle to reliably locate and map underground parking garage scenes. However, parking garage scenes possess robust and long-term stable semantic features, such as parking lines, lane lines, stop lines, road markings, and pillars. Furthermore, with the development of neural networks, semantic recognition, detection, and segmentation have become more accurate. Thus, utilizing semantic features in SLAM research has a natural advantage over traditional feature points in terms of feature robustness and long-term stability. Existing semantic-based visual SLAM methods primarily use 2D semantic information on the ground, such as parking spaces, speed bumps, and arrows. Most of these parking garage SLAM algorithms utilize inverse perspective mapping (IPM) to obtain a panoramic view, and then perform semantic segmentation within this panoramic view. They employ signage commonly found in parking lots, such as guide signs, parking lines, and speed bumps, fused with wheel odometers or IMUs for semantic point map construction and localization. These algorithms only utilize ground semantic information as SLAM features, therefore they cannot operate stably in scenes lacking ground semantic information. For example, in scenarios with relatively sparse ground semantics, such as near the entrance of an underground parking garage or at a corner, the above methods are prone to large positioning errors or positioning failures. Summary of the Invention

[0006] This invention aims to solve the problems existing in the background technology, specifically the problem that the underground parking garage environment varies greatly with light and time, and traditional visual positioning methods often cannot achieve stable positioning. It provides a visual SLAM method based on 3D semantics in the underground parking garage scene to improve the stability and accuracy of real-time positioning and mapping.

[0007] This invention uses a deep neural network to perform 3D pillar detection and traffic sign detection from the front view, and ground semantic segmentation from the surround view. The semantic extraction results are post-processed for underground parking scenes and SLAM tasks.

[0008] This invention designs semantic association and local mapping algorithms based on the extracted 3D semantic information. It performs inter-frame semantic association between the 3D semantic elements detected in the current frame and the 3D semantic elements in the previous frame, optimizing the inter-frame pose. For associated semantic elements, the corresponding semantic elements in the map are updated; while unassociated semantic elements are added to the local map, achieving map updating and fusion.

[0009] This invention addresses the cumulative drift problem by proposing a loop closure detection algorithm based on 3D semantics. A semantic descriptor is constructed for loop closure detection. Upon detection, loop closure correction is performed first to optimize the current frame pose, followed by global optimization to improve the poses of all keyframes and the 3D semantic map, thereby enhancing SLAM accuracy and robustness.

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

[0011] I. A Visual SLAM Method Based on 3D Semantics for Basement Scenes

[0012] 1) The front view and surround view are acquired by the mobile terminal. The front view and surround view are used to detect 3D columns, detect air traffic signs and perform semantic segmentation of ground signs, thereby obtaining 3D semantic information.

[0013] 2) Take the initial frame as a keyframe, then calculate the initial pose of the current frame based on 3D semantic information; determine the keyframe based on the motion distance and motion angle. If the current frame is a keyframe, construct the initial local semantic map based on the initial frame and the current frame; otherwise, continue to calculate the initial pose of the next frame until the current frame is a keyframe and the initial local semantic map is obtained, and then take the next frame as the current frame and execute 3).

[0014] 3) Calculate the initial pose of the current frame based on 3D semantic information, and then optimize the initial pose of the current frame using the current local semantic map to obtain the optimized pose of the current frame.

[0015] 4) Based on motion distance and motion angle, keyframes are determined. If the current frame is a keyframe, the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain the updated local semantic map. Then, a semantic descriptor is constructed using the 3D semantic information of the current keyframe. Next, loop closure is determined using the semantic descriptor. If the current keyframe constitutes a loop, loop closure correction and global optimization are performed to obtain the global map. If the current frame is not a keyframe or does not constitute a loop, step 5) is executed directly.

[0016] 5) Repeat steps 3)-4) to process the 3D semantic information of the next frame until a complete local semantic map is generated, thereby obtaining the global map;

[0017] 6) Repeat steps 2)-5) to accumulate the local semantic maps and obtain the final global map.

[0018] In step 1), a front view is acquired using a front-view camera, and the front view is used to detect 3D pillars and air traffic signs, respectively, to obtain 3D pillar information and air traffic sign information.

[0019] In step 1), multiple fisheye images from multiple perspectives are acquired using multiple fisheye cameras. Then, the multiple fisheye images from multiple perspectives are projected onto the ground and stitched together to form a panoramic view. The panoramic view is then used to perform semantic segmentation of ground markings to obtain ground marking information.

[0020] In step 2) or 3), calculating the initial pose of the current frame based on 3D semantic information specifically involves:

[0021] After matching the 3D semantic information of the current frame with the 3D semantic information of the previous frame, the inter-frame pose is obtained, and then the inter-frame pose is used to calculate the initial pose of the current frame.

[0022] The step of matching the 3D semantic information of the current frame with the 3D semantic information of the previous frame to obtain the inter-frame pose is as follows:

[0023] First, the nearest neighbor search is used to match the 3D pillars of the current frame with the previous frame. Then, the initial inter-frame pose is calculated using the matched 3D pillars. Next, 3D-3D matching is performed using the 3D semantic points of air traffic signs and ground signs to optimize the initial inter-frame pose, thereby obtaining the final inter-frame pose.

[0024] In step 3), after optimizing the initial pose of the current frame using the current local semantic map, the optimized pose of the current frame is obtained, specifically as follows:

[0025] The 3D semantic information of the current frame is projected onto the local coordinate system, and then the 3D semantic information of the current frame is matched using the current local semantic map to obtain the optimized pose of the current frame.

[0026] In step 4), the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain the updated local semantic map, specifically as follows:

[0027] If there are 3D pillars in the current frame's 3D semantic information that are associated with the current local semantic map, then update the association count of the corresponding pillar in the map, and recalculate the center point position and the mean yaw angle of the associated pillars based on the current frame's 3D semantic information. Then, update the center point position and yaw angle of the associated pillars in the local semantic map according to the new center point position and the mean yaw angle. If there are no associated pillars, then create a new 3D pillar in the map based on the current frame's 3D semantic information to obtain the updated local semantic map.

[0028] In step 4), the 3D semantic information is used to construct a semantic descriptor, specifically as follows:

[0029] The semantic 3D points of the current frame are divided into blocks, and the semantic tags with the highest frequency in each block are used as the features of that block to construct a description matrix, which is then used as a semantic descriptor.

[0030] II. A storage medium

[0031] The storage medium stores a computer program that, when executed by a processor, implements the method described.

[0032] The computer program referred to therein is an instruction that implements the method.

[0033] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0034] 1. This invention extracts stable 3D semantic information from underground parking garage scenes. In addition to commonly used ground semantics, it innovatively extracts 3D pillars and 3D traffic signs on the ground. The extracted 3D semantic elements are post-processed to provide rich semantic features for the SLAM algorithm and improve the robustness of the algorithm.

[0035] 2. This invention utilizes extracted 3D semantic information to perform inter-frame semantic association and local map semantic association, optimizes the inter-frame pose, and updates and fuses the local semantic map, enabling the algorithm to obtain better real-time positioning results and 3D semantic maps.

[0036] 3. This invention utilizes 3D semantic elements to design a loop closure detection algorithm. After detecting a loop, loop closure correction and global optimization are performed to optimize the 3D semantic map and pose, thereby improving the localization and mapping accuracy of the SLAM algorithm. Attached Figure Description

[0037] Figure 1 This is a basic framework for a SLAM method based on 3D semantic information in a basement scenario.

[0038] Figure 2 This is a schematic diagram of the traffic sign detection and plane fitting results.

[0039] Figure 3 The flowchart shows the linear fitting process for ground semantic segmentation results.

[0040] Figure 4 This is a schematic diagram of the straight line fitting results.

[0041] Figure 5 This is a schematic diagram of a local 3D semantic map.

[0042] Figure 6 This is a schematic diagram of semantic loop correction.

[0043] Figure 7 This is a schematic diagram of the trajectory results of various SLAM algorithms at the same location.

[0044] Figure 8 This is a schematic diagram of a ground semantic map and a 3D semantic map. Detailed Implementation

[0045] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0046] like Figure 1 As shown, the present invention includes the following steps:

[0047] 1) The front view and surround view are acquired by the mobile terminal. The front view and surround view are used to detect 3D columns, detect air traffic signs and perform semantic segmentation of ground signs, thereby obtaining 3D semantic information.

[0048] Specifically: A front-view image is acquired using a front-view camera. 3D pillars and aerial traffic signs are detected using this image, yielding 3D pillar information and aerial traffic sign information respectively. In this embodiment, 3D pillar detection specifically involves: inputting the front-view image, using a PGD network as the base model to predict various attributes. The regressed attributes include the 2D bounding box (specifically, the distance from the point to the top, bottom, left, and right edges of the 2D bounding box), the size of the 3D bounding box (length, width, and height), the depth of the center point of the 3D bounding box, the orientation category, and the orientation angle. After obtaining the 2D projection and depth of the center point, it is back-projected into 3D space according to camera parameters to obtain the 3D coordinates of the center point, thus obtaining a complete representation of the pillar's 3D bounding box. Traffic sign detection specifically involves: inputting the front-view image, using Polygon-YOLOv5 as the base framework to detect traffic sign corner points, and outputting the coordinates, category, and confidence score of the four corner points of the traffic sign. The categories are divided into square signs, circular signs, and diamond signs according to their shape, such as... Figure 2 As shown in (a). Then, the depth results of the entire image are obtained through the monocular depth estimation network iDisc, as shown... Figure 2 As shown in (b). Then, the points inside the traffic sign corner detection area are extracted, as shown in... Figure 2 As shown in (c), the Random Sampling Consensus (RANSAC) algorithm is used for plane fitting to remove drift points, resulting in a depth with better consistency, as shown in [example image]. Figure 2 As shown in (d), the 3D semantic information of the traffic sign was obtained.

[0049] Multiple fisheye images are acquired using multiple fisheye cameras. These images are then projected onto the ground and stitched together to form a surround view. The surround view is then used for semantic segmentation of ground markings to obtain ground marking information. In this embodiment, the ground semantic segmentation specifically involves: inputting the surround view image, using DeepLabV3++ as the base model for ground semantic segmentation, classifying pixels into five categories: lanes, parking lines, arrows, speed bumps, and background, and outputting a dense semantic segmentation result image. Parking spaces on the ground often have straight center lines, so the segmentation result can be fitted with a straight line. This ensures smooth edges and eliminates some distortion effects. First, different connected components are distinguished from the segmentation result. Then, connected components with an area less than 100 pixels are removed. Finally, the RANSAC algorithm is used to fit a straight line to each connected component. The straight line fitting process is as follows: Figure 3As shown. Input the data for each connected component, set the fitting parameters, including the maximum number of iterations (1000), the fitting error threshold (0.01 meters), the threshold for the number of interior points on the fitted line (20), and the number of fitted lines (20). Iterate continuously, randomly selecting two points in each iteration to build a spatial line model, and calculate the distance from the sampled remainder set to the spatial line. If the distance is less than the fitting error threshold, it is considered an interior point, and the number of interior points is counted. After iteration, select the model parameters corresponding to the set with the most interior points. After obtaining the fitted line, we set the line width to 10cm as the final post-processing result. The final line fitting result is shown below. Figure 4 of (a), Figure 4 (b) and Figure 4 As shown in (c).

[0050] 2) The initial frame is used as a keyframe. Then, the initial pose of the current frame is calculated based on 3D semantic information. The keyframe is determined based on the motion distance and motion angle. If the current frame is a keyframe, an initial local semantic map is constructed based on the initial frame and the current frame. Otherwise, the initial pose of the next frame is calculated until the current frame is a keyframe and the initial local semantic map is obtained. Then, the next frame is used as the current frame and 3) is executed. In this embodiment, a local semantic map is constructed every 30 meters. The local coordinate system is the vehicle coordinate system at the start time of constructing the local semantic map (i.e., the initial frame time of each local semantic map). That is, the local coordinate system is the coordinate system in which the local semantic map is located during the SLAM process.

[0051] 3) Calculate the initial pose of the current frame based on 3D semantic information, and then optimize the initial pose of the current frame using the current local semantic map to obtain the optimized pose of the current frame.

[0052] In 2) or 3), the initial pose of the current frame is calculated based on 3D semantic information, specifically as follows:

[0053] The 3D semantic information extracted from the current frame image is matched with the 3D semantic information extracted from the previous frame to obtain the inter-frame pose, and then the inter-frame pose is used to calculate the initial pose of the current frame.

[0054] The inter-frame pose is obtained by matching the 3D semantic information extracted from the current frame with the 3D semantic information extracted from the previous frame. Specifically:

[0055] First, the nearest neighbor search is used to match the 3D pillars of the current frame with the previous frame. Then, the initial inter-frame pose is calculated using the matched 3D pillars. Next, 3D-3D matching is performed using the 3D semantic points of air traffic signs and ground signs to optimize the initial inter-frame pose, thereby obtaining the final inter-frame pose.

[0056] Specifically:

[0057] First, use a uniform motion model to estimate the initial inter-frame pose. This represents the pose transformation from frame k to frame (k-1). Then, the 3D pillars detected in frame k are projected onto frame (k-1) based on their inter-frame poses. Nearest neighbor search is used to find the pillar with the closest center point in frame (k-1). If the nearest neighbor distance is less than 1 meter, the inter-frame pillar match is considered successful; otherwise, there is no inter-frame match. The inter-frame poses are optimized to make the poses of all matched pillars as close as possible in the same coordinate system, as shown in the following formula:

[0058]

[0059]

[0060] in, This represents the pose from frame k to frame (k-1) to be optimized, where N represents the number of matching pillar pairs between the two frames, and CO... i Denotes the column in the i-th pair of matches, |||| 2 This indicates that the error term in the middle is squared. This represents the error term constructed based on the pillars and the relationship between poses between frames. This error term represents the pose of the i-th pillar in the k-th frame. The inter-frame pose that should be projected onto the pillar in frame k-1 and match the inter-frame pose in frame k-1. Get as close as possible. This indicates that the matrix is ​​solved by Lie algebra, transforming it from a 4x4 pose matrix into a 6-dimensional vector. The inter-frame pose is obtained by least-squares optimization of equation (2) using the LM (Levenberg–Marquardt) algorithm.

[0061] Then, a higher-precision inter-frame pose is obtained by using a large number of traffic sign points and ground semantic segmentation results for pose optimization. Since traffic sign points and segmentation results are dense pixels, points are downsampled, with one pixel sampled every four pixels. The downsampled points are projected onto the vehicle coordinate system. Traffic sign points are projected onto the vehicle coordinate system using only the regions whose depth was recovered during semantic extraction, based on camera intrinsics and vehicle coordinate system extrinsics. Ground segmentation results are projected onto the vehicle coordinate system according to the pixel values ​​at calibration and the actual physical distance. Then, inter-frame matching is performed on these semantic 3D points using Iterative Closest Point (ICP). During the matching process, the plinth-based inter-frame matching optimized pose is used as the initial pose, achieving inter-frame semantic point nearest neighbor association and inter-frame pose optimization.

[0062] In step 3), the optimized pose of the current frame is obtained by optimizing the initial pose of the current frame using the current local semantic map, specifically as follows:

[0063] The 3D semantic information of the current frame is projected onto the local coordinate system, and then the 3D semantic information of the current frame is matched using the current local semantic map, that is, the pose of the current frame is optimized to obtain the optimized pose of the current frame.

[0064] 4) Keyframe determination is performed based on motion distance and motion angle. If the current frame is a keyframe, the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain the updated local semantic map. Then, a semantic descriptor is constructed using the 3D semantic information of the current keyframe. Next, loop closure is determined using the semantic descriptor, that is, whether the current frame forms a loop with the historical keyframes is detected based on the semantic descriptor. If the current keyframe forms a loop, loop closure correction and global optimization are performed, that is, the semantic elements and the poses of all keyframes are jointly optimized to obtain the global map, that is, the localization and mapping results are obtained, which are composed of the 3D semantic information of all keyframes. If the current frame is not a keyframe or does not form a loop, step 5) is executed directly. The keyframe selection principle is: the motion distance from the previous keyframe is greater than 1 meter or the angle change from the previous keyframe is greater than 5 degrees.

[0065] In step 4), the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain the updated local semantic map, specifically as follows:

[0066] If the current frame's 3D semantic information contains 3D pillars associated with the current local semantic map, the association count of the corresponding pillar in the map is updated. Then, based on the current frame's 3D semantic information, the mean center point position and yaw angle of the associated pillars are recalculated. Next, the center point position and yaw angle of the associated pillars in the local semantic map are updated according to the new center point position and yaw angle mean. If no associated pillar exists, a new 3D pillar is created in the map based on the current frame's 3D semantic information, thus obtaining an updated local semantic map. Voxel downsampling is used for map updates of 3D semantic points in the map.

[0067] In this embodiment, the association is determined using the column category, size, center point position, and yaw angle. The criteria for 3D column association are that the semantic category labels are consistent, the maximum size deviation is less than a threshold, and the center point position deviation and yaw angle deviation are also less than a threshold.

[0068] The specific associations in the local semantic map are as follows:

[0069] First, associate the 3D pillars. Then, obtain the pose transformation of the current frame relative to the local coordinate system through inter-frame pose accumulation. Then the current frame pillars are projected onto the local coordinate system, as shown in the following formula:

[0070]

[0071] in, The pose of the pillar in the vehicle coordinate system of the current frame, after pose transformation Convert to pillar pose in local coordinate system Next, pillar association is performed in the local coordinate system. However, due to the use of accumulated pose, there is a certain amount of accumulated drift, so it cannot be directly matched with nearest neighbors like inter-frame association, and mismatches may occur. This invention utilizes multi-dimensional pillar information for association. First, it filters by category and size, finding pillars with the same category and a maximum size difference within 0.2 meters. Then, it judges the center point position and yaw angle. If the center point position deviation is within 1 meter and the yaw angle difference is within 5 degrees, the association is successful. Adding multiple rule judgments can effectively prevent mismatches. The judgment rules can be expressed by the following formula:

[0072]

[0073] in, These represent the category, size, center point location, and yaw angle of the pillars in the local map, respectively. k ,S k ,P k ,Y k This indicates the type, size, center point position, and yaw angle of the pillar in the current frame. S ,Thr P ,Thr Y These are the set size difference threshold, center point offset threshold, and yaw angle deviation threshold, respectively, where |||| represents the absolute value. After completing the column association using the above method, optimization equations similar to formulas (1) and (2) are used to optimize the pose of the current frame relative to the local coordinate system.

[0074] For traffic sign points and ground semantic points, the pose transformation of the current frame relative to the local coordinate system obtained by column association optimization is used as the initial pose. Then, iterative closest point (ICP) matching is used to optimize the initial pose between the semantic points of the current frame and the semantic points of the local map, resulting in a higher-precision relative pose of the current frame relative to the local map.

[0075] The pose transformation obtained from the above local semantic map association optimization The semantic features of the current keyframe are projected onto the local coordinate system. In addition to storing detection information such as bar label, size, center point position, and yaw angle, the 3D bars in the semantic map also store the number of associations for that bar, and calculate the mean and variance of the center point position and yaw angle, as shown in the following formula:

[0076] Local CO ={li ,S i ,P i ,Y i C i AvgP i AvgY i} i＝1,2,…N (5)

[0077] Among them, Local CO Represents the pillars in a local map, where N represents the number of pillars, l i ,S i ,P i ,Y i Let C represent the category, size, center point position, and yaw angle of the i-th column, respectively. i AvgP i AvgY i Let $\mathbf{i}$ represent the association count, mean center point position, and mean yaw angle of the $i$-th column, respectively. For the 3D columns in the current keyframe, if there are associated columns in the local map, the association count of the associated columns in the local map is updated, and the mean and variance are recalculated. The position and yaw angle of the map column are updated based on the new mean. Since the column size is generally fixed, the column size is not updated when the map is updated. If there are no associated columns in the local map, a new column needs to be created in the local map, and the information in formula (5) is calculated.

[0078] Traffic signs and ground semantic points are both 3D points. To reduce map storage space, this invention first extracts voxels from these 3D semantic points, with a voxel size of 0.2 meters. When a new keyframe appears, the keyframe semantics are first transformed to the local coordinate system according to the pose transformation, then added to the local map, and the local map is downsampled again. This method completes the construction of a local 3D semantic map, including 3D pillars, 3D traffic signs, and ground semantic points, such as... Figure 5 of (a), Figure 5 (b) and Figure 5 As shown in (c).

[0079] Specifically, 3D semantic information is used to construct semantic descriptors, as follows:

[0080] The semantic 3D points of the current frame are divided into blocks, and the most frequent semantic tag within each block is used as the feature to construct a description matrix, which is then used as the semantic descriptor. The original Scan context extracts the coordinates of the highest point in the point cloud within each block. In this embodiment, the 3D semantic map mainly consists of ground semantic points, 3D traffic signs, and 3D pillars. Since the height coordinates of the ground semantic points are all 0, using height as block information is unreasonable. Therefore, this embodiment designs a semantic Scan context for ground semantic points and 3D traffic sign points, which involves counting the number of semantic points of each category within a block and using the tag corresponding to the most frequent semantic point within the block as the value of the corresponding position in the description matrix.

[0081] 5) Repeat steps 3)-4) to process the 3D semantic information of the next frame until a complete local semantic map is generated, thereby obtaining the global map;

[0082] 6) Repeat steps 2)-5) to accumulate the local semantic maps and obtain the final global map.

[0083] In this embodiment, the loop closure determination is specifically as follows:

[0084] This invention designs a semantic-based Scan Context, an improvement on the point cloud-based Scan context method, primarily utilizing semantic point category and spatial distribution information. The original Scan context extracts the coordinates of the highest point in the point cloud within each block. In this invention's 3D semantic map, which mainly consists of ground semantic points, 3D traffic signs, and 3D pillars, the height coordinates of ground semantic points are all 0, making the use of height as block information unreasonable. Therefore, this invention designs a semantic Scan context for ground semantic points and 3D traffic sign points, which counts the number of semantic points of each category within a block and uses the label corresponding to the semantic point with the highest number of points as the value of the corresponding position in the description matrix. This mainly includes eight categories: center line, stop line, arrow, speed bump, square traffic sign, circular traffic sign, diamond traffic sign, and background, thereby achieving loop closure judgment based on 3D semantic points.

[0085] However, due to the high similarity of the underground parking lot scenes, using only 3D semantic points for loop closure detection will still result in false detections. Therefore, this invention has performed post-detection verification. When a loop closure is detected, it is only considered a correct loop closure if the loop closure is detected in three consecutive keyframes.

[0086] Specifically, semantic loop correction includes:

[0087] After successfully detecting a loop closure, loop closure correction needs to be performed on the current frame to eliminate the drift error of the current frame, such as... Figure 6As shown. Among them. This represents the pose of the i-th keyframe in the global coordinate system. Let's assume the current keyframe is k. c A loop closure was detected in keyframe k1. Represents the 3D pillars and 3D semantic points of the loop frame. This represents the 3D pillars and 3D semantic points of the current keyframe. First, ICP matching is performed on the 3D semantic points of the current keyframe and the loopback frame to calculate the pose transformation between the two frames. Then, the pose transformation matrix between the two frames is used to project the current frame onto the loopback frame coordinate system. Finally, a nearest neighbor search is used to find the matching 3D semantic points and 3D pillars between the current frame and the loopback frame, thus achieving... and The relationship between them.

[0088] The pose of the current keyframe is optimized using the correlation results through the following formula:

[0089]

[0090] in, This represents the current frame k to be optimized. c The pose transformation to loopback frame k1. N represents the number of associated pillars, M represents the number of associated 3D semantic points, |||| 2 This indicates that the error term in the middle is squared. Let represent the observation error of the i-th associated pillar, which is to optimize the pose to make the poses of the associated pillars as close as possible. The formula is as follows:

[0091]

[0092] in, It is the inverse of the pose of the i-th associated pillar in the loop-loop frame coordinate system. This represents the pose of the i-th associated pillar in the current frame coordinate system. This indicates that Lie algebra is used to solve the matrix, transforming the 4x4 pose matrix into a 6-dimensional vector. Let represent the observation error of the j-th associated 3D semantic point. By optimizing the pose, the positions of the associated semantic points are made as close as possible, as shown in the following formula:

[0093]

[0094] in, This represents the j-th associated 3D semantic point in the loop-loop frame coordinate system. This represents the j-th associated 3D semantic point in the current frame coordinate system. The pose result is obtained by least-squares optimization of equation (6) using the LM (Levenberg–Marquardt) algorithm.

[0095] The current frame pose can be corrected based on the optimized pose, as shown in the following formula:

[0096]

[0097] in, This indicates the pose of the current frame in the global coordinate system after loop closure correction. This indicates the pose of the loopback frames obtained by superimposing the odometry data in the global coordinate system. This represents the pose transformation of the current frame relative to the loop-loop frame obtained through optimization using formula (6), thereby completing the semantic loop-loop correction.

[0098] The global optimization specifically includes:

[0099] The main focus is on optimizing the poses of all keyframes and the 3D semantic map to improve SLAM accuracy and robustness. This invention designs two optimization factors: inter-frame pose constraints and 3D pillar observation constraints. The optimization formulas are shown below:

[0100]

[0101] in, This is the quantity to be optimized, where K represents the total number of keyframes, and S represents the total number of pillars in the map. 2 This indicates that the error term in the middle is squared. This is the inter-frame pose constraint error, ensuring that the optimized global pose conforms to the inter-frame constraints. Its formula is as follows:

[0102]

[0103] in, It is the inter-frame pose from the i-th keyframe to the (i+1)-th keyframe, and is the observation value. This represents the inverse of the pose from the i-th keyframe to the global coordinate system. This represents the pose of the (i+1)th keyframe in the global coordinate system. This indicates that Lie algebra is used to solve the matrix, transforming the 4x4 pose matrix into a 6-dimensional vector. This refers to the 3D pillar observation error, ensuring consistency between the 3D pillars constructed in the semantic map and the detected pillars. The formula is as follows:

[0104]

[0105] in, It is the j-th semantic location in the map Figure 3 The inverse pose of column D in the global coordinate system This represents the pose of the i-th keyframe in the global coordinate system. This represents the pose of the j-th 3D pillar detected in the i-th keyframe.

[0106] During the global optimization process described above, it is important to note that the two frames in which a loopback is detected are: Figure 6 The poses of the two frames outlined in the Chinese box are fixed and not optimized globally, because the poses at the loop closure have already been corrected during pose correction. Only the poses of all keyframes in the middle of the loop and the 3D pillars in the map need to be optimized. The keyframe poses and pillar poses are obtained by least-squares optimization of formula (10) using the LM (Levenberg–Marquardt) algorithm.

[0107] The algorithm of this invention completes loop closure detection and global optimization, mitigating pose errors caused by accumulated drift and ensuring ground accuracy. Figure 1 Consistency. This completes the entire 3D semantic SLAM algorithm process.

[0108] To evaluate the effectiveness of this invention, it was tested on eight sequences collected in an underground parking garage scenario. This invention sets two evaluation metrics, one of which is the Absolute Trajectory Error (ATE), defined as follows:

[0109]

[0110] Where N is the total number of frames in the sequence. P represents the predicted pose of the i-th frame. i And pose truth value G i The deviation, Trans(·) represents taking the translation component, |||| 2 The squared value is used. Therefore, the above formula calculates the root mean squared error (RMSE) of the trajectory translation component. This metric reflects the absolute error of trajectory estimation, but the ATE error increases with the trajectory size. Therefore, this embodiment also calculates the relative translation error (RTE), an evaluation metric proposed by the KITTI dataset. Eight distance intervals [100, 200, ..., 800] meters were selected to calculate the relative pose error, and the average value was taken. The formula is as follows:

[0111]

[0112] Where i, j represent the indices of two frames separated by a fixed distance, length(i, j) represents this fixed distance, set to [100, 200, ..., 800] meters, κ represents the total number of κ pairs of selectable frames, Θ represents the calculation of the relative pose transformation between two frames, p j Θp i This represents the pose transformation from frame i to frame j. p represents the estimated and true pose values, respectively, and the final output is the percentage relative error.

[0113] This invention compares the feature point fusion IMU method VINS-Mono, ORB-SLAM3, the semantic and feature point fusion method Cube-SLAM, and the geospatial semantic SLAM method AVP-SLAM. Cube-SLAM obtains 3D bounding boxes of objects through hidden point sampling and fuses them with feature points for pose optimization. Since the accuracy of the 3D bounding boxes obtained through hidden point sampling is relatively low, to more fairly compare the accuracy differences caused by SLAM algorithms, this invention uses 3D bounding boxes of pillars detected by PGD and fuses them with feature points to run Cube-SLAM, with other settings remaining unchanged. Since there is no open-source code for AVP-SLAM, this invention reproduces it based on the implementation details disclosed in the paper.

[0114] The localization accuracy comparison results of various SLAM methods are shown in Tables 1 and 2. As can be seen from the tables, the method in this embodiment achieves higher accuracy in both ATE and RTE compared to the feature point method, semantic fusion feature point method, and ground semantic method. This is mainly because this embodiment utilizes stable 3D semantic information from the underground parking scene, is less affected by dynamic objects in the scene, and uses ground and various types of semantic information for association and optimization, greatly improving the algorithm's localization accuracy.

[0115] Table 1 shows the comparison results of ATE indices for various SLAM algorithms in an underground parking environment.

[0116]

[0117] As can be seen from Tables 1 and 2, the algorithm of this invention exhibits strong stability across the eight sequences, with absolute positioning accuracy ranging from 0.2 to 0.6 meters and relative positioning accuracy within 1.2%. In contrast, the positioning accuracy of feature-point based methods or ground semantic methods fluctuates significantly. The feature-point method lacks robustness in high-dynamic and weak-texture scenes, while the ground semantic method suffers from insufficient available semantic information, leading to a significant impact on positioning accuracy due to the richness of ground semantics.

[0118] Table 2 shows the comparison results of RTE indicators for various SLAM algorithms in the underground parking environment.

[0119]

[0120] Combining the localization results of various SLAM algorithms, it can be seen that the presence of loop closures in the sequence and the algorithm's ability to detect and optimize for loop closures have a significant impact on the final localization result. The SLAM-04 and SLAM-08 sequences do not contain loop closures, so Cube-SLAM and ORB-SLAM3 have larger errors on these two sequences. Therefore, once mismatches or deviations in scale recovery occur, the final trajectory will drift significantly. Figure 7 As shown, where Figure 7 (a) is a schematic diagram of ORB-SLAM3 feature point method localization. Figure 7 (b) is a schematic diagram of Cube-SLAM feature point fusion pillar localization. It can be seen that due to the lack of loop closure, the trajectories all experienced significant drift. The algorithm of this invention is as follows: Figure 7 As shown in (c), although there is no closed loop, the trajectory drift is small.

[0121] The comparison results of the ground semantic map and the 3D semantic map constructed by this invention are as follows: Figure 8 (a) and Figure 8 As shown in (b), it is clear that using 3D semantics can observe more feature information. At corners, even if ground semantics are sparse, there are 3D pillars that can serve as observation constraints to ensure positioning stability.

[0122] It can be seen that the present invention can effectively improve the accuracy and stability of SLAM in underground parking garage scenarios. Compared with traditional feature point-based SLAM methods and ground semantic-based SLAM algorithms, the SLAM method based on 3D semantic information in the present invention has significant improvements in accuracy and robustness.

[0123] The above embodiments are used to explain and illustrate the present invention, but not to limit the present invention. Any modifications and changes made to the present invention within the spirit and scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A visual SLAM method based on 3D semantics for a basement scene, characterized in that, Includes the following steps: 1) The front view and surround view are acquired by the mobile terminal. The front view and surround view are used to detect 3D columns, detect air traffic signs and perform semantic segmentation of ground signs, thereby obtaining 3D semantic information. In step 1), multiple fisheye images from multiple perspectives are acquired using multiple fisheye cameras. Then, the multiple fisheye images from multiple perspectives are projected onto the ground and stitched together to form a ring view. The ring view is then used to perform semantic segmentation of ground markings to obtain ground marking information. 2) The initial frame is used as a keyframe. Then, the initial pose of the current frame is calculated based on 3D semantic information. Keyframe determination is performed based on motion distance and motion angle. If the current frame is a keyframe, an initial local semantic map is constructed based on the initial frame and the current frame. Otherwise, the initial pose of the next frame is calculated until the current frame is a keyframe and the initial local semantic map is obtained. Then, the next frame is used as the current frame and 3) is executed. 3) Calculate the initial pose of the current frame based on 3D semantic information, and then optimize the initial pose of the current frame using the current local semantic map to obtain the optimized pose of the current frame. 4) Keyframe determination is performed based on motion distance and motion angle. If the current frame is a keyframe, the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain an updated local semantic map. Then, a semantic descriptor is constructed using the 3D semantic information of the current keyframe. Next, loop closure is determined using the semantic descriptor. If the current keyframe constitutes a loop, loop closure correction and global optimization are performed to obtain a global map. If the current frame is not a keyframe or does not constitute a loop, step 5) is executed directly. 5) Repeat steps 3)-4) to process the 3D semantic information of the next frame until a complete local semantic map is generated, thereby obtaining the global map; 6) Repeat steps 2)-5) to accumulate the local semantic maps and obtain the final global map.

2. The visual SLAM method based on 3D semantics in a basement scene according to claim 1, characterized in that, In step 1), a front view is acquired using a front-view camera, and 3D pillars and air traffic signs are detected using the front view to obtain 3D pillar information and air traffic sign information respectively.

3. The visual SLAM method based on 3D semantics in a basement scene according to claim 1, characterized in that, In step 2) or 3), calculating the initial pose of the current frame based on 3D semantic information specifically involves: After matching the 3D semantic information of the current frame with the 3D semantic information of the previous frame, the inter-frame pose is obtained, and then the inter-frame pose is used to calculate the initial pose of the current frame.

4. A visual SLAM method based on 3D semantics in a basement scene according to claim 3, characterized in that, The step of matching the 3D semantic information of the current frame with the 3D semantic information of the previous frame to obtain the inter-frame pose is as follows: First, the nearest neighbor search is used to match the 3D pillars of the current frame with the previous frame. Then, the initial inter-frame pose is calculated using the matched 3D pillars. Next, 3D-3D matching is performed using the 3D semantic points of air traffic signs and ground signs to optimize the initial inter-frame pose, thereby obtaining the final inter-frame pose.

5. A visual SLAM method based on 3D semantics in a basement scene according to claim 1, characterized in that, In step 3), after optimizing the initial pose of the current frame using the current local semantic map, the optimized pose of the current frame is obtained, specifically as follows: The 3D semantic information of the current frame is projected onto the local coordinate system, and then the 3D semantic information of the current frame is matched using the current local semantic map to obtain the optimized pose of the current frame.

6. A visual SLAM method based on 3D semantics in a basement scene according to claim 1, characterized in that, In step 4), the 3D semantic information of the current frame is fused and updated with the current local semantic map to obtain the updated local semantic map, specifically as follows: If there are 3D pillars in the current frame's 3D semantic information that are associated with the current local semantic map, then update the association count of the corresponding pillar in the map, and recalculate the center point position and the mean yaw angle of the associated pillars based on the current frame's 3D semantic information. Then, update the center point position and yaw angle of the associated pillars in the local semantic map according to the new center point position and the mean yaw angle. If there are no associated pillars, then create a new 3D pillar in the map based on the current frame's 3D semantic information to obtain the updated local semantic map.

7. A visual SLAM method based on 3D semantics for a basement scene according to claim 1, characterized in that, In step 4), the 3D semantic information is used to construct a semantic descriptor, specifically as follows: The semantic 3D points of the current frame are divided into blocks, and the semantic tags with the highest frequency in each block are used as the features of that block to construct a description matrix, which is then used as a semantic descriptor.

8. A storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the method described in any one of claims 1 to 7.

9. A storage medium according to claim 8, characterized in that, The computer program thereunder is an instruction that implements the method of any one of claims 1 to 7.

Images