SLAM method, device and system based on multi-level space structure

Abstract

The invention relates to the technical field of indoor positioning and mapping, and particularly discloses an SLAM method, device and system based on a multi-level spatial structure, and the method comprises the steps: obtaining image information and image depth information at the same moment; preprocessing the image information and the image depth information; performing geometric consistency verification on the image key frame information, and performing local optimization processing on the image key frame information passing the geometric consistency verification; loopback detection is carried out according to the image key frame information, and corresponding main direction constraint conditions are constructed according to different loopback detection results; performing global optimization on the local optimization map information and the local optimization camera pose information according to a main direction constraint condition to obtain global optimization map information and global optimization camera information; and constructing a map according to the global optimization map information and the global optimization camera information. According to the SLAM method based on the multi-level space structure, the positioning and map precision can be improved.

Description

Technical Field

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

[0002] In recent years, the research on autonomous driving technology has become a hot topic and trend. Visual SLAM (Simultaneous Localization and Mapping) is an extremely important part of the environmental perception level in autonomous driving technology. The features extracted in indoor scenes are not stable enough. Traditional visual SLAM systems cannot rely on unstable features to robustly locate and map. Therefore, solving the problem of unstable features extracted from indoor scenes is very important for the promotion of visual SLAM. In recent years, the field of computer vision has carried out a lot of work on visual SLAM in different scenarios and has achieved a lot of results. However, due to the unstable features of indoor scenes, visual SLAM is still difficult to work robustly in indoor scenes.

[0003] In the prior art, the Manhattan hypothesis and the Atlanta world hypothesis are commonly used structural assumptions for indoor scenes. When used in combination with line features, they can increase the observation and constraints of pose estimation and overcome the problem of unstable features in indoor scenes. SLAM methods based on these assumptions achieve certain effects in indoor scenes by defining the geometric structural features of indoor scenes so that the extracted line features need to meet the geometric structural assumptions defined by them. However, existing SLAM methods based on structural assumptions have difficulty in defining the geometric structural features of indoor environments well, thereby introducing additional estimation errors.

[0004] Therefore, how to solve the problem in the prior art that the positioning and map accuracy of visual SLAM is reduced due to the instability of indoor scene features has become a technical problem that needs to be urgently solved by those skilled in the art. Summary of the invention

[0005] The present invention provides a SLAM method based on a multi-level spatial structure, a SLAM device based on a multi-level spatial structure and a SLAM system based on a multi-level spatial structure, which solve the problem in the related art that visual SLAM is not stable enough in indoor scene features, resulting in reduced positioning and map accuracy.

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

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

[0008] Preprocessing the image information and the image depth information to obtain image key frame information;

[0009] Performing geometric consistency verification on the image key frame information, and performing local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information;

[0010] Performing loop detection according to the image key frame information, and constructing corresponding main direction constraint conditions according to different loop detection results;

[0011] Performing global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain global optimized map information and global optimized camera information;

[0012] A map is constructed according to the global optimized map information and the global optimized camera information.

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

[0014] Extracting features from the image information and the image depth information respectively to obtain image point features and image line features;

[0015] Performing feature matching according to the image point features and the image line features, and calculating the camera pose according to the feature matching relationship;

[0016] A constraint vector is extracted, and image key frame information is generated according to the constraint vector.

[0017] Further, extracting the constraint vector and generating image key frame information according to the constraint vector includes:

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

[0019] Calculating the normal vector corresponding to the pixel in the image information according to the orthogonal structured environment space distance to obtain the constraint vector;

[0020] According to the key frame generation strategy, frames that meet the preset conditions are selected and marked as key frames to obtain the image key frame information, wherein the preset conditions at least include that a constraint vector is extracted at the current moment, or that a constraint vector is not extracted at the current moment, the current frame is more than a preset frame number away from the previous frame and the number of image point features is greater than a preset point feature number threshold, and the image line feature data is greater than a preset line feature number threshold.

[0021] Further, the image key frame information is subjected to geometric consistency verification, and the image key frame information after geometric consistency verification is subjected to local optimization processing to obtain local optimized map information and local optimized camera pose information, including:

[0022] fusing the local map information contained in the image key frame information;

[0023] Verify the geometric consistency of map points and map lines in the fused local map information;

[0024] An optimization task is constructed according to the weight information of the map lines and the map points, and local optimization is performed on the local map information after the geometric consistency verification according to the optimization task to obtain the local optimized map information and the local optimized camera pose information.

[0025] Further, an optimization task is constructed according to the weight information of the map lines and the map points, and local optimization is performed on the local map information after the geometric consistency verification according to the optimization task to obtain the local optimized map information and the local optimized camera pose information, including:

[0026] Determine the weight information of the map line according to the direction vector of the map line and the direction vector of the main direction constraint;

[0027] Construct optimization tasks based on the weight information of map lines and map points, and determine the cost function to be optimized;

[0028] According to the optimization task and the cost function to be optimized, local map information that has passed geometric consistency verification is locally optimized to obtain local optimized map information and local optimized camera pose information.

[0029] Furthermore, loop detection is performed according to the image key frame information, and corresponding main direction constraint conditions are constructed according to different loop detection results, including:

[0030] Performing loop closure detection on the image key frame information according to a loop closure detection strategy;

[0031] Determining whether a loop is detected in the loop detection;

[0032] If a loop is detected in the loop detection, determining that the loop detection result is a completed loop, and using the main direction constraint condition contained in the key frame detected by the loop as the main direction constraint condition of the current key frame;

[0033] If no loop is detected in the loop detection, the loop detection result is determined to be an incomplete loop, and the constraint vector of the current key frame is optimized according to the optimization algorithm to obtain the main direction constraint condition of the current key frame.

[0034] Furthermore, the constraint vector of the current key frame is optimized according to the optimization algorithm to obtain the main direction constraint condition of the current key frame, including:

[0035] The constraint vector of the current key frame is roughly optimized according to the MeanShift algorithm, and a rough optimization result is obtained;

[0036] The rough optimization result is optimized according to the KMeans clustering algorithm to obtain the main direction constraint condition of the current key frame.

[0037] Further, loop detection is performed according to the image key frame information, and corresponding main direction constraint conditions are respectively constructed according to different loop detection results, and the following is also performed before the step of determining whether a loop is detected in the loop detection:

[0038] Determine whether the initialization is completed;

[0039] If initialization is not completed, the first key frame containing the constraint vector is optimized, and the initial main direction constraint condition is generated according to the optimized result;

[0040] If the initialization is completed, the step of determining whether a loop is detected is executed.

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

[0042] An acquisition module, used to acquire image information and image depth information at the same moment;

[0043] A preprocessing module, used for preprocessing the image information and the image depth information to obtain image key frame information;

[0044] A local optimization module is used to perform geometric consistency verification on the image key frame information, and perform local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information;

[0045] A loop detection and main direction optimization module, used to perform loop detection according to the image key frame information, and construct corresponding main direction constraint conditions according to different loop detection results;

[0046] A global optimization module, used for globally optimizing the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain the global optimized map information and the global optimized camera information;

[0047] A map construction module is used to construct a map according to the global optimized map information and the global 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 SLAM device based on a multi-level spatial structure as described above, which is 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 the multi-level spatial structure is used to perform preprocessing according to image information and image depth information at the same time, and realize map construction based on local optimization processing, loop detection and main direction optimization processing and global optimization processing.

[0051] The SLAM method based on a multi-level spatial structure provided by the present invention obtains image key frame information after preprocessing image information and image depth information, performs local optimization processing after geometric consistency verification based on the image key frame information, and constructs a main direction constraint after loop detection based on the image key frame information, and then globally optimizes the result after the local optimization processing based on the main direction constraint, thereby realizing the final map construction. The SLAM method based on a multi-level spatial structure uses a dynamic optimization strategy based on the structured feature detection result to dynamically optimize the posture, and uses a loop detection method to suppress the accumulation of drift errors, thereby effectively reducing the computational complexity and improving the loop accuracy, thereby improving the robustness and accuracy of visual SLAM positioning and mapping in indoor scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation of the present invention.

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

[0054] Figure 2 A flow chart of the method for obtaining image key frame information provided by the present invention.

[0055] Figure 3 A flow chart of the method for extracting constraint vectors and generating image key frame information provided by the present invention.

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

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

[0058] Figure 6 A flow chart of the method for constructing main direction constraints provided by the present invention.

[0059] Figure 7 This is a schematic diagram of the constraint vector optimization principle provided by the present invention.

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

[0061] Fig. 9 This is a structural block diagram of the SLAM system based on a multi-level spatial structure provided by the present invention. DETAILED DESCRIPTION

[0062] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0063] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work 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 and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so as to describe the embodiments of the present invention described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0065] In this embodiment, a SLAM method based on a multi-level spatial structure is provided. Figure 1 is a flow chart of a SLAM method based on a multi-level spatial structure according to an embodiment of the present invention. Figure 1 As shown, including:

[0066] S100, acquiring image information and image depth information at the same moment;

[0067] In an embodiment of the present invention, the image acquisition device acquires image information and image depth information of the environment at the same moment in real time, thereby being able to obtain multiple sets of image information and image depth information with timestamp information, and the timestamp information carried by each set of image information and image depth information is acquired at the same moment.

[0068] S200, preprocessing the image information and the image depth information to obtain image key frame information;

[0069] Specifically, the acquired image information and image depth information are preprocessed by performing feature recognition and feature extraction on them to obtain image key frame information.

[0070] S300, performing geometric consistency verification on the image key frame information, and performing local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information;

[0071] In an embodiment of the present invention, geometric consistency verification is performed on the image key frame information obtained as mentioned above, and then local optimization processing is performed on the image key frame information that has passed the geometric consistency verification, so that features of poor quality can be brought out, making the obtained local optimized map information and local optimized camera pose information more accurate.

[0072] S400, performing loop detection according to the image key frame information, and constructing corresponding main direction constraint conditions according to different loop detection results;

[0073] In an embodiment of the present invention, loop detection is performed based on the above-mentioned image key frame information, and corresponding main direction constraints are constructed based on different loop detection structures, so that dynamic optimization can be achieved to improve the optimization efficiency and the positioning and mapping accuracy after optimization.

[0074] S500, performing global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain global optimized map information and global optimized camera information;

[0075] In an embodiment of the present invention, the local optimized map information and the local optimized camera pose information are globally optimized based on the main direction constraint conditions obtained above, and the global optimized map information and the global optimized camera information are obtained. It should be understood that by performing the above loop detection and obtaining the main direction constraint conditions based on dynamic optimization, and then realizing global optimization processing, the computational complexity can be effectively reduced and the loop accuracy can be improved, thereby improving the positioning and mapping accuracy after optimization.

[0076] S600: construct a map according to the global optimized map information and the global optimized camera information.

[0077] In an embodiment of the present invention, a map is constructed based on the above-mentioned global optimized map information and the global optimized camera information, thereby improving the robustness and accuracy of visual SLAM in indoor scene positioning and mapping.

[0078] In summary, the SLAM method based on a multi-level spatial structure provided by the present invention obtains image key frame information after preprocessing image information and image depth information, and performs local optimization processing after geometric consistency verification based on the image key frame information, and constructs a main direction constraint after loop detection based on the image key frame information, and then globally optimizes the results after local optimization processing based on the main direction constraint, thereby realizing the final map construction. The SLAM method based on a multi-level spatial structure uses a dynamic optimization strategy based on the structured feature detection results to dynamically optimize the pose, and uses a loop detection method to suppress the accumulation of drift errors, thereby effectively reducing the computational complexity and improving the loop accuracy, thereby improving the robustness and accuracy of visual SLAM positioning and mapping in indoor scenes.

[0079] In the embodiment of the present invention, the image information and the image depth information are preprocessed to obtain image key frame information, such as Figure 2 As shown, including:

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

[0081] In the embodiment of the present invention, feature extraction is performed on the image information and the 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 on the pixel plane are marked as p i (u, v), the image line feature is represented by the endpoint and its direction vector, assuming that the starting point of the line feature in the pixel plane is s j , the end point is e j , then the line feature l j Represented as l j =(e j -s j ) / ||e j -s j ||. Then, the image point features and image line features of adjacent frames are matched.

[0083] S220, performing feature matching according to the image point features and the image line features, and calculating the camera pose according to the feature matching relationship;

[0084] Specifically, the camera pose Pose = {R t , t t}, the calculation formula is as follows:

[0085]

[0086] in, Represents the image point feature error term: The line feature error is calculated by projecting the 3D feature onto the 2D plane through the back-projection function π; represents the weighted value of the image point feature, Represents 3D point features, R t and t t Respectively represent the rotation and translation of the camera pose at time t; the error term of the image line feature Represents the weighted value of image line features; represents 3D line features, and Respectively represent the starting point and end point of the 3D line feature, n j A vector representing the direction of the projection of a map line onto the plane.

[0087] S230: Extract constraint vectors, and generate image key frame information according to the constraint vectors.

[0088] In the embodiment of the present invention, the ground normal vector at time t can be extracted by 3F2N (Three-Filters-to-Normal, 3-way normal vector filter), and the normal vectors corresponding to the remaining pixels are calculated using the orthogonal structured environment space distance to form the constraint vector N t = {N0, ..., N n}.

[0089] Specifically, the constraint vector is extracted, and the image key frame information is generated according to the constraint vector, such as Figure 3 As shown, including:

[0090] S231, extracting the ground normal vector at the current moment;

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

[0092] S232, calculating the normal vector corresponding to the pixel in the image information according to the orthogonal structured environment space distance, to obtain the constraint vector;

[0093] According to the orthogonal structured environment space distance calculation, the normal vectors corresponding to the remaining pixels are calculated to form 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 to obtain the image key frame information, wherein the preset conditions at least include that a constraint vector is extracted at the current moment, or that a constraint vector is not extracted at the current moment, the current frame is more than a preset frame number away from the previous frame and the number of image point features is greater than a preset point feature number threshold, and the image line feature data is greater than a preset line feature number threshold.

[0095] It should be understood that if the constraint vector N is extracted at the current frame time t t = {N0, ..., N n}, then mark the frame at the current time t as the key frame KF t If the constraint vector N is not extracted at the current time t t = {N0, ..., N n}, you need to select the frame whose distance from the current frame to the previous frame is greater than the preset frame number and whose image point feature quantity is greater than the preset point feature quantity threshold and whose image line feature data is greater than the preset line feature quantity threshold to mark as the key frame KF t For example, you can select a frame that is more than 20 frames away from the previous frame, and the number of extracted point features is more than 45, and the number of line features is more than 10, and mark it as a key frame KF t .

[0096] In the embodiment of the present invention, after the key frames are extracted, dynamic adjustment weights may be added to the features based on the main direction constraints to optimize the map points, map lines, and camera poses.

[0097] Specifically, in an embodiment of the present invention, the image key frame information is subjected to geometric consistency verification, and the image key frame information after geometric consistency verification is subjected to local optimization processing to obtain local optimized map information and local optimized camera pose information, such as Figure 4 As shown, including:

[0098] S310, fusing the local map information contained in the image key frame information;

[0099] In the embodiment of the present invention, the local map information included in the image key frame information is fused, that is, the local map included in the key frame KF is fused to obtain the fused local map information.

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

[0101] In the embodiment of the present invention, for a local map point Map Line Optimize and remove 3D features with poor quality.

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

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

[0104] In an embodiment of the present invention, an optimization task is constructed according to the weight information of the map lines and the map points, and local optimization is performed on the local map information after the geometric consistency verification according to the optimization task to obtain the local optimized map information and the local optimized camera pose information, such as Figure 5 As shown, including:

[0105] S331, determining weight information of the map line according to the direction vector of the map line and the direction vector of the main direction constraint;

[0106] In the embodiment of the present invention, the correlation degree of the constraint can be determined by using the line feature and the main direction cosine value: Represents the direction vector of the map line, n i Represents the direction vector of the main direction constraint, ξ represents the correlation degree of the constraint, and can also be understood as the dynamic optimization weight of the line feature.

[0107] S332, constructing an optimization task according to the weight information of the map lines and the map points, and determining a cost function to be optimized;

[0108] Specifically, the weights of map point features and map line features are used to construct the g2o optimization task, and the cost function to be optimized is as follows:

[0109]

[0110] S333. Locally optimize the local map information that has passed the geometric consistency verification according to the optimization task and the cost function to be optimized to obtain local optimized map information and local optimized camera pose information.

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

[0112] Specifically, loop detection is performed according to the image key frame information, and corresponding main direction constraints are constructed according to different loop detection results, such as Figure 6 As shown, including:

[0113] S410, performing loop detection on the image key frame information according to a loop detection strategy;

[0114] It should be understood that STSL (Spatial-temporal sensitive Sub-global Map Loop) is used to detect whether the loop is completed, and a specific strategy is selected according to the detection result; 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 to the global area for matching during matching.

[0116] S420, determining whether a loop is detected in the loop detection;

[0117] It should be understood that STSL is used to access the global map and match the most recent keyframe with all keyframes containing the main direction constraints. If the number of point features that can be matched is greater than the preset point feature number threshold (for example, it can be 20) and the number of line features is greater than the preset line feature number threshold (for example, it can be 10), and the main direction constraint can be matched in two directions, then the loop is considered successful, that is, it is determined that a loop 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 in the loop frame and the key frame is greater than 20, and there are at least two line features corresponding to the main direction constraint that can be matched, the loop is considered to be completed and the constraint vector is not optimized; if the loop is not completed, the constraint vector is optimized.

[0119] S430: If a loop is detected in the loop detection, determine that the loop detection result is a completed loop, and use the main direction constraint condition included in the key frame detected by the loop as the main direction constraint condition of the current key frame;

[0120] It should be understood that if a loop is detected, the frame LKF detected by the loop is used. i The main direction constraint contained is used as the current keyframe KF i The main direction constraint MC j .

[0121] S440: If no loop is detected in the loop detection, determine that the loop detection result is an incomplete loop, and optimize the constraint vector of the current key frame according to the optimization algorithm to obtain the main direction constraint condition of the current key frame.

[0122] In the embodiment of the present invention, if no loop is detected, the MeanShift algorithm is used to roughly optimize the constraint vector of the current key frame, and then the KMeans clustering algorithm is used to optimize the main direction constraint MC j .

[0123] Specifically, the constraint vector of the current key frame is optimized according to the optimization algorithm to obtain the main direction constraint condition of the current key frame, including:

[0124] The constraint vector of the current key frame is roughly optimized according to the MeanShift algorithm, and a rough optimization result is obtained;

[0125] The rough optimization result is optimized according to the KMeans clustering algorithm to obtain the main direction constraint condition of the current key frame.

[0126] It should be understood that if no loop 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 projected onto the Gaussian sphere, and then all endpoints are numbered to represent the constraint vector to be optimized: N coarse ={n1, ..., n n}, the endpoint coordinates n of the constraint vector i =(x i ,y i , z i ) is the parameter to be optimized of the constraint vector.

[0128] In the embodiment of the present invention, a coarse-to-fine constraint optimization method is used. First, the mean shift algorithm of the cutting plane Gaussian kernel function is used to perform preliminary filtering on the constraint vector; then KMeans is used to process the endpoints, and the clustering loss function is defined as follows:

[0129]

[0130] Among them, n i represents the i-th sample, c i Indicates n i The cluster to which it belongs, represents the center point corresponding to the cluster, N represents the total number of sample endpoints, and only the cluster with larger density U = {u1, ..., u n}.

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

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

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

[0134] In an embodiment of the present invention, loop detection is performed according to the image key frame information, and corresponding main direction constraints are respectively constructed according to different loop detection results, and the following steps are also performed before the step of determining whether a loop is detected in the loop detection:

[0135] Determine whether the initialization is completed;

[0136] If initialization is not completed, the first key frame containing the constraint vector is optimized, and the initial main direction constraint condition is generated according to the optimized result;

[0137] If the initialization is completed, the step of determining whether a loop is detected is executed.

[0138] It should be understood that before loop detection, it is necessary to first determine whether the SLAM system has completed initialization, that is, to determine whether the initialization state quantity in the SLAM system has changed. If it has changed, it is determined that the initialization is completed. If it has not changed, the first key frame containing the constraint vector is optimized according to the timestamp information to optimize its constraint vector to generate the initial main direction constraint MC0. If it is determined that the initialization has been completed, the step of detecting loops can be continued.

[0139] In summary, the SLAM method based on the multi-level spatial structure provided by the present invention, when applied in an indoor environment, can extract and match point, line features and constraint vectors, select key frames, and eliminate features of poor quality, construct g2o optimization tasks according to the dynamic optimization weights added to the features, perform BA optimization, and use the STSL loop detection method, and use a coarse-to-fine constraint vector optimization method to obtain the main direction constraint, and perform global BA. This SLAM method based on the multi-level spatial structure of the present invention uses a more generalized structured hypothesis and introduces line features, which improves the robustness and accuracy of the visual SLAM system in indoor scene positioning and mapping; in addition, due to the use of dynamic weight adjustment factors, and constructing g2o optimization tasks according to the dynamic optimization weights added to the features, the efficiency of BA optimization is improved, and the positioning and mapping accuracy after optimization is improved; when detecting loops, by using STSL to detect loops, the loop method is more accurate and efficient, and can reduce the computational complexity and improve the loop accuracy.

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

[0141] An acquisition module 110 is used to acquire image information and image depth information at the same moment;

[0142] A preprocessing module 120, configured to preprocess the image information and the image depth information to obtain image key frame information;

[0143] A local optimization module 130 is used to perform geometric consistency verification on the image key frame information, and perform local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information;

[0144] A loop detection and main direction optimization module 140, configured to perform loop detection according to the image key frame information, and construct corresponding main direction constraint conditions according to different loop detection results;

[0145] A global optimization module 150, configured to globally optimize the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain the global optimized map information and the global optimized camera information;

[0146] The map construction module 160 is used to construct a map according to the global optimized map information and the global optimized camera information.

[0147] The SLAM device based on a multi-level spatial structure provided by the present invention obtains image key frame information after preprocessing image information and image depth information, performs local optimization processing after geometric consistency verification based on the image key frame information, and constructs main direction constraints after loop detection based on the image key frame information, and then globally optimizes the results after local optimization processing based on the main direction constraints, thereby realizing the final map construction. The SLAM device based on a multi-level spatial structure uses a dynamic optimization strategy based on the structured feature detection result to dynamically optimize the posture, and uses a loop detection method to suppress the accumulation of drift errors, thereby effectively reducing the computational complexity and improving the loop accuracy, thereby improving the robustness and accuracy of visual SLAM positioning and mapping in indoor scenes.

[0148] The specific working principle of the SLAM device based on the multi-level spatial structure provided by the present invention can be referred to the description of the SLAM method based on the multi-level spatial structure in the previous text, which 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 Fig. 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 the multi-level spatial structure is used to perform preprocessing according to image information and image depth information at the same time, and realize map construction based on local optimization processing, loop detection and main direction optimization processing and global optimization processing.

[0152] The SLAM system based on a multi-level spatial structure provided by the present invention adopts the SLAM device based on a multi-level spatial structure as mentioned above, uses a dynamic optimization strategy based on structured feature detection results to dynamically optimize the posture, and uses a loop detection method to suppress the accumulation of drift errors, thereby effectively reducing the computational complexity and improving the loop accuracy, thereby improving the robustness and accuracy of visual SLAM positioning and mapping in indoor scenes.

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

[0154] It is to be understood that the above embodiments are merely exemplary embodiments used to illustrate the principles of the present invention, but the present invention is not limited thereto. For those of ordinary skill 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: Obtain image information and image depth information at the same time; Preprocessing the image information and the image depth information to obtain image key frame information; Performing geometric consistency verification on the image key frame information, and performing local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information; Performing loop detection according to the image key frame information, and constructing corresponding main direction constraint conditions according to different loop detection results; Performing global optimization on the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain global optimized map information and global optimized camera information; A map is constructed according to the global optimized map information and the global optimized camera information.

2. The SLAM method based on a multi-level spatial structure according to claim 1, characterized in that: Preprocessing the image information and the image depth information to obtain image key frame information includes: Extracting features from the image information and the image depth information respectively to obtain image point features and image line features; Performing feature matching according to the image point features and the image line features, and calculating the camera pose according to the feature matching relationship; A constraint vector is extracted, and image key frame information is generated according to the constraint vector.

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

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

5. The SLAM method based on a multi-level spatial structure according to claim 4, characterized in that: An optimization task is constructed according to the weight information of the map lines and the map points, and local optimization is performed on the local map information after the geometric consistency verification according to the optimization task to obtain the local optimized map information and the local optimized camera pose information, including: Determine the weight information of the map line according to the direction vector of the map line and the direction vector of the main direction constraint; Construct optimization tasks based on the weight information of map lines and map points, and determine the cost function to be optimized; According to the optimization task and the cost function to be optimized, local map information that has passed geometric consistency verification is locally optimized to obtain local optimized map information and local optimized camera pose information.

6. The SLAM method based on a multi-level spatial structure according to claim 1, characterized in that: Loop detection is performed according to the image key frame information, and corresponding main direction constraint conditions are constructed according to different loop detection results, including: Performing loop closure detection on the image key frame information according to a loop closure detection strategy; Determining whether a loop is detected in the loop detection; If a loop is detected in the loop detection, determining that the loop detection result is a completed loop, and using the main direction constraint condition contained in the key frame detected by the loop as the main direction constraint condition of the current key frame; If no loop is detected in the loop detection, the loop detection result is determined to be an incomplete loop, and the constraint vector of the current key frame is optimized according to the optimization algorithm to obtain the main direction constraint condition of the current key frame.

7. The SLAM method based on a multi-level spatial structure according to claim 6, characterized in that: After optimizing the constraint vector of the current key frame according to the optimization algorithm, the main direction constraint conditions of the current key frame are obtained, including: The constraint vector of the current key frame is roughly optimized according to the MeanShift algorithm, and a rough optimization result is obtained; The rough optimization result is optimized according to the KMeans clustering algorithm to obtain the main direction constraint condition of the current key frame.

8. The SLAM method based on a multi-level spatial structure according to claim 6, characterized in that: Performing loop detection according to the image key frame information, and constructing corresponding main direction constraint conditions according to different loop detection results, and also including the following steps before determining whether a loop is detected in the loop detection: Determine whether initialization is currently completed; If initialization is not completed, the first key frame containing the constraint vector is optimized, and the initial main direction constraint condition is generated according to the optimized result; If the initialization is completed, the step of determining whether a loop is detected is executed.

9. A SLAM device based on a multi-level spatial structure, characterized in that: include: An acquisition module, used to acquire image information and image depth information at the same moment; A preprocessing module, used for preprocessing the image information and the image depth information to obtain image key frame information; A local optimization module is used to perform geometric consistency verification on the image key frame information, and perform local optimization processing on the image key frame information that has passed the geometric consistency verification to obtain local optimized map information and local optimized camera pose information; A loop detection and main direction optimization module, used to perform loop detection according to the image key frame information, and construct corresponding main direction constraint conditions according to different loop detection results; A global optimization module, used for globally optimizing the local optimized map information and the local optimized camera pose information according to the main direction constraint condition to obtain the global optimized map information and the global optimized camera information; A map construction module is used to construct a map according to the global optimized map information and the global optimized camera information.

10. A SLAM system based on a multi-level spatial structure, characterized in that: include: An image acquisition device and a SLAM device based on a multi-level spatial structure as claimed in claim 9, 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 the multi-level spatial structure is used to perform preprocessing according to image information and image depth information at the same time, and realize map construction based on local optimization processing, loop detection and main direction optimization processing and global optimization processing.

Images