Semantic map-based sparse feature closed environment visual relocation method

Abstract

The invention discloses a sparse feature closed environment visual relocation method based on a semantic map. The method comprises the following steps: constructing a dense semantic three-dimensional map by combining semantic information of an input image with a traditional visual SLAM (Simultaneous Localization and Mapping) mapping thread; extracting parameters of an object of interest according to the semantic map to form a node-edge structure, and then completely extracting a semantic corresponding relation of the relocated object through an undirected graph sub-graph matching algorithm; and an accurate relocation result is obtained by combining ICP point cloud matching. According to the method, the semantic segmentation and the visual odometer are fused for mapping, so that the method has universality and high expandability, meanwhile, the repositioning robustness in complex scenes such as a similar structure environment and a sparse feature environment is improved, the CPU computing burden is remarkably reduced, and deployment in computing equipment is facilitated.

Description

Technical Field

[0001] This invention relates to the field of SLAM visual relocalization for unmanned systems, and specifically to a visual relocalization method for closed environments based on sparse features of semantic maps. Background Technology

[0002] Visual Simultaneous Localization and Mapping (SLAM) technology is a core support for fields such as autonomous driving, robot navigation, and augmented reality. Traditional visual SLAM technology mainly relies on sparse geometric feature points for localization and mapping. While it performs well in ideal environments with stable lighting, it faces significant challenges in long-term, large-scale, and complex real-world applications. These challenges include: sensitivity to drastic changes in lighting, seasonal changes, and scene perspective changes, leading to decreased feature matching success rates and localization loss; and in scenes with repetitive or sparse textures (such as long corridors), loop closure detection methods based on appearance descriptors are prone to mismatches, resulting in relocalization failures. Furthermore, maps constructed by traditional SLAM systems are mostly geometric point clouds, lacking high-level semantic information, which limits the robot's environmental understanding and intelligent task execution capabilities. Summary of the Invention

[0003] The purpose of this invention is to provide a visual relocalization method for closed environments based on sparse features of semantic maps.

[0004] The technical solution to achieve the purpose of this invention is: a visual relocalization method for closed environments based on sparse features of semantic maps, comprising the following steps:

[0005] Step 1: Acquire RGB image data streams and corresponding depth image data streams of sparse feature enclosed environment data using a visual sensor. Perform Gaussian filtering on the depth image data stream to obtain a filtered depth image data stream. Perform timestamp synchronization verification on the RGB data stream and depth map data stream to construct an RGBD image stream in units of frames. Use the RGBD image stream as input to the ORB-SLAM algorithm, select the ORB-SLAM mode as RGBD mode, and obtain the visual odometry estimation results, including keyframes, keyframe poses, and the corresponding RGBD images of the keyframes.

[0006] Step 2: Train a PyTorch PSPNet image semantic segmentation convolutional neural network on the ADE20k dataset; convert the training weights of PyTorch PSPNet to .trt format using TensorRT; modify the ORB-SLAM algorithm system by adding a semantic map construction thread, input the RGB images of keyframes obtained in Step 1 through visual odometry into the PSPNet network deployed by TensorRT for semantic segmentation, and obtain the corresponding semantic segmentation images; construct a 3D dense semantic point cloud map based on the keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor.

[0007] Step 3: In the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filter out special semantic point clouds of interest and cluster them into individual semantic object point clouds; for each individual semantic object point cloud, the feature information of the object is extracted, including the semantic category of the object, the coordinate position of the center of the object point cloud, the radius of the circumcircle of the object point cloud, and the normal vector of the object point cloud set; each object is used as a node of the undirected graph, the distance between the center points of the object is used as the edge of the undirected graph, and the feature information of the object is used as the node attribute of the undirected graph to construct a global undirected graph of 3D semantic abstraction, and the graph structure information is written to a file for saving;

[0008] Step 4: For the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph through the improved VF2 matching algorithm of subgraph isomorphism.

[0009] Step 5: The matching relationship information obtained in Step 4 is solved using the RANSAC algorithm and SVD to obtain the initial transformation matrix between the global map and the local map, which is the preliminary visual relocalization result. The initial transformation matrix is ​​optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained.

[0010] Further, in step 2, a PyTorch PSPNet image semantic segmentation convolutional neural network is trained on the ADE20k dataset; the training weights of PyTorch PSPNet are converted to TRT format using TensorRT; the ORB-SLAM algorithm system is modified by adding a semantic map construction thread, and the RGB images of keyframes obtained by visual odometry in step 1 are input into the PSPNet network deployed by TensorRT for semantic segmentation to obtain the corresponding semantic segmentation images; based on the keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor, a 3D dense semantic point cloud map is constructed, specifically as follows:

[0011] Step 2.1, Semantic segmentation network structure adjustment and training: Replace the feedforward network in the PyTorch PSPNet image semantic segmentation convolutional neural network with MobileNet, and uniformly specify the input and output image size of the network as 512 pixels × 512 pixels; use the ADE20k dataset to train the adjusted network to obtain the corresponding network training weights;

[0012] Step 2.2, Network weight format conversion and deployment optimization: Convert the PyTorch weight files obtained from training into the trt format suitable for efficient deployment through the TensorRT inference framework; during the conversion process, specify the weight quantization method as FP16 quantization, and specify the network input data format as 1×3×512×512 and the output data format as 512×512×1;

[0013] Step 2.3, Semantic Map Construction Thread Integration: Modify the system framework of the ORB-SLAM algorithm to integrate an independent semantic map construction thread. Specifically, start this thread in the ORB-SLAM System file and create corresponding thread management files PointClouding.cpp and PointClouding.h to implement the map construction logic of steps 2 to 3. In the thread, the semantic segmentation network optimized by TensorRT is called to perform inference on the keyframe RGB image to obtain the semantic segmentation image.

[0014] Step 2.4, Generation of 3D Dense Semantic Point Cloud Map: Based on the keyframe poses, RGBD images obtained in Step 1, the semantic segmentation images obtained in Step 2.3, and the intrinsic parameters of the visual sensor, the map is constructed by calculating and fusing point by point using the following projection formula, thereby constructing a 3D dense semantic point cloud map with semantic labels.

[0015]

[0016] in The coordinates of a single point in the generated point cloud map. These are the pixel coordinates of the point in the image. For intrinsic parameters of the vision sensor, This represents the depth value of the corresponding depth image.

[0017] Further, in step 3, within the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filtering out special semantic point clouds of interest and clustering them into individual semantic object point clouds. For each individual semantic object point cloud, the feature information of the object is extracted, including the object's semantic category, the coordinates of the object's point cloud center, the radius of the object's point cloud's circumcircle, and the normal vector of the object's point cloud set. Each individual object is used as a node in an undirected graph, the distance between object center points is used as an edge in the undirected graph, and the object's feature information is used as the node attributes of the undirected graph. This constructs a global undirected graph of 3D semantic abstraction, and the graph structure information is written to a file for saving. The specific method is as follows:

[0018] Step 3.1, Point Cloud Preprocessing: For the 3D dense semantic point cloud map, firstly, voxel filters in the Point Cloud Library (PCL) are used for downsampling to reduce computation; then, statistical outlier removal (SOR) filters are used to remove outliers, where the threshold for the number of nearest neighbors used to determine whether a point is an outlier is set to 500; based on the semantic segmentation results, point cloud data with specified semantic labels of interest are selected.

[0019] Step 3.2, Object point cloud clustering and segmentation: For the filtered single semantic point cloud, the density-based spatial clustering (DBSCAN) algorithm is used to cluster the points that are spatially close to each other into independent point cloud clusters. Each point cloud cluster is regarded as an independent semantic object.

[0020] Step 3.3, Object Feature Information Extraction: For each independent semantic object point cloud cluster, extract the following feature information:

[0021] (1) Coordinates of the center of the object's point cloud

[0022]

[0023] in Represents the center point of the object's point cloud. express coordinates This indicates the total number of points in the object's point cloud. Three-dimensional spatial coordinates representing each point;

[0024] (2) Radius of the circumcircle of the object's point cloud

[0025]

[0026] (3) Normal vector of the object point cloud set

[0027] Calculate the covariance matrix of point clouds ;

[0028]

[0029] Perform eigenvalue decomposition on the covariance matrix to obtain the corresponding eigenvectors. After normalization, it becomes the normal vector of the object point cloud set;

[0030] Step 3.4, Global Undirected Graph Construction: Each extracted independent semantic object is used as a node in the graph; the Euclidean distance between the center points of any two objects is calculated. If the distance is less than a preset distance threshold, an undirected edge is established between the two nodes, and the weight of the edge is the distance between the two center points; the attributes of each node are composed of its corresponding object feature information; finally, all nodes, edges and node attributes together constitute a global undirected graph describing the semantic topology of the environment, and it is stored in a file.

[0031] Further, in step 4, for the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph using the improved VF2 matching algorithm based on subgraph isomorphism. The specific method is as follows:

[0032] Step 4.1, Local Undirected Graph Construction: Construct a local undirected graph based on the RGBD data of the current frame or consecutive frames to be relocated; if the input is single-frame data, directly perform point cloud processing and feature extraction steps; if the input is a consecutive frame sequence, repeat steps 1 to 3 to construct the local undirected graph.

[0033] Step 4.2, Matching Priority Setting: When extracting object information from a local 3D dense semantic point cloud map, the matching priority is set according to the quality of the extracted point cloud clusters; the priority order is as follows:

[0034] (a) Boundary integrity: Point cloud clusters with complete and clear boundaries have a higher matching priority;

[0035] (b) Point cloud object radius: Under the condition of similar boundary sharpness, point cloud clusters with smaller semantic object radii are matched first;

[0036] That is, small semantic objects with clear and obvious boundaries have a higher matching priority than other objects with incomplete boundaries in the local map;

[0037] Step 4.3, Subgraph Isomorphic Matching Conditions: Based on the VF2 subgraph isomorphic matching algorithm, calculate the matching relationships between nodes and edges between the local undirected graph and the global undirected graph; the improved VF2 algorithm defines the following matching conditions and procedures:

[0038] Matching conditions include:

[0039] (a) For each node in the local graph, there must be a unique corresponding node in the global graph;

[0040] (b) Different nodes in the local graph have different corresponding nodes in the global graph;

[0041] (c) For every edge in the local graph, there must be an edge in the global graph, and the nodes at both ends of the edge must correspond one-to-one with the nodes at both ends of the edge in the local graph.

[0042] (d) All matching nodes must have the same semantic category label;

[0043] (e) The length error of all matching edges must be within a preset threshold range;

[0044] The matching process includes:

[0045] (a) Initialization: Start from an empty match state;

[0046] (b) Candidate generation: In the current state, if the current matching state is not equal to the subgraph that needs to be matched, that is, the matched subgraph is not completed, then a set of candidate node pairs is generated;

[0047] (c) Feasibility check and recursive search: For each candidate pair in the candidate set, add the currently calculated candidate pair to the current matching state and check whether the matching conditions are met. If they are met, enter the new matching state and recursively call the matching process (a)-(c); if they are not met, cancel the operation of adding the candidate pair to the matching and try the next candidate pair.

[0048] (d) Termination condition: All candidate pairs have been tried, and the current matching status is output as the result;

[0049] Step 4.4, matching result selection: perform incomplete graph matching based on matching priority; if the number of candidate matching groups obtained from the initial matching exceeds the set number, then the matching groups containing low-priority nodes and their associated edges are removed; finally, if the proportion of successfully matched vertices and edges in a certain matching group exceeds the set threshold, then the group is determined to be a valid coarse matching.

[0050] Further, in step 5, the matching relationship information obtained in step 4 is used to solve for the initial transformation matrix between the global map and the local map using the RANSAC algorithm and SVD, which is the preliminary visual relocalization result. The initial transformation matrix is ​​then optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained. The specific method is as follows:

[0051] Step 5.1, Solving for the initial transformation matrix: Based on the matching relationship obtained in Step 4, the initial transformation matrix between the global undirected graph and the local undirected graph is solved using the Random Sample Consensus (RANSAC) algorithm and Singular Value Decomposition (SVD).

[0052] Step 5.2, RANSAC Iteration Process: Randomly select multiple sets of matching samples from the matching relationships, each set containing at least 3 pairs of matching center points and normal vectors;

[0053] The candidate rotation matrix and translation vector for each group of samples are solved by SVD.

[0054] Calculate the reprojection error of all matching points based on the candidate transformation matrix, and determine the points with errors less than a set threshold as interior points to form a consensus set.

[0055] The above steps are executed iteratively, and the candidate transformation matrix with the largest consensus set is retained as the optimal initial transformation matrix;

[0056] Step 5.3, Iteration Termination Condition: The RANSAC iteration terminates when one of the following conditions is met:

[0057] (a) The proportion of the number of points within the consensus set to the total number of matching points exceeds a preset threshold;

[0058] (b) The number of iterations reaches the preset maximum number of iterations;

[0059] Step 5.4, ICP Fine Registration Optimization: Using the optimal initial transformation matrix as the initial value for the Iterative Closest Point (ICP) registration algorithm, fine registration is performed on the global map and the local cloud map, including:

[0060] For each point in the transformed local point cloud, search for the corresponding point with the closest Euclidean distance in the global point cloud and establish a point pair relationship;

[0061] Based on the point-pair relationship, calculate the rigid transformation matrix that minimizes the mean square error between all point pairs.

[0062] Iteratively update the point cloud position and transformation matrix until the mean square error is lower than the set threshold or the maximum number of iterations is reached, and output the final accurate transformation matrix as the visual relocalization result.

[0063] A visual relocalization system for enclosed environments based on semantic maps with sparse features includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the visual relocalization method for enclosed environments based on semantic maps with sparse features.

[0064] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the aforementioned visual relocalization method for sparse features in enclosed environments based on semantic maps.

[0065] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned visual relocalization method for closed environments based on sparse features of semantic maps.

[0066] Compared with the prior art, the significant advantages of this invention are: (1) It uses a semantic segmentation neural network combined with SLAM mapping to make full use of environmental semantic information and avoids excessive neglect of map information; (2) It uses a semantic map for matching and relocation, avoiding the problem of relocation error or failure to achieve relocation in the traditional SLAM algorithm in a sparse feature closed environment; (3) Its innovation lies in combining graph theory mathematical tools and semantic information, and using the subgraph isomorphism VF2 algorithm to improve the stability and scalability of the matching algorithm; (4) Compared with the traditional SLAM relocation algorithm, the improved algorithm improves the relocation accuracy by introducing semantic information, while ensuring the robustness of the algorithm model, and has high practical value. Attached Figure Description

[0067] Figure 1 This is a flowchart of the overall process for a visual relocalization method for closed environments based on sparse features of semantic maps.

[0068] Figure 2 This is a test of the error distribution of a sparse feature-based visual relocalization method for closed environments based on semantic maps on a dataset.

[0069] Figure 3 This is a schematic diagram illustrating the relocalization results of a sparse feature-based visual relocalization method for closed environments, tested on a dataset. Detailed Implementation

[0070] This invention provides a visual relocalization method for enclosed environments based on sparse features using semantic maps, such as... Figure 1 As shown, the process includes the following steps: 1. ORB-SLAM to establish a visual odometry; 2. PSPNet combined with the visual odometry to establish a 3D semantic map; 3. Construction of 3D undirected graph elements; 4. Improved VF2 subgraph isomorphic matching algorithm; 5. RANSAC and ICP to complete accurate relocalization.

[0071] The design and implementation of each step are described below:

[0072] Step 1: Acquire RGB image data streams and corresponding depth image data streams of sparse feature enclosed environment data using a visual sensor. Perform Gaussian filtering on the depth image data stream to obtain a filtered depth image data stream. Perform timestamp synchronization verification on the RGB data stream and depth map data stream to construct an RGBD image stream in units of frames. Use the RGBD image stream as input to the ORB-SLAM algorithm, select the ORB-SLAM mode as RGBD mode, and obtain the visual odometry estimation results, including keyframes, keyframe poses, and the corresponding RGBD images of the keyframes.

[0073] Step 2: Train a PyTorch PSPNet image semantic segmentation convolutional neural network on the ADE20k dataset; convert the training weights of PyTorch PSPNet to .trt format using TensorRT; modify the ORB-SLAM algorithm system by adding a semantic map construction thread, input the RGB images of keyframes obtained in Step 1 through visual odometry into the PSPNet network deployed by TensorRT for semantic segmentation, and obtain the corresponding semantic segmentation images; construct a 3D dense semantic point cloud map based on the keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor.

[0074] Step 3: In the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filter out special semantic point clouds of interest and cluster them into individual semantic object point clouds; for each individual semantic object point cloud, the feature information of the object is extracted, including the semantic category of the object, the coordinate position of the center of the object point cloud, the radius of the circumcircle of the object point cloud, and the normal vector of the object point cloud set; each object is used as a node of the undirected graph, the distance between the center points of the object is used as the edge of the undirected graph, and the feature information of the object is used as the node attribute of the undirected graph to construct a global undirected graph of 3D semantic abstraction, and the graph structure information is written to a file for saving;

[0075] Step 4: For the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph through the improved VF2 matching algorithm of subgraph isomorphism.

[0076] Step 5: The matching relationship information obtained in Step 4 is used to solve the initial transformation matrix between the global map and the local map using the RANSAC algorithm and SVD, which is the preliminary visual relocalization result. The initial transformation matrix is ​​optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained.

[0077] Further, in step 2, a PyTorch PSPNet image semantic segmentation convolutional neural network is trained on the ADE20k dataset; the training weights of PyTorch PSPNet are converted to TRT format using TensorRT; the ORB-SLAM algorithm system is modified by adding a semantic map construction thread, and the RGB images of keyframes obtained by visual odometry in step 1 are input into the PSPNet network deployed by TensorRT for semantic segmentation to obtain the corresponding semantic segmentation images; based on the keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor, a 3D dense semantic point cloud map is constructed, specifically as follows:

[0078] Step 2.1, Semantic segmentation network structure adjustment and training: Replace the feedforward network in the PyTorch PSPNet image semantic segmentation convolutional neural network with MobileNet, and uniformly specify the input and output image size of the network as 512 pixels × 512 pixels; use the ADE20k dataset to train the adjusted network to obtain the corresponding network training weights;

[0079] Step 2.2, Network weight format conversion and deployment optimization: Convert the PyTorch weight files obtained from training into the trt format suitable for efficient deployment through the TensorRT inference framework; during the conversion process, specify the weight quantization method as FP16 quantization, and specify the network input data format as 1×3×512×512 and the output data format as 512×512×1;

[0080] Step 2.3, Semantic Map Construction Thread Integration: Modify the system framework of the ORB-SLAM algorithm to integrate an independent semantic map construction thread. Specifically, start this thread in the ORB-SLAM System file and create corresponding thread management files PointClouding.cpp and PointClouding.h to implement the map construction logic of steps 2 to 3. In the thread, the semantic segmentation network optimized by TensorRT is called to perform inference on the keyframe RGB image to obtain the semantic segmentation image.

[0081] Step 2.4, Generation of 3D Dense Semantic Point Cloud Map: Based on the keyframe poses, RGBD images obtained in Step 1, the semantic segmentation images obtained in Step 2.3, and the intrinsic parameters of the visual sensor, the map is constructed by calculating and fusing point by point using the following projection formula, thereby constructing a 3D dense semantic point cloud map with semantic labels.

[0082]

[0083] in The coordinates of a single point in the generated point cloud map. These are the pixel coordinates of the point in the image. For intrinsic parameters of the vision sensor, This represents the depth value of the corresponding depth image.

[0084] Further, in step 3, within the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filtering out special semantic point clouds of interest and clustering them into individual semantic object point clouds. For each individual semantic object point cloud, the feature information of the object is extracted, including the object's semantic category, the coordinates of the object's point cloud center, the radius of the object's point cloud's circumcircle, and the normal vector of the object's point cloud set. Each individual object is used as a node in an undirected graph, the distance between object center points is used as an edge in the undirected graph, and the object's feature information is used as the node attributes of the undirected graph. This constructs a global undirected graph of 3D semantic abstraction, and the graph structure information is written to a file for saving. The specific method is as follows:

[0085] Step 3.1, Point Cloud Preprocessing: For the 3D dense semantic point cloud map, firstly, voxel filters in the Point Cloud Library (PCL) are used for downsampling to reduce computation; then, statistical outlier removal (SOR) filters are used to remove outliers, where the threshold for the number of nearest neighbors used to determine whether a point is an outlier is set to 500; based on the semantic segmentation results, point cloud data with specified semantic labels of interest are selected.

[0086] Step 3.2, Object point cloud clustering and segmentation: For the filtered single semantic point cloud, the density-based spatial clustering (DBSCAN) algorithm is used to cluster the points that are spatially close to each other into independent point cloud clusters. Each point cloud cluster is regarded as an independent semantic object.

[0087] Step 3.3, Object Feature Information Extraction: For each independent semantic object point cloud cluster, extract the following feature information:

[0088] (1) Coordinates of the center of the object's point cloud

[0089]

[0090] in Represents the center point of the object's point cloud. express coordinates This indicates the total number of points in the object's point cloud. Three-dimensional spatial coordinates representing each point;

[0091] (2) Radius of the circumcircle of the object's point cloud

[0092]

[0093] (3) Normal vector of the object point cloud set

[0094] Calculate the covariance matrix of point clouds ;

[0095]

[0096] Perform eigenvalue decomposition on the covariance matrix to obtain the corresponding eigenvectors. After normalization, it becomes the normal vector of the object point cloud set;

[0097] Step 3.4, Global Undirected Graph Construction: Each extracted independent semantic object is used as a node in the graph; the Euclidean distance between the center points of any two objects is calculated. If the distance is less than a preset distance threshold, an undirected edge is established between the two nodes, and the weight of the edge is the distance between the two center points; the attributes of each node are composed of its corresponding object feature information; finally, all nodes, edges and node attributes together constitute a global undirected graph describing the semantic topology of the environment, and it is stored in a file.

[0098] Further, in step 3, within the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filtering out special semantic point clouds of interest and clustering them into individual semantic object point clouds. For each individual semantic object point cloud, the feature information of the object is extracted, including the object's semantic category, the coordinates of the object's point cloud center, the radius of the object's point cloud's circumcircle, and the normal vector of the object's point cloud set. Each individual object is used as a node in an undirected graph, the distance between object center points is used as an edge in the undirected graph, and the object's feature information is used as the node attributes of the undirected graph. This constructs a global undirected graph of 3D semantic abstraction, and the graph structure information is written to a file for saving. The specific method is as follows:

[0099] Step 3.1, Point Cloud Preprocessing: For the 3D dense semantic point cloud map, firstly, voxel filters in the Point Cloud Library (PCL) are used for downsampling to reduce computation; then, statistical outlier removal (SOR) filters are used to remove outliers, where the threshold for the number of nearest neighbors used to determine whether a point is an outlier is set to 500; based on the semantic segmentation results, point cloud data with specified semantic labels of interest are selected.

[0100] Step 3.2, Object point cloud clustering and segmentation: For the filtered single semantic point cloud, the density-based spatial clustering (DBSCAN) algorithm is used to cluster the points that are spatially close to each other into independent point cloud clusters. Each point cloud cluster is regarded as an independent semantic object.

[0101] Step 3.3, Object Feature Information Extraction: For each independent semantic object point cloud cluster, extract the following feature information:

[0102] (1) Coordinates of the center of the object's point cloud

[0103]

[0104] in Represents the center point of the object's point cloud. express coordinates This indicates the total number of points in the object's point cloud. Three-dimensional spatial coordinates representing each point;

[0105] (2) Radius of the circumcircle of the object's point cloud

[0106]

[0107] (3) Normal vector of the object point cloud set

[0108] Calculate the covariance matrix of point clouds ;

[0109]

[0110] Perform eigenvalue decomposition on the covariance matrix to obtain the corresponding eigenvectors. After normalization, it becomes the normal vector of the object point cloud set;

[0111] Step 3.4, Global Undirected Graph Construction: Each extracted independent semantic object is used as a node in the graph; the Euclidean distance between the center points of any two objects is calculated. If the distance is less than a preset distance threshold, an undirected edge is established between the two nodes, and the weight of the edge is the distance between the two center points; the attributes of each node are composed of its corresponding object feature information; finally, all nodes, edges and node attributes together constitute a global undirected graph describing the semantic topology of the environment, and it is stored in a file.

[0112] Further, in step 4, for the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph using the improved VF2 matching algorithm based on subgraph isomorphism. The specific method is as follows:

[0113] Step 4.1, Local Undirected Graph Construction: Construct a local undirected graph based on the RGBD data of the current frame or consecutive frames to be relocated; if the input is single-frame data, directly perform point cloud processing and feature extraction steps; if the input is a consecutive frame sequence, repeat steps 1 to 3 to construct the local undirected graph.

[0114] Step 4.2, Matching Priority Setting: When extracting object information from a local 3D dense semantic point cloud map, the matching priority is set according to the quality of the extracted point cloud clusters; the priority order is as follows:

[0115] (a) Boundary integrity: Point cloud clusters with complete and clear boundaries have a higher matching priority;

[0116] (b) Point cloud object radius: Under the condition of similar boundary sharpness, point cloud clusters with smaller semantic object radii are matched first;

[0117] That is, small semantic objects with clear and obvious boundaries have a higher matching priority than other objects with incomplete boundaries in the local map;

[0118] Step 4.3, Subgraph Isomorphic Matching Conditions: Based on the VF2 subgraph isomorphic matching algorithm, calculate the matching relationships between nodes and edges between the local undirected graph and the global undirected graph; the improved VF2 algorithm defines the following matching conditions and procedures:

[0119] Matching conditions include:

[0120] (a) For each node in the local graph, there must be a unique corresponding node in the global graph;

[0121] (b) Different nodes in the local graph have different corresponding nodes in the global graph;

[0122] (c) For every edge in the local graph, there must be an edge in the global graph, and the nodes at both ends of the edge must correspond one-to-one with the nodes at both ends of the edge in the local graph.

[0123] (d) All matching nodes must have the same semantic category label;

[0124] (e) The length error of all matching edges must be within a preset threshold range;

[0125] The matching process includes:

[0126] (a) Initialization: Start from an empty match state;

[0127] (b) Candidate generation: In the current state, if the current matching state is not equal to the subgraph that needs to be matched, that is, the matched subgraph is not completed, then a set of candidate node pairs is generated;

[0128] (c) Feasibility check and recursive search: For each candidate pair in the candidate set, add the currently calculated candidate pair to the current matching state and check whether the matching conditions are met. If they are met, enter the new matching state and recursively call the matching process (a)-(c); if they are not met, cancel the operation of adding the candidate pair to the matching and try the next candidate pair.

[0129] (d) Termination condition: All candidate pairs have been tried, and the current matching status is output as the result;

[0130] Step 4.4, matching result selection: perform incomplete graph matching based on matching priority; if the number of candidate matching groups obtained from the initial matching exceeds the set number, then the matching groups containing low-priority nodes and their associated edges are removed; finally, if the proportion of successfully matched vertices and edges in a certain matching group exceeds the set threshold, then the group is determined to be a valid coarse matching.

[0131] Step 4.5, Selection of matching results: Perform incomplete graph matching based on matching priority; if the number of candidate matching groups obtained from the initial matching exceeds 10, then the matching groups containing low-priority nodes and their associated edges are removed; finally, if the proportion of successfully matched vertices and edges in a certain matching group exceeds 80%, then the group is determined to be a valid coarse matching.

[0132] Further, in step 5, the matching relationship information obtained in step 4 is used to solve for the initial transformation matrix between the global map and the local map using the RANSAC algorithm and SVD, which is the preliminary visual relocalization result. The initial transformation matrix is ​​then optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained. The specific method is as follows:

[0133] Step 5.1, Solving for the initial transformation matrix: Based on the matching relationship obtained in Step 4, the initial transformation matrix between the global undirected graph and the local undirected graph is solved using the Random Sample Consensus (RANSAC) algorithm and Singular Value Decomposition (SVD).

[0134] Step 5.2, RANSAC iterative process: Randomly select multiple sets of matching samples from the matching relationship, each set containing at least 3 pairs of matching center points and normal vectors;

[0135] The candidate rotation matrix and translation vector for each group of samples are solved by SVD.

[0136] The reprojection error of all matching points is calculated based on the candidate transformation matrix. Points with translation error less than the threshold of 0.25m or rotation cosine error less than 0.1 are classified as interior points to form a consensus set.

[0137] The above steps are executed iteratively, and the candidate transformation matrix with the largest consensus set is retained as the optimal initial transformation matrix;

[0138] Step 5.3, Iteration Termination Condition: The RANSAC iteration terminates when one of the following conditions is met:

[0139] (a) The consensus set size exceeds the preset 80% node threshold;

[0140] (b) The number of iterations reaches the maximum number of iterations, 1000;

[0141] Step 5.4, ICP Fine Registration Optimization: Using the optimal initial transformation matrix as the initial value for the Iterative Closest Point (ICP) registration algorithm, fine registration is performed on the global map and the local cloud map, including:

[0142] For each point in the transformed local point cloud, search for the corresponding point with the closest Euclidean distance in the global point cloud and establish a point pair relationship;

[0143] Based on the point-pair relationship, calculate the rigid transformation matrix that minimizes the mean square error between all point pairs.

[0144] Iteratively update the point cloud position and transformation matrix until the mean square error is less than 0.05 meters or the maximum number of iterations of 1000 is reached, and output the final accurate transformation matrix as the visual relocalization result.

[0145] Following the steps outlined above, the relocalization results were compared with those of traditional algorithms using the TUM dataset, the 7scence-office dataset, and a sparse feature long corridor environment test dataset. The results are shown in Table 1.

[0146] Table 1. Comparison results of relocation error data algorithms under three different datasets.

[0147]

[0148] Figure 3 The computational visualization results of this algorithm are presented. The relocalization results show that the semantic segmentation error of the local map has a relatively small impact on the overall relocalization result. The semantic segmentation of the left window in the image suffers from some issues with point cloud classification due to reflections in the window glass and errors in camera distance calculation. However, the final relocalization still aligns well with the window object on the global map. Figure 2The relocalization accuracy distribution of this algorithm on three datasets is described. It can be seen that this algorithm achieves good global relocalization performance in environments with high similarity. Approximately 50% of the keyframe sequences have localization errors within the range of 0.1-0.2m, and approximately 80% of the keyframe sequences have localization errors less than 0.2m, demonstrating the good relocalization performance of this algorithm in environments with high similarity, sparse features, and large camera sway. For the TUM dataset and the 7-Scenes dataset, the results of this algorithm demonstrate that it still has good relocalization performance in open indoor environments and densely featured indoor environments. Table 1 describes the comparison of the relocalization accuracy of this algorithm with ORB-SLAM2 and PoseNet2 on the three datasets. Finally, the localization distance error of this algorithm is 15.41cm, and the angle error is 12.27°. The reason for the large error between ORB-SLAM and this method on the 7-Scenes dataset is that the RGB images and depth images of the dataset are not strictly aligned, leading to localization errors in the RGBD algorithm due to the misalignment of the depth map. The comparison shows that the relocalization accuracy of this algorithm is better than PoseNet2 in a large indoor environment. In ordinary environments (TUM and 7-scene datasets), the localization accuracy is slightly weaker than ORB-SLAM. However, in special environments, it solves the problems of large rotation error and inaccurate localization in environments with similar sparse features that ORB-SLAM has.

[0149] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0150] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these modifications and improvements all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A visual relocalization method for enclosed environments based on sparse features of semantic maps, characterized in that, Includes the following steps: Step 1: Acquire RGB image data streams and corresponding depth image data streams of sparse feature enclosed environment data through a visual sensor; perform Gaussian filtering on the depth image data streams to obtain filtered depth image data streams; and perform timestamp synchronization verification on the RGB data streams and depth map data streams to construct RGBD image streams in units of frames. Using the RGBD image stream as input to the ORB-SLAM algorithm, and selecting the RGBD mode as the ORB-SLAM mode, the visual odometry estimation results are obtained, including keyframes, keyframe poses, and the corresponding RGBD images of the keyframes. Step 2: Train a PyTorch PSPNet image semantic segmentation convolutional neural network on the ADE20k dataset; convert the training weights of PyTorch PSPNet to .trt format using TensorRT; modify the ORB-SLAM algorithm system by adding a semantic map construction thread, and input the RGB images of the keyframes obtained by visual odometry in Step 1 into the PSPNet network deployed by TensorRT for semantic segmentation to obtain the corresponding semantic segmentation image. A 3D dense semantic point cloud map is constructed based on keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor. Step 3: In the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filter out special semantic point clouds of interest and cluster them into individual semantic object point clouds; for a single semantic object point cloud, the feature information of the object is extracted, including the semantic category of the object, the center coordinate position of the object point cloud, the radius of the circumcircle of the object point cloud, and the normal vector of the object point cloud set. By treating individual objects as nodes of an undirected graph, the distance between object center points as edges of the undirected graph, and object feature information as node attributes of the undirected graph, a global undirected graph with three-dimensional semantic abstraction is constructed, and the graph structure information is written to a file for saving. Step 4: For the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph through the improved VF2 matching algorithm of subgraph isomorphism. Step 5: The matching relationship information obtained in Step 4 is solved using the RANSAC algorithm and SVD to obtain the initial transformation matrix between the global map and the local map, which is the preliminary visual relocalization result. The initial transformation matrix is ​​optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained.

2. The visual relocalization method for sparse feature-based enclosed environments based on semantic maps according to claim 1, characterized in that, Step 2: Train a PyTorch PSPNet image semantic segmentation convolutional neural network on the ADE20k dataset; convert the training weights of PyTorch PSPNet to TRT format using TensorRT; modify the ORB-SLAM algorithm system by adding a semantic map construction thread, inputting the RGB images of keyframes obtained from visual odometry in Step 1 into the PSPNet network deployed with TensorRT for semantic segmentation, obtaining the corresponding semantic segmentation images; construct a 3D dense semantic point cloud map based on keyframes, keyframe poses, keyframe RGBD images, keyframe semantic segmentation images, and intrinsic parameters of the visual sensor. The specific method is as follows: Step 2.1, Semantic segmentation network structure adjustment and training: Replace the feedforward network in the PyTorch PSPNet image semantic segmentation convolutional neural network with MobileNet, and uniformly specify the input and output image size of the network as 512 pixels × 512 pixels; use the ADE20k dataset to train the adjusted network to obtain the corresponding network training weights; Step 2.2, Network weight format conversion and deployment optimization: Convert the PyTorch weight files obtained from training into the trt format suitable for efficient deployment using the TensorRT inference framework; During the conversion process, the quantization method of the weights is specified as FP16 quantization, and the input data format of the network is specified as 1×3×512×512, and the output data format is specified as 512×512×1. Step 2.3, Semantic Map Construction Thread Integration: Modify the system framework of the ORB-SLAM algorithm to integrate an independent semantic map construction thread. Specifically, start this thread in the ORB-SLAM System file and create corresponding thread management files PointClouding.cpp and PointClouding.h to implement the map construction logic of steps 2 to 3. In the thread, the semantic segmentation network optimized by TensorRT is called to perform inference on the keyframe RGB image to obtain the semantic segmentation image. Step 2.4, Generation of 3D Dense Semantic Point Cloud Map: Based on the keyframe poses, RGBD images obtained in Step 1, the semantic segmentation images obtained in Step 2.3, and the intrinsic parameters of the visual sensor, the map is constructed by calculating and fusing point by point using the following projection formula, thereby constructing a 3D dense semantic point cloud map with semantic labels. ； in The coordinates of a single point in the generated point cloud map. These are the pixel coordinates of the point in the image. For intrinsic parameters of the vision sensor, This represents the depth value of the corresponding depth image.

3. The visual relocalization method for sparse feature-based enclosed environments based on semantic maps according to claim 1, characterized in that, Step 3: In the semantic map construction thread, the PCL point cloud processing library is used to process the 3D dense semantic point cloud map, filter out special semantic point clouds of interest and cluster them into individual semantic object point clouds; for a single semantic object point cloud, the feature information of the object is extracted, including the semantic category of the object, the center coordinate position of the object point cloud, the radius of the circumcircle of the object point cloud, and the normal vector of the object point cloud set. By treating individual objects as nodes in an undirected graph, the distance between object center points as edges, and object feature information as node attributes, a global undirected graph with 3D semantic abstraction is constructed. The graph structure information is then written to a file for storage. The specific method is as follows: Step 3.1, Point Cloud Preprocessing: For the 3D dense semantic point cloud map, firstly, voxel filters in the Point Cloud Library (PCL) are used for downsampling to reduce computation; then, statistical outlier removal (SOR) filters are used to remove outliers, where the threshold for the number of nearest neighbors used to determine whether a point is an outlier is set to 500; based on the semantic segmentation results, point cloud data with specified semantic labels of interest are selected. Step 3.2, Object point cloud clustering and segmentation: For the filtered single semantic point cloud, the density-based spatial clustering (DBSCAN) algorithm is used to cluster the points that are spatially close to each other into independent point cloud clusters. Each point cloud cluster is regarded as an independent semantic object. Step 3.3, Object Feature Information Extraction: For each independent semantic object point cloud cluster, extract the following feature information: (1) Coordinates of the center of the object's point cloud ； in Represents the center point of the object's point cloud. express coordinates This indicates the total number of points in the object's point cloud. Three-dimensional spatial coordinates representing each point; (2) Radius of the circumcircle of the object's point cloud : ； (3) Normal vector of the object point cloud set Calculate the covariance matrix of point clouds ; ； Perform eigenvalue decomposition on the covariance matrix to obtain the corresponding eigenvectors. After normalization, it becomes the normal vector of the object point cloud set; Step 3.4, Global Undirected Graph Construction: Each extracted independent semantic object is used as a node in the graph; the Euclidean distance between the center points of any two objects is calculated. If the distance is less than a preset distance threshold, an undirected edge is established between the two nodes, and the weight of the edge is the distance between the two center points; the attributes of each node are composed of its corresponding object feature information; finally, all nodes, edges and node attributes together constitute a global undirected graph describing the semantic topology of the environment, and it is stored in a file.

4. The visual relocalization method for sparse feature-based enclosed environments based on semantic maps according to claim 1, characterized in that, Step 4: For the RGBD data of the current frame or consecutive frames to be relocated, repeat steps 1-3 to obtain a local undirected graph of 3D semantic abstraction; read the global map file saved in step 3, and calculate the matching relationship between the corresponding nodes and edges of the local undirected graph of 3D semantic abstraction in the global undirected graph using the improved VF2 matching algorithm based on subgraph isomorphism. The specific method is as follows: Step 4.1, Local Undirected Graph Construction: Construct a local undirected graph based on the RGBD data of the current frame or consecutive frames to be relocated; if the input is single-frame data, directly perform point cloud processing and feature extraction steps; if the input is a consecutive frame sequence, repeat steps 1 to 3 to construct the local undirected graph. Step 4.2, Matching Priority Setting: When extracting object information from a local 3D dense semantic point cloud map, the matching priority is set according to the quality of the extracted point cloud clusters; the priority order is as follows: (a) Boundary integrity: Point cloud clusters with complete and clear boundaries have a higher matching priority; (b) Point cloud object radius: Under the condition of similar boundary sharpness, point cloud clusters with smaller semantic object radii are matched first; That is, small semantic objects with clear and obvious boundaries have a higher matching priority than other objects with incomplete boundaries in the local map; Step 4.3, Subgraph Isomorphic Matching Conditions: Based on the VF2 subgraph isomorphic matching algorithm, calculate the matching relationships between nodes and edges between the local undirected graph and the global undirected graph; the improved VF2 algorithm defines the following matching conditions and procedures: Matching conditions include: (a) For each node in the local graph, there must be a unique corresponding node in the global graph; (b) Different nodes in the local graph have different corresponding nodes in the global graph; (c) For every edge in the local graph, there must be an edge in the global graph, and the nodes at both ends of the edge must correspond one-to-one with the nodes at both ends of the edge in the local graph. (d) All matching nodes must have the same semantic category label; (e) The length error of all matching edges must be within a preset threshold range; The matching process includes: (a) Initialization: Start from an empty match state; (b) Candidate generation: In the current state, if the current matching state is not equal to the subgraph that needs to be matched, that is, the matched subgraph is not completed, then a set of candidate node pairs is generated; (c) Feasibility check and recursive search: For each candidate pair in the candidate set, add the currently calculated candidate pair to the current matching state and check whether the matching conditions are met. If they are met, enter the new matching state and recursively call the matching process (a)-(c); if they are not met, cancel the operation of adding the candidate pair to the matching and try the next candidate pair. (d) Termination condition: All candidate pairs have been tried, and the current matching status is output as the result; Step 4.4, matching result selection: perform incomplete graph matching based on matching priority; if the number of candidate matching groups obtained from the initial matching exceeds the set number, then the matching groups containing low-priority nodes and their associated edges are removed; finally, if the proportion of successfully matched vertices and edges in a certain matching group exceeds the set threshold, then the group is determined to be a valid coarse matching.

5. The visual relocalization method for closed environments based on sparse features according to claim 1, characterized in that, Step 5: Using the matching relationship information obtained in Step 4, the initial transformation matrix between the global and local maps is obtained through the RANSAC algorithm and SVD, which is the preliminary visual relocalization result. The initial transformation matrix is ​​then optimized using the ICP point cloud registration method to obtain the accurate transformation matrix. After matrix transformation, the accurate visual relocalization output is obtained. The specific method is as follows: Step 5.1, Solving for the initial transformation matrix: Based on the matching relationship obtained in Step 4, the initial transformation matrix between the global undirected graph and the local undirected graph is solved using the Random Sample Consensus (RANSAC) algorithm and Singular Value Decomposition (SVD). Step 5.2, RANSAC Iteration Process: Randomly select multiple sets of matching samples from the matching relationships, each set containing at least 3 pairs of matching center points and normal vectors; The candidate rotation matrix and translation vector for each group of samples are solved by SVD. Calculate the reprojection error of all matching points based on the candidate transformation matrix, and determine the points with errors less than a set threshold as interior points to form a consensus set. The above steps are executed iteratively, and the candidate transformation matrix with the largest consensus set is retained as the optimal initial transformation matrix; Step 5.3, Iteration Termination Condition: The RANSAC iteration terminates when one of the following conditions is met: (a) The proportion of the number of points within the consensus set to the total number of matching points exceeds a preset threshold; (b) The number of iterations reaches the preset maximum number of iterations; Step 5.4, ICP Fine Registration Optimization: Using the optimal initial transformation matrix as the initial value for the Iterative Closest Point (ICP) registration algorithm, fine registration is performed on the global map and the local cloud map, including: For each point in the transformed local point cloud, search for the corresponding point with the closest Euclidean distance in the global point cloud and establish a point pair relationship; Based on the point-pair relationship, calculate the rigid transformation matrix that minimizes the mean square error between all point pairs. Iteratively update the point cloud position and transformation matrix until the mean square error is lower than the set threshold or the maximum number of iterations is reached, and output the final accurate transformation matrix as the visual relocalization result.

6. A visual relocalization system for sparse features in enclosed environments based on semantic maps, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the visual relocalization method for sparse features in enclosed environments based on semantic maps as described in any one of claims 1-5.

7. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the visual relocalization method for sparse features in enclosed environments based on semantic maps as described in any one of claims 1-5.

8. A computer-readable storage medium having a computer program stored thereon, wherein when executed by a processor, the computer program implements the visual relocalization method for sparse features in enclosed environments based on semantic maps as described in any one of claims 1-5.

Images