SLAM methods, devices, and systems based on multi-level spatial structures

Abstract

This invention relates to the field of indoor positioning and mapping technology, specifically disclosing a SLAM method, apparatus, and system based on a multi-level spatial structure. The method includes: acquiring image information and image depth information at the same time; preprocessing the image information and the image depth information; performing geometric consistency verification on image keyframe information, and performing local optimization processing on the image keyframe information that has passed geometric consistency verification; performing loop closure detection based on the image keyframe information, and constructing corresponding principal direction constraints based on different loop closure detection results; performing global optimization on the locally optimized map information and locally optimized camera pose information based on the principal direction constraints to obtain globally optimized map information and globally optimized camera information; and constructing a map based on the globally optimized map information and globally optimized camera information. The SLAM method based on a multi-level spatial structure provided by this invention can improve positioning and mapping accuracy.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning and mapping technology, and in particular to a SLAM method, a SLAM device, and a SLAM system based on a multi-level spatial structure. Background Technology

[0002] In recent years, research on autonomous driving technology has become a hot topic and a trend. Visual SLAM (Simultaneous Localization and Mapping), as a crucial part of environmental perception in autonomous driving technology, suffers from unstable features extracted in indoor scenes. Traditional visual SLAM systems cannot robustly localize and map using unstable features. Therefore, solving the problem of unstable features extracted in indoor scenes is vital for the widespread adoption of visual SLAM. In recent years, the field of computer vision has conducted extensive work on visual SLAM in various scenarios, achieving significant results. However, due to the instability of features in indoor scenes, visual SLAM still struggles to work robustly in indoor environments.

[0003] In existing technologies, the Manhattan hypothesis and the Atlanta world hypothesis are commonly used structural assumptions for indoor scenes. Combined with line features, they can increase the observations and constraints for pose estimation, overcoming the problem of unstable features in indoor scenes. SLAM methods based on these assumptions define the geometric structure features of the indoor scene, requiring the extracted line features to satisfy these defined geometric structure assumptions, achieving certain results in indoor scenes. However, existing SLAM methods based on structural assumptions struggle to accurately define the geometric structure features of indoor environments, thus introducing additional estimation errors.

[0004] Therefore, how to solve the problem of decreased positioning and mapping accuracy caused by the lack of stability of visual SLAM features in indoor scenes has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] This invention provides a SLAM method, a SLAM device, and a SLAM system based on a multi-level spatial structure, which solves the problem of decreased positioning and mapping accuracy caused by unstable features in indoor scenes in related technologies.

[0006] As a first aspect of the present invention, a SLAM method based on a multi-level spatial structure is provided, comprising:

[0007] Acquire image information and image depth information at the same time;

[0008] The image information and the image depth information are preprocessed to obtain image keyframe information;

[0009] Geometric consistency verification is performed on the image keyframe information, and local optimization processing is performed on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information.

[0010] Loop closure detection is performed based on the keyframe information of the image, and corresponding main direction constraints are constructed based on different loop closure detection results.

[0011] The local optimized map information and local optimized camera pose information are globally optimized based on the main direction constraints to obtain globally optimized map information and globally optimized camera information.

[0012] A map is constructed based on the globally optimized map information and the globally optimized camera information.

[0013] Further, the image information and the image depth information are preprocessed to obtain image keyframe information, including:

[0014] Feature extraction is performed on the image information and the image depth information respectively to obtain image point features and image line features;

[0015] Feature matching is performed based on the image point features and the image line features, and the camera pose is calculated based on the feature matching relationship;

[0016] Extract constraint vectors and generate image keyframe information based on the constraint vectors.

[0017] Further, constraint vectors are extracted, and image keyframe information is generated based on the constraint vectors, including:

[0018] Extract the ground normal vector at the current moment;

[0019] The constraint vector is obtained by calculating the normal vector corresponding to the pixel in the image information based on the orthogonal structured environment spatial distance;

[0020] According to the keyframe generation strategy, frames that meet preset conditions are selected and marked as keyframes to obtain the image keyframe information. The preset conditions include at least the following: constraint vectors are extracted at the current time, or constraint vectors are not extracted at the current time, the distance between the current frame and the previous frame is greater than a preset number of frames and the number of image point features is greater than a preset threshold, and the number of image line feature data is greater than a preset threshold.

[0021] Further, geometric consistency verification is performed on the image keyframe information, and local optimization processing is performed on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information, including:

[0022] The local map information contained in the keyframe information of the image is fused;

[0023] Perform geometric consistency verification on map points and lines in the fused local map information;

[0024] An optimization task is constructed based on the weight information of map lines and map points. The local map information that has passed the geometric consistency verification is then locally optimized based on the optimization task to obtain locally optimized map information and locally optimized camera pose information.

[0025] Furthermore, an optimization task is constructed based on the weight information of map lines and map points, and the local map information that has passed geometric consistency verification is locally optimized according to the optimization task to obtain locally optimized map information and locally optimized camera pose information, including:

[0026] The weight information of the map lines is determined based on the direction vector of the map lines and the direction vector of the main direction constraint.

[0027] An optimization task is constructed based on the weight information of map lines and map points, and the cost function to be optimized is determined.

[0028] Based on the optimization task and the cost function to be optimized, the local map information after passing the geometric consistency verification is locally optimized to obtain locally optimized map information and locally optimized camera pose information.

[0029] Furthermore, loop closure detection is performed based on the keyframe information of the image, and corresponding principal direction constraints are constructed based on different loop closure detection results, including:

[0030] The image keyframe information is subjected to loop closure detection according to the loop closure detection strategy;

[0031] Determine whether a loop closure is detected during the loop closure detection;

[0032] If a loop closure is detected in the loop closure detection, the loop closure detection result is determined to be a completed loop closure, and the main direction constraint condition contained in the keyframe of the loop closure detection is used as the main direction constraint condition of the current keyframe.

[0033] If no loop closure is detected in the loop closure detection, the loop closure detection result is determined to be an incomplete loop closure, and the main direction constraint conditions of the current keyframe are obtained after optimizing the constraint vector of the current keyframe according to the optimization algorithm.

[0034] Furthermore, the main direction constraints of the current keyframe are obtained by optimizing the constraint vector of the current keyframe according to the optimization algorithm, including:

[0035] The constraint vector of the current keyframe is coarsely optimized using the MeanShift algorithm, and the coarse optimization result is obtained.

[0036] The coarse optimization results are then optimized using the KMeans clustering algorithm to obtain the main direction constraints of the current keyframe.

[0037] Furthermore, loop closure detection is performed based on the keyframe information of the image, and corresponding principal direction constraints are constructed based on different loop closure detection results. This also includes steps performed before determining whether a loop closure is detected in the loop closure detection process.

[0038] Determine if initialization is complete.

[0039] If initialization is not completed, optimize the first keyframe containing the constraint vector and generate the initial principal direction constraints based on the optimization results;

[0040] If initialization is complete, proceed to determine if a loopback has been detected.

[0041] As another aspect of the present invention, a SLAM device based on a multi-level spatial structure is provided, comprising:

[0042] The acquisition module is used to acquire image information and image depth information at the same time.

[0043] The preprocessing module is used to preprocess the image information and the image depth information to obtain image keyframe information;

[0044] The local optimization module is used to perform geometric consistency verification on the image keyframe information, and to perform local optimization processing on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information.

[0045] The loop closure detection and main direction optimization module is used to perform loop closure detection based on the key frame information of the image, and to construct corresponding main direction constraints based on different loop closure detection results.

[0046] The global optimization module is used to perform global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint conditions, so as to obtain global optimized map information and global optimized camera information.

[0047] The map building module is used to build a map based on the globally optimized map information and the globally optimized camera information.

[0048] As another aspect of the present invention, a SLAM system based on a multi-level spatial structure is provided, comprising: an image acquisition device and the aforementioned SLAM device based on a multi-level spatial structure communicatively connected to the image acquisition device.

[0049] The image acquisition device is used to acquire image information and image depth information at the same time.

[0050] The SLAM device based on a multi-level spatial structure is used to preprocess image information and image depth information at the same time, and to build a map based on local optimization processing, loop closure detection and main direction optimization processing, and global optimization processing.

[0051] The SLAM method based on multi-level spatial structure provided by this invention obtains keyframe information of the image after preprocessing image information and image depth information. Based on the keyframe information, geometric consistency verification is performed and local optimization is carried out. After loop closure detection is performed based on the keyframe information, principal direction constraints are constructed. Then, the results of local optimization are globally optimized based on the principal direction constraints to achieve the final map construction. This SLAM method based on multi-level spatial structure uses a dynamic optimization strategy based on structured feature detection results to dynamically optimize pose. At the same time, a loop closure detection method is used to suppress the accumulation of drift error, thereby effectively reducing computational complexity and improving loop closure accuracy, and improving the robustness and accuracy of visual SLAM in indoor scene localization and mapping. Attached Figure Description

[0052] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0053] Figure 1 A flowchart of the SLAM method based on a multi-level spatial structure provided by the present invention.

[0054] Figure 2 The flowchart illustrates the method for obtaining keyframe information of an image provided by this invention.

[0055] Figure 3 The flowchart illustrates the method for extracting constraint vectors and generating image keyframe information provided by this invention.

[0056] Figure 4 A flowchart of the local optimization processing method provided by the present invention.

[0057] Figure 5 A flowchart illustrating a specific implementation of the local optimization process provided by this invention.

[0058] Figure 6 The flowchart of the method for constructing principal direction constraints provided by the present invention is shown.

[0059] Figure 7 A schematic diagram illustrating the constraint vector optimization principle provided by this invention.

[0060] Figure 8 The structural block diagram of the SLAM device based on a multi-level spatial structure provided by the present invention.

[0061] Figure 9 The structural block diagram of the SLAM system based on a multi-level spatial structure provided by the present invention. Detailed Implementation

[0062] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0063] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0064] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0065] This embodiment provides a SLAM method based on a multi-level spatial structure. Figure 1 This is a flowchart of a SLAM method based on a multi-level spatial structure according to an embodiment of the present invention, such as... Figure 1 As shown, it includes:

[0066] S100: Acquire image information and image depth information at the same time;

[0067] In this embodiment of the invention, the image acquisition device acquires image information and image depth information of the environment at the same time in real time, thus obtaining multiple sets of image information and image depth information with timestamp information. The timestamp information of each set of image information and image depth information is acquired at the same time.

[0068] S200: Preprocess the image information and the image depth information to obtain image keyframe information;

[0069] Specifically, image keyframe information is obtained by performing preprocessing such as feature recognition and feature extraction on the acquired image information and image depth information.

[0070] S300. Perform geometric consistency verification on the image keyframe information, and perform local optimization processing on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information.

[0071] In this embodiment of the invention, geometric consistency verification is performed on the obtained image keyframe information, and then local optimization processing is performed on the image keyframe information that has passed the geometric consistency verification, thereby enabling the extraction of features with poor quality, making the obtained local optimized map information and local optimized camera pose information more accurate.

[0072] S400. Perform loop closure detection based on the keyframe information of the image, and construct corresponding main direction constraints based on different loop closure detection results.

[0073] In this embodiment of the invention, loop closure detection is performed based on the above-mentioned keyframe information of the image, and corresponding main direction constraints are constructed based on different loop closure detection structures, thereby enabling dynamic optimization to improve optimization efficiency and the accuracy of optimized localization and mapping.

[0074] S500: Perform global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint conditions to obtain global optimized map information and global optimized camera information.

[0075] In this embodiment of the invention, based on the obtained principal direction constraints, global optimization processing is performed on the locally optimized map information and the locally optimized camera pose information to obtain globally optimized map information and globally optimized camera information. It should be understood that by using the above-mentioned loop closure detection and obtaining principal direction constraints based on dynamic optimization, and then achieving global optimization processing, computational complexity can be effectively reduced, loop closure accuracy improved, and thus the accuracy of the optimized localization and mapping improved.

[0076] S600: Construct a map based on the global optimized map information and the global optimized camera information.

[0077] In this embodiment of the invention, a map is constructed based on the aforementioned globally optimized map information and globally optimized camera information, thereby improving the robustness and accuracy of visual SLAM in indoor scene localization and mapping.

[0078] In summary, the SLAM method based on multi-level spatial structure provided by this invention obtains keyframe information of the image after preprocessing image information and image depth information. Based on this keyframe information, geometric consistency verification is performed followed by local optimization processing. Loop closure detection is then performed based on the keyframe information to construct principal direction constraints. Finally, the results of the local optimization processing are globally optimized based on the principal direction constraints to achieve the final map construction. This multi-level spatial structure-based SLAM method uses a dynamic optimization strategy based on structured feature detection results to dynamically optimize pose, while using loop closure detection to suppress the accumulation of drift errors. This effectively reduces computational complexity and improves loop closure accuracy, thereby enhancing the robustness and accuracy of visual SLAM in indoor scene localization and mapping.

[0079] In this embodiment of the invention, the image information and the image depth information are preprocessed to obtain image keyframe information, such as... Figure 2 As shown, it includes:

[0080] S210. Perform feature extraction on the image information and the image depth information respectively to obtain image point features and image line features;

[0081] In this embodiment of the invention, feature extraction is performed on image information and image depth information respectively, and feature matching is performed based on the extracted features, and then the camera pose is calculated based on the feature matching relationship.

[0082] Specifically, the coordinates of the extracted image feature point i in the pixel plane are denoted as p. i (u, v), image line features are represented using endpoints and their direction vectors, assuming the line feature originates at point s in the pixel plane. j The endpoint is e j Then the line feature l j Represented as l j =(e j -s j ) / ||e j -s j Then, a matching relationship is established between the image point features and image line features of adjacent frames.

[0083] S220. Perform feature matching based on the image point features and the image line features, and calculate the camera pose based on the feature matching relationship;

[0084] Specifically, image point features and image line features are used for matching to calculate the camera pose Pose = {R}. t , t t The calculation formula is as follows:

[0085]

[0086] in, Represents the image point feature error term: Line feature error is calculated by projecting 3D features onto a 2D plane using the back projection function π. Represents the weighted value of image point features. Represents 3D point features, R t and t t Represent the rotation and translation components of the camera pose at time t; and the error term of the image line features, respectively. This represents the weighted value of the image line features; Indicates 3D line features. and n represents the start and end points of the 3D line feature, respectively. j This represents the direction vector of the map line projected onto the plane.

[0087] S230. Extract constraint vectors and generate image keyframe information based on the constraint vectors.

[0088] In this embodiment of the invention, the ground normal vector at time t can be extracted using 3F2N (Three-Filters-to-Normal), while the normal vectors corresponding to the remaining pixels are calculated using orthogonal structured environmental spatial distance, forming a constraint vector N. t ={N0, ..., N n}

[0089] Specifically, constraint vectors are extracted, and image keyframe information is generated based on the constraint vectors, such as... Figure 3 As shown, it includes:

[0090] S231. Extract the ground normal vector at the current moment;

[0091] It should be understood that, in this embodiment of the invention, the ground normal vector at time t can be extracted using 3F2N.

[0092] S232. Calculate the normal vector corresponding to the pixel in the image information based on the orthogonal structured environment spatial distance to obtain the constraint vector;

[0093] The normal vectors corresponding to the remaining pixels are calculated based on the spatial distance of the orthogonal structured environment, forming the constraint vector N. t={N0, ..., N n}

[0094] S233. Select frames that meet preset conditions as key frames according to the key frame generation strategy, and obtain the image key frame information. The preset conditions include at least the following: constraint vector is extracted at the current time, or constraint vector is not extracted at the current time, the distance between the current frame and the previous frame is greater than a preset number of frames and the number of image point features is greater than a preset threshold, and the number of image line feature data is greater than a preset threshold.

[0095] It should be understood that if the constraint vector N is extracted from the frame at the current time t... t ={N0, ..., N n If}, then the frame at time t is marked as the keyframe KF. t If the constraint vector N is not extracted at time t... t ={N0, ..., N n If the current frame is more than a preset number of frames away from the previous frame, and the number of image point features is greater than a preset threshold, and the number of image line feature data is greater than a preset threshold, then the frames that are selected as keyframes (KF) need to be marked. t For example, frames that are more than 20 frames away from the previous frame and have extracted more than 45 point features and more than 10 line features can be selected as keyframes (KF). t .

[0096] In this embodiment of the invention, after extracting keyframes, dynamic adjustment weights can be added to features based on the main direction constraint to optimize map points, map lines, and camera pose.

[0097] Specifically, in this embodiment of the invention, geometric consistency verification is performed on the image keyframe information, and local optimization processing is performed on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information, such as... Figure 4 As shown, it includes:

[0098] S310. The local map information contained in the keyframe information of the image is fused.

[0099] In this embodiment of the invention, the local map information contained in the image keyframe information is fused, that is, the fused local map information is obtained after fusing the local map contained in the keyframe KF.

[0100] S320. Perform geometric consistency verification on map points and map lines in the fused local map information;

[0101] In this embodiment of the invention, local map points Map lines Optimize by removing 3D features of poor quality.

[0102] S330. Construct an optimization task based on the weight information of map lines and map points, and perform local optimization on the local map information after passing the geometric consistency verification according to the optimization task to obtain locally optimized map information and locally optimized camera pose information.

[0103] Specifically, a g2o optimization task is constructed based on the weights of point features and line features, and local BA (Boundary Adjustment) optimization is performed to obtain a more accurate pose and local map.

[0104] In this embodiment of the invention, an optimization task is constructed based on the weight information of map lines and map points. Then, based on the optimization task, local map information that has passed geometric consistency verification is locally optimized to obtain locally optimized map information and locally optimized camera pose information, such as... Figure 5 As shown, it includes:

[0105] S331. Determine the weight information of the map lines based on the direction vector of the map lines and the direction vector of the main direction constraint.

[0106] In this embodiment of the invention, the correlation between line features and the cosine value of the principal direction can be used to determine the degree of constraint correlation: n represents the direction vector of a map line. i ξ represents the direction vector of the main direction constraint, and ξ represents the degree of correlation of the constraint, which can also be understood as the dynamic optimization weight of the line feature.

[0107] S332. Construct an optimization task based on the weight information of map lines and map points, and determine the cost function to be optimized;

[0108] Specifically, the g2o optimization task is constructed using the weights of map point features and map line features. The cost function to be optimized is as follows:

[0109]

[0110] S333. Based on the optimization task and the cost function to be optimized, perform local optimization on the local map information after geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information.

[0111] In this embodiment of the invention, local BA optimization is performed using the g2o optimization library and a constructed cost function.

[0112] Specifically, loop closure detection is performed based on the keyframe information of the image, and corresponding principal direction constraints are constructed based on different loop closure detection results, such as... Figure 6 As shown, it includes:

[0113] S410. Perform loop closure detection on the keyframe information of the image according to the loop closure detection strategy;

[0114] It should be understood that STSL (Spatial-temporal sensitive Sub-global Map Loop) is used to detect whether a loop closure has been completed, and a specific strategy is selected based on the detection results; and a coarse-to-fine optimization algorithm is used to optimize the constraint vector.

[0115] Specifically, STSL uses ReID to add ReID to map points and map lines corresponding to the main direction constraints, and projects the features of the current frame onto the global region for matching during the matching process.

[0116] S420. Determine whether a loop closure is detected in the loop closure detection;

[0117] It should be understood that when using STSL to access the global map and matching the most recent keyframe with all keyframes containing the main direction constraint, if the number of matching point features is greater than the preset threshold for the number of point features (e.g., 20) and the number of line features is greater than the preset threshold for the number of line features (e.g., 10), and the main direction constraint can be matched in two directions, then the loop closure is considered successful, i.e., a loop closure is detected.

[0118] Specifically, the BOW dictionary can be used to match map point features, and the LBD descriptor can be used to match map line features. If the number of points matched between the loopback frame and the keyframe is greater than 20, and at least two line features corresponding to the main direction constraint can be matched, the loopback is considered complete, and the constraint vector is not optimized; if the loopback is not complete, the constraint vector is optimized.

[0119] S430. If a loop closure is detected in the loop closure detection, the loop closure detection result is determined to be a completed loop closure, and the main direction constraint condition contained in the key frame of the loop closure detection is used as the main direction constraint condition of the current key frame.

[0120] It should be understood that if a loop closure is detected, the LKF of the detected frame will be used. i The main direction constraints contained therein serve as the current keyframe KF. i Main direction constraint MC j .

[0121] S440. If no loop closure is detected in the loop closure detection, the loop closure detection result is determined to be an incomplete loop closure, and the main direction constraint conditions of the current key frame are obtained after optimizing the constraint vector of the current key frame according to the optimization algorithm.

[0122] In this embodiment of the invention, if no loop closure is detected, the MeanShift algorithm is used to coarsely optimize the constraint vector of the current keyframe, and then the KMeans clustering algorithm is used to further optimize it to obtain the principal direction constraint MC. j .

[0123] Specifically, the main direction constraints of the current keyframe are obtained by optimizing the constraint vector of the current keyframe according to the optimization algorithm, including:

[0124] The constraint vector of the current keyframe is coarsely optimized using the MeanShift algorithm, and the coarse optimization result is obtained.

[0125] The coarse optimization results are then optimized using the KMeans clustering algorithm to obtain the main direction constraints of the current keyframe.

[0126] It should be understood that if no loop closure is detected, the constraint vector needs to be optimized. Here, the constraint vector is first extracted and then optimized to obtain the main direction constraint.

[0127] like Figure 7 As shown, the endpoints of the constraint vector are normalized and then projected onto a Gaussian sphere. All endpoints are then numbered to represent the constraint vector to be optimized: N coarse ={n1, ..., n n}, the coordinates of the endpoints n of the constraint vector i =(x i y i , z i ) represents the parameters to be optimized in the constraint vector.

[0128] In this embodiment of the invention, a coarse-to-fine constraint optimization method is used. First, a mean-shift algorithm using the tangent plane Gaussian kernel function is used to initially filter the constraint vectors; then, KMeans is used to process the endpoints, and the clustering loss function is defined as follows:

[0129]

[0130] Where, n i Let c represent the i-th sample. i n i The cluster to which it belongs U represents the center point of the cluster, and N represents the total number of sample endpoints. Only clusters with higher density are retained: U = {u1, ..., u2}. n}

[0131] For different clusters u iThe mean-shift algorithm using the Gaussian kernel function of the tangent plane is used to solve for the centroid of the optimal cluster obtained in this step after classification. The constraint vector corresponding to the endpoint closest to the centroid, i.e., the principal direction constraint, is shown in the following formula:

[0132] MC = {N1, ..., N} m}

[0133] Finally, based on the main direction constraints, the locally optimized map information and the locally optimized camera pose information are globally optimized to obtain globally optimized map information and globally optimized camera information, and a map is constructed based on the globally optimized structure.

[0134] In this embodiment of the invention, loop closure detection is performed based on the image keyframe information, and corresponding principal direction constraints are constructed based on different loop closure detection results. The method also includes steps performed before determining whether a loop closure is detected in the loop closure detection:

[0135] Determine if initialization is complete.

[0136] If initialization is not completed, optimize the first keyframe containing the constraint vector and generate the initial principal direction constraints based on the optimization results;

[0137] If initialization is complete, proceed to determine if a loopback has been detected.

[0138] It should be understood that before performing loop closure detection, it is necessary to first determine whether the SLAM system has completed initialization, that is, whether the initialization state variables in the SLAM system have changed. If they have changed, initialization is considered complete. If they have not changed, the first keyframe containing the constraint vector is optimized based on the timestamp information to optimize its constraint vector and generate the initial principal direction constraint MC0. If it is determined that initialization has been completed, the loop closure detection step can continue.

[0139] In summary, the SLAM method based on a multi-level spatial structure provided by this invention, when applied to indoor environments, can extract and match point and line features and constraint vectors, select keyframes, and remove features of poor quality. It then constructs a g2o optimization task based on dynamically optimized weights added to the features, performs BA optimization, and uses the STSL loop closure detection method. A coarse-to-fine constraint vector optimization method is used to obtain the principal direction constraint, and global BA is then performed. This multi-level spatial structure-based SLAM method improves the robustness and accuracy of visual SLAM systems in indoor scene localization and mapping by using more generalized structured assumptions and introducing line features. Furthermore, the use of dynamic weight adjustment factors and the construction of a g2o optimization task based on dynamically optimized weights added to the features improves the efficiency of BA optimization and enhances the accuracy of the optimized localization and mapping. In loop closure detection, the use of STSL makes this method more accurate and efficient, reducing computational complexity and improving loop closure accuracy.

[0140] As another embodiment of the present invention, a SLAM device 100 based on a multi-level spatial structure is provided, wherein, as Figure 8 As shown, it includes:

[0141] The acquisition module 110 is used to acquire image information and image depth information at the same time.

[0142] The preprocessing module 120 is used to preprocess the image information and the image depth information to obtain image keyframe information;

[0143] The local optimization module 130 is used to perform geometric consistency verification on the image keyframe information and perform local optimization processing on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information.

[0144] The loop closure detection and main direction optimization module 140 is used to perform loop closure detection based on the key frame information of the image, and to construct corresponding main direction constraints based on different loop closure detection results.

[0145] The global optimization module 150 is used to perform global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint conditions, so as to obtain global optimized map information and global optimized camera information.

[0146] The map building module 160 is used to build a map based on the global optimized map information and the global optimized camera information.

[0147] The SLAM device based on a multi-level spatial structure provided by this invention obtains keyframe information of the image after preprocessing image information and image depth information. Based on the keyframe information, geometric consistency verification is performed, followed by local optimization processing. Loop closure detection is performed based on the keyframe information to construct principal direction constraints. Then, the results of local optimization processing are globally optimized based on the principal direction constraints to achieve the final map construction. This SLAM device based on a multi-level spatial structure uses a dynamic optimization strategy based on structured feature detection results to dynamically optimize pose. At the same time, a loop closure detection method is used to suppress the accumulation of drift error, thereby effectively reducing computational complexity and improving loop closure accuracy, and improving the robustness and accuracy of visual SLAM in indoor scene localization and mapping.

[0148] The specific working principle of the SLAM device based on multi-level spatial structure provided by this invention can be referred to the description of the SLAM method based on multi-level spatial structure above, and will not be repeated here.

[0149] As another embodiment of the present invention, a SLAM system 10 based on a multi-level spatial structure is provided, wherein, as Figure 9 As shown, it includes: an image acquisition device 200 and the aforementioned SLAM device 100 based on a multi-level spatial structure, which is communicatively connected to the image acquisition device.

[0150] The image acquisition device 200 is used to acquire image information and image depth information at the same time.

[0151] The SLAM device 100 based on a multi-level spatial structure is used to preprocess image information and image depth information at the same time, and to build a map based on local optimization processing, loop closure detection and main direction optimization processing, and global optimization processing.

[0152] The SLAM system based on multi-level spatial structure provided by this invention adopts the SLAM device based on multi-level spatial structure described above, uses a dynamic optimization strategy based on structured feature detection results to dynamically optimize pose, and uses a loop closure detection method to suppress the accumulation of drift error, thereby effectively reducing computational complexity and improving loop closure accuracy, and improving the robustness and accuracy of visual SLAM in indoor scene localization and mapping.

[0153] The specific working principle of the SLAM system based on multi-level spatial structure provided by this invention can be referred to the description of the SLAM method based on multi-level spatial structure above, and will not be repeated here.

[0154] It is understood that the above embodiments are merely exemplary implementations used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A SLAM method based on a multi-level spatial structure, characterized in that, include: Acquire image information and image depth information at the same time; The image information and the image depth information are preprocessed to obtain image keyframe information; Geometric consistency verification is performed on the image keyframe information, and local optimization processing is performed on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information. Loop closure detection is performed based on the keyframe information of the image, and corresponding main direction constraints are constructed based on different loop closure detection results. The local optimized map information and local optimized camera pose information are globally optimized based on the main direction constraints to obtain globally optimized map information and globally optimized camera information. A map is constructed based on the globally optimized map information and the globally optimized camera information; Loop closure detection is performed based on the keyframe information of the image, and corresponding principal direction constraints are constructed based on different loop closure detection results, including: The image keyframe information is subjected to loop closure detection according to the loop closure detection strategy; Determine whether a loop closure is detected during the loop closure detection; If a loop closure is detected in the loop closure detection, the loop closure detection result is determined to be a completed loop closure, and the main direction constraint condition contained in the keyframe of the loop closure detection is used as the main direction constraint condition of the current keyframe. If no loop closure is detected in the loop closure detection, the loop closure detection result is determined to be an incomplete loop closure, and the main direction constraint conditions of the current keyframe are obtained after optimizing the constraint vector of the current keyframe according to the optimization algorithm.

2. The SLAM method based on multi-level spatial structure according to claim 1, characterized in that, Preprocessing the image information and the image depth information to obtain image keyframe information includes: Feature extraction is performed on the image information and the image depth information respectively to obtain image point features and image line features; Feature matching is performed based on the image point features and the image line features, and the camera pose is calculated based on the feature matching relationship; Extract constraint vectors and generate image keyframe information based on the constraint vectors.

3. The SLAM method based on multi-level spatial structure according to claim 2, characterized in that, Extracting constraint vectors and generating image keyframe information based on the constraint vectors includes: Extract the ground normal vector at the current moment; The constraint vector is obtained by calculating the normal vector corresponding to the pixel in the image information based on the orthogonal structured environment spatial distance; According to the keyframe generation strategy, frames that meet preset conditions are selected and marked as keyframes to obtain the image keyframe information. The preset conditions include at least the following: constraint vectors are extracted at the current time, or constraint vectors are not extracted at the current time, the distance between the current frame and the previous frame is greater than a preset number of frames and the number of image point features is greater than a preset threshold, and the number of image line feature data is greater than a preset threshold.

4. The SLAM method based on multi-level spatial structure according to claim 1, characterized in that, The image keyframe information is subjected to geometric consistency verification, and the keyframe information that passes the geometric consistency verification is subjected to local optimization processing to obtain locally optimized map information and locally optimized camera pose information, including: The local map information contained in the keyframe information of the image is fused; Perform geometric consistency verification on map points and lines in the fused local map information; An optimization task is constructed based on the weight information of map lines and map points. The local map information that has passed the geometric consistency verification is then locally optimized based on the optimization task to obtain locally optimized map information and locally optimized camera pose information.

5. The SLAM method based on multi-level spatial structure according to claim 4, characterized in that, An optimization task is constructed based on the weight information of map lines and map points. Then, based on this optimization task, local map information that has passed geometric consistency verification is locally optimized to obtain locally optimized map information and locally optimized camera pose information, including: The weight information of the map lines is determined based on the direction vector of the map lines and the direction vector of the main direction constraint. An optimization task is constructed based on the weight information of map lines and map points, and the cost function to be optimized is determined. Based on the optimization task and the cost function to be optimized, the local map information after passing the geometric consistency verification is locally optimized to obtain locally optimized map information and locally optimized camera pose information.

6. The SLAM method based on multi-level spatial structure according to claim 1, characterized in that, The main direction constraints of the current keyframe are obtained by optimizing the constraint vector of the current keyframe using the optimization algorithm, including: The constraint vector of the current keyframe is coarsely optimized using the MeanShift algorithm, and the coarse optimization result is obtained. The coarse optimization results are then optimized using the KMeans clustering algorithm to obtain the main direction constraints of the current keyframe.

7. The SLAM method based on multi-level spatial structure according to claim 1, characterized in that, Loop closure detection is performed based on the keyframe information of the image, and corresponding principal direction constraints are constructed based on different loop closure detection results. This also includes a step performed before determining whether a loop closure is detected in the loop closure detection process: Determine if initialization is complete. If initialization is not completed, optimize the first keyframe containing the constraint vector and generate the initial principal direction constraints based on the optimization results; If initialization is complete, proceed to determine if a loopback has been detected.

8. A SLAM device based on a multi-level spatial structure, used to implement the SLAM method based on a multi-level spatial structure as described in any one of claims 1 to 7, characterized in that, include: The acquisition module is used to acquire image information and image depth information at the same time. The preprocessing module is used to preprocess the image information and the image depth information to obtain image keyframe information; The local optimization module is used to perform geometric consistency verification on the image keyframe information, and to perform local optimization processing on the image keyframe information that has passed the geometric consistency verification to obtain locally optimized map information and locally optimized camera pose information. The loop closure detection and main direction optimization module is used to perform loop closure detection based on the key frame information of the image, and to construct corresponding main direction constraints based on different loop closure detection results. The global optimization module is used to perform global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint conditions, so as to obtain global optimized map information and global optimized camera information. The map building module is used to build a map based on the globally optimized map information and the globally optimized camera information.

9. A SLAM system based on a multi-level spatial structure, characterized in that, include: The image acquisition device and the SLAM device based on a multi-level spatial structure as described in claim 8, which is communicatively connected to the image acquisition device. The image acquisition device is used to acquire image information and image depth information at the same time. The SLAM device based on a multi-level spatial structure is used to preprocess image information and image depth information at the same time, and to build a map based on local optimization processing, loop closure detection and main direction optimization processing, and global optimization processing.

Images