SLAM loopback detection method and device and positioning equipment
By adopting polar line search and affine transform block matching technology in SLAM technology and combining feature descriptor verification, the success rate and positioning accuracy of SLAM loopback detection are improved, and the problem of low success rate of loopback detection in the prior art is solved.
Patent Information
- Application Number
- CN202411718821.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-11-27
- Publication Date
- 2025-04-25
AI Technical Summary
The success rate of loopback detection in existing SLAM technology is not high, mainly due to the inaccurate matching of feature points between the historical frame image and the current frame image during the feature matching process, resulting in low positioning accuracy.
By selecting a historical frame image similar to the current frame image from the SLAM map, calculating the initial relative pose, determining the pole lines of the feature points in the historical frame image in the current frame image, using an affine transformation relationship to match the feature points, and verifying the accuracy of the matching pair through the feature descriptor.
It improves the success rate and accuracy of SLAM loopback detection, and enhances positioning accuracy and robustness.
Smart Images
Figure CN119888470A_ABST
Abstract
Description
Background Art
[0002] At present, most positioning devices (such as mobile robots, cruise devices, head-mounted display devices (Head Mount Display, HMD)), etc.) are based on visual-inertial odometry (VIO) for positioning.
[0003] Taking head-mounted display devices as an example, users interact with the virtual world based on the positioning results of the head by the head-mounted display device. However, when users wear head-mounted display devices for immersive experience, they usually move continuously in a large space, and VIO will produce cumulative errors after running for a period of time. Loop detection is needed to determine whether the user has returned to the area he has passed before, so as to correct the accumulated errors. Therefore, loop detection is an important part of the complete SLAM process.
[0004] In the feature matching process of traditional loop detection technology, in order to improve the positioning accuracy of VIO in SLAM technology and calculate more accurate depth information, a small number of high-precision feature points are usually selected for feature matching. Moreover, when a loop occurs, the historical frame image and the current frame image are likely to be the same scene observed from different distances and angles. This results in the current frame image not necessarily mentioning the feature points at the same position as the historical frame image, and the number of feature points with accurate depth originally selected is relatively small, resulting in a low success rate of SLAM loop detection.
[0005] Therefore, improving the success rate of loop detection has become an urgent problem to be solved in SLAM technology. Summary of the invention
[0006] The embodiments of the present application provide a SLAM loop detection method, apparatus and positioning device, which are used to improve the success rate of SLAM loops and thus improve positioning accuracy.
[0007] In a first aspect, an embodiment of the present application provides a SLAM loop detection method, comprising:
[0008] Selecting a historical frame image similar to the current frame image from the constructed SLAM map, and calculating the initial relative pose between the current frame image and the historical frame image;
[0009] Determine, according to the initial relative position and posture, an epipolar line of a first feature point in the current frame image; wherein the first feature point is any feature point extracted from the historical frame image;
[0010] Determine an affine transformation relationship between the historical frame image and the current frame image according to the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line;
[0011] Determine, according to the affine transformation relationship, a second feature point that matches the first feature point in the current frame image;
[0012] Determining whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to feature descriptors of the first feature point and the second feature point;
[0013] Loop correction is performed based on the retained feature point matching pairs.
[0014] The beneficial effects of the above technical solution are as follows: after filtering out the historical frame image from the SLAM map, by determining the polar line of the first feature point in the historical frame image in the current frame image, and the corresponding projection block of the image block of the first feature point on the polar line, the feature tracking of the first feature point with accurate depth information in the historical frame image is realized in the current frame image, thereby increasing the number of feature point matching pairs between the current frame image and the historical frame image, and ensuring the success rate and robustness of the SLAM loop. On the other hand, by calculating the descriptor of the first feature point in the historical frame image and the second feature point in the current frame image, it is determined whether to retain the feature point matching pair consisting of the first feature point and the second feature point, thereby realizing the verification of the feature tracking result using the feature matching result, ensuring the accuracy of the feature point matching pair used in the SLAM loop, and thus improving the accuracy of the SLAM loop.
[0015] Optionally, determining the affine transformation relationship between the historical frame image and the current frame image according to the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line includes:
[0016] An image block centered on the first feature point is constructed in the historical frame image, and pixel points contained in the image block are projected onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain a projection block corresponding to the image block on the epipolar line; wherein the projection point corresponding to the first feature point is located on the epipolar line;
[0017] According to the grayscale information of the pixel points contained in the image block and the grayscale information of the projection points contained in the projection block, the affine transformation relationship between the historical frame image and the current frame image is determined by using the least squares method.
[0018] The beneficial effects of the above technical solution are: using block matching technology based on affine transformation, according to the initial relative posture between the historical frame image and the current frame image and the depth information corresponding to the historical frame image, the projection block of the image block of the first feature point in the historical frame image on the polar line in the current frame image is determined, so that according to the grayscale information of the image block and the projection block, the affine transformation relationship for tracking the first feature point in the current frame image is determined, and a fine search for a single feature point is achieved. To a certain extent, the influence of scale and rotation can be overcome, the accuracy of feature tracking can be improved, and the success rate of SLAM loop can be improved.
[0019] Optionally, the determining whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to the feature descriptors of the first feature point and the second feature point includes:
[0020] Calculating a first descriptor of the first feature point;
[0021] According to the affine transformation relationship, sampling the neighborhood of the second feature point in the current frame image to obtain a plurality of sampling points;
[0022] Calculating and weighting the descriptors of the plurality of sampling points, and using the weighted descriptors as the descriptors of the second feature points;
[0023] If the similarity between the first descriptor and the second descriptor is greater than a preset similarity threshold, the feature point matching pair consisting of the first feature point and the second feature point is retained, otherwise the feature point matching pair consisting of the first feature point and the second feature point is discarded.
[0024] The beneficial effect of the above technical solution is: by performing neighborhood resampling of the second feature point matched by the first feature point in the current frame image, and using the weighted result of the sampling point descriptor as the descriptor of the second feature point, the influence of scale and rotation on feature matching is reduced, the accuracy of feature matching is improved, and the accuracy of SLAM loop is thereby improved.
[0025] Optionally, determining the epipolar line of the first feature point in the current frame image according to the initial relative pose includes:
[0026] Acquire depth information of the first feature point;
[0027] Determine an epipolar line of the first feature point in the current frame image according to the depth information and the initial relative position and posture.
[0028] The beneficial effects of the above technical solution are: using the polar line search technology to determine the polar line of the first feature point in the historical frame image in the current frame image, to achieve a rough search of a single feature point in the current frame image, to eliminate the impact of the feature point depth calculation error, and to increase the accuracy and speed of feature tracking.
[0029] Optionally, the calculating the initial relative pose between the current frame image and the historical frame image includes:
[0030] Acquire a plurality of neighborhood points around the first feature point;
[0031] According to the grayscale information of the first feature point and the plurality of neighborhood points, combined with a preset initial pose value, a least squares method is used to determine an initial relative pose between the current frame image and the historical frame image.
[0032] The beneficial effects of the above technical solution are: based on the photometric invariance assumption, using the image alignment technology of sparse feature points, according to the grayscale information of the first feature point and the neighborhood point, the initial relative pose between the current frame image and the historical frame image is obtained, thereby providing a better initial value for feature tracking and speeding up the feature tracking.
[0033] Optionally, the least squares formula of the initial relative posture is expressed as:
[0034]
[0035] Among them, m is the number of first feature points, n is the number of neighborhood points, α is the photometric scaling factor, β is the photometric shift factor, π(·) and π(·) -1 are the forward projection function and the reverse projection function of the camera model, respectively, d old is the depth information of the first feature point in the camera coordinate system of the historical frame image, is the initial relative pose between the current frame image and the historical frame image, and I(·) is the grayscale information.
[0036] The beneficial effect of the above technical solution is: when the feature points in the current frame image are unknown, based on the rough initial pose value, the solution process of the initial relative pose between the current frame image and the historical frame image is converted into a least squares problem, thereby obtaining a more accurate relative pose and speeding up the feature tracking.
[0037] In a second aspect, an embodiment of the present application provides a SLAM loop detection device, comprising:
[0038] A frame matching module is used to select a historical frame image similar to the current frame image from the constructed SLAM map, and calculate the initial relative pose between the current frame image and the historical frame image;
[0039] An epipolar line search module, used to determine an epipolar line of a first feature point in the current frame image according to the initial relative position; wherein the first feature point is any feature point extracted from the historical frame image;
[0040] A block matching module, configured to determine an affine transformation relationship between the historical frame image and the current frame image based on the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line; and determine a second feature point matched by the first feature point in the current frame image based on the affine transformation relationship;
[0041] A verification module, used for determining whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to feature descriptors of the first feature point and the second feature point;
[0042] The correction module is used to perform loop correction based on the retained feature point matching pairs.
[0043] Optionally, the block matching module is specifically used for:
[0044] An image block centered on the first feature point is constructed in the historical frame image, and pixel points contained in the image block are projected onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain a projection block corresponding to the image block on the epipolar line; wherein the projection point corresponding to the first feature point is located on the epipolar line;
[0045] According to the grayscale information of the pixel points contained in the image block and the grayscale information of the projection points contained in the projection block, the affine transformation relationship between the historical frame image and the current frame image is determined by using the least squares method.
[0046] Optionally, the verification module is specifically used for:
[0047] Calculating a first descriptor of the first feature point;
[0048] According to the affine transformation relationship, sampling the neighborhood of the second feature point in the current frame image to obtain a plurality of sampling points;
[0049] Calculating and weighting the descriptors of the plurality of sampling points, and using the weighted descriptors as the descriptors of the second feature points;
[0050] If the similarity between the first descriptor and the second descriptor is greater than a preset similarity threshold, the feature point matching pair consisting of the first feature point and the second feature point is retained, otherwise the feature point matching pair consisting of the first feature point and the second feature point is discarded.
[0051] Optionally, the epipolar line search module is specifically used for:
[0052] Acquire depth information of the first feature point;
[0053] Determine an epipolar line of the first feature point in the current frame image according to the depth information and the initial relative position and posture.
[0054] Optionally, the frame matching module is specifically used for:
[0055] Acquire a plurality of neighborhood points around the first feature point;
[0056] According to the grayscale information of the first feature point and the plurality of neighborhood points, combined with a preset initial pose value, a least squares method is used to determine an initial relative pose between the current frame image and the historical frame image.
[0057] Optionally, the least squares formula of the initial relative posture is expressed as:
[0058]
[0059] Among them, m is the number of first feature points, n is the number of neighborhood points, α is the photometric scaling factor, β is the photometric shift factor, π(·) and π(·) -1 are the forward projection function and the reverse projection function of the camera model, respectively, d old is the depth information of the first feature point in the camera coordinate system of the historical frame image, is the initial relative pose between the current frame image and the historical frame image, and I(·) is the grayscale information.
[0060] In a third aspect, an embodiment of the present application provides a positioning device, including a processor, a memory, and a multi-eye camera, wherein the multi-eye camera, the memory, and the processor are connected via a bus;
[0061] The multi-camera is used to collect scene images;
[0062] The memory stores a computer program, and the processor executes the steps of any one of the SLAM loop detection methods provided in the first aspect according to the computer program.
[0063] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores computer-executable instructions, and when the computer-executable instructions are executed, the steps of any one of the SLAM loop detection methods provided in the first aspect can be implemented.
[0064] The technical effects brought about by any implementation method in the second to fourth aspects can refer to the technical effects brought about by the corresponding implementation method in the first aspect, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS
[0065] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0066] Figure 1 A flow chart of the SLAM loop detection method provided in the embodiment of the present application;
[0067] Figure 2 A schematic diagram of the relationship between the previous and next frames provided in an embodiment of the present application;
[0068] Figure 3 A schematic diagram of a head mounted display device using a SLAM loop detection method provided in an embodiment of the present application;
[0069] Figure 4 A schematic diagram of the polar line search principle provided in an embodiment of the present application;
[0070] Figure 5 A flowchart of a block matching method based on affine transformation provided in an embodiment of the present application;
[0071] Figure 6 A schematic diagram of image block projection provided in an embodiment of the present application;
[0072] Figure 7 A flow chart of a feature tracking result verification method provided in an embodiment of the present application;
[0073] Figure 8 A schematic diagram of feature point resampling provided in an embodiment of the present application;
[0074] Fig. 9 A system architecture diagram of SLAM provided in an embodiment of the present application;
[0075] Fig.10 A structural diagram of a SLAM loop detection device provided in an embodiment of the present application;
[0076] Fig.11 A structural diagram of a positioning device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0077] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the technical solution of the present application, rather than all of the embodiments. Based on the embodiments recorded in the application documents, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the technical solution of the present application.
[0078] Based on the exemplary embodiments shown in this application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application. In addition, although the disclosure in this application is introduced according to one or several exemplary examples, it should be understood that each aspect of these disclosures can also constitute a complete technical solution separately.
[0079] It should be noted that the brief description of terms in this application is only for the convenience of understanding the embodiments described below, and is not intended to limit the embodiments of this application. Unless otherwise specified, these terms should be understood according to their common and usual meanings.
[0080] The terms "first", "second", etc. in the specification and claims of this application and the above drawings are used to distinguish similar or similar objects or entities, and do not necessarily mean to limit a specific order or sequence, unless otherwise indicated. It should be understood that the terms used in this way can be interchangeable under appropriate circumstances, for example, they can be implemented in an order other than those given in the diagrams or descriptions of the embodiments of this application.
[0081] In addition, the terms "include" and "have" and any variations thereof are intended to cover but not exclude inclusion, for example, a product or device comprising a list of components is not necessarily limited to those components expressly listed but may include other components not expressly listed or inherent to such products or devices.
[0082] The term "module" as used in this application refers to any known or later developed hardware, software, firmware, artificial intelligence, fuzzy logic, or combination of hardware and / or software code that is capable of performing the functions associated with that element.
[0083] The following is an overview of the design concept of the embodiments of the present application in conjunction with application scenarios.
[0084] At present, most traditional loop detection methods combine the bag-of-words model with feature matching. First, the bag-of-words model is used to find historical frame images similar to the current frame scene, and then feature points are extracted and descriptors are calculated in the current frame image. They are matched with the feature points and descriptors of the historical frame images whose depths have been accurately estimated. When the number of matches is large enough, it is determined that a loop has occurred between the two frames, and the PnP algorithm is used to calculate the relative posture between the two frames. However, in order to ensure the positioning accuracy of VIO in SLAM, a small number of high-precision feature points are usually selected for feature matching. This results in a small number of feature points with accurate depth extracted from the historical frame images. In addition, since it is impossible to pre-judge where these feature points in the historical frame images will appear in the current frame images, enough feature points will be extracted from the current frame images to match the feature points with accurate depth in the historical frame images. However, when a loop occurs, the historical frame image and the current frame image may be the same scene observed from different distances and angles. This results in the current frame image not necessarily mentioning features at the same position as in the historical frame image. In addition, the number of feature points with accurate depth in the historical frame images is relatively small, resulting in low feature matching accuracy and insufficient number of point pairs, which in turn leads to inaccurate or failed loop detection.
[0085] In view of this, an embodiment of the present application provides a SLAM loop detection method, which uses a frame matching technology based on a bag-of-words model, a feature tracking technology based on polar line search and block matching, and a feature matching technology to perform SLAM loop detection. By adding a feature tracking link, feature points with accurate depth in historical frame images are tracked in the current frame image, and the feature matching results are used to verify the feature tracking effect, which effectively improves the number of feature matches and the matching accuracy, thereby effectively improving the success rate of SLAM loop detection.
[0086] It should be noted that the SLAM loop detection method provided in the embodiments of the present application can be applied to head-mounted display devices, mobile robots, cruise devices, etc.
[0087] like Figure 1 As shown in FIG. 1 , the SLAM loop detection method process provided by the embodiment of the present application mainly includes the following steps:
[0088] S101: Select a historical frame image similar to the current frame image from the constructed SLAM map, and calculate the initial relative pose between the current frame image and the historical frame image.
[0089] In one example, after acquiring the current frame image, the bag-of-words model is used to calculate the scene similarity between the image in the constructed SLAM map and the current frame image, and the frame image with the highest similarity is used as the historical frame image for generating the loop.
[0090] Optionally, the embodiments of the present application do not impose restrictive requirements on the features used for scene similarity calculation, including but not limited to texture features, edge features, and grayscale histogram features.
[0091] Usually, the same 3D point in space may be observed in two frames of images taken while moving, such as Figure 2 As shown. According to the photometric invariance hypothesis, the photometric value of the same 3D point in space on different images changes linearly. Assuming (u old ,v old ) and (u cur ,v cur ) are the position coordinates of the same 3D image in the historical frame image and the current frame image respectively, then the following assumptions hold:
[0092] I old (u old ,v old )=αI cur (u cur ,v cur )+β Formula 1
[0093] Among them, α is the photometric scaling factor, β is the photometric shift coefficient, and I represents the grayscale value.
[0094] At the same time, the position coordinates of the same 3D point in space on different images satisfy the following camera imaging principle:
[0095]
[0096] Among them, π(·) and π(·) -1 are the forward projection function and the reverse projection function of the camera model, respectively, d old is the depth information of the historical frame image, is the initial relative pose between the current frame image and the historical frame image.
[0097] Take head mounted display devices as an example. Figure 3 As shown, the head-mounted display device includes multiple cameras, and each frame of image includes images synchronously captured by the multiple cameras. Based on the images captured by the multiple cameras, the depth information of each frame of image can be calculated.
[0098] When performing SLAM loop closure, it is necessary to extract feature points in the image. The embodiment of the present application does not impose any restrictive requirements on the feature point extraction method, including but not limited to feature extraction algorithms such as ORB, Brief, SHIF, and SURF.
[0099] When the scenes of the historical frame image that generates the loop are similar to those of the current frame image and the similarity is high enough, feature points with accurate depth information in the historical frame image can be used to estimate the initial relative pose between the historical frame image and the current frame image.
[0100] Assuming that any feature point extracted from the historical frame image is recorded as the first feature point, based on multiple first feature points in the historical frame image, the initial relative pose between the current frame image and the historical frame image can be calculated. The process is as follows:
[0101] First, multiple neighborhood points around the first feature point are obtained.
[0102] Then, according to the grayscale information of the first feature point and multiple neighborhood points, combined with the preset initial pose value, the least squares method is used to determine the initial relative pose between the current frame image and the historical frame image.
[0103] Taking head-mounted display devices as an example, the device is usually given a calibrated camera pose when it leaves the factory. This pose is used as the initial pose value of the least squares method, and the initial relative pose between the current frame image and the historical frame image is calculated using each first feature point and multiple neighborhood points around it.
[0104] In an embodiment of the present application, when the feature points in the current frame image are unknown, the process of solving the initial relative pose between the current frame image and the historical frame image is converted into a least squares problem based on a rough initial pose value, thereby obtaining a more accurate relative pose and speeding up feature tracking.
[0105] Specifically, the formula of the least squares method is expressed as:
[0106]
[0107] Wherein, m is the number of first feature points in the historical frame image, and n is the number of neighborhood points around each first feature point.
[0108] By substituting the position coordinates of the first feature point (u old ,v old ) and its depth information d in the camera coordinate system of the historical frame image old , the above least squares problem can be solved to obtain the initial relative pose between the current frame image and the historical frame image.
[0109] In an embodiment of the present application, based on the grayscale information of the sparse first feature points and neighborhood points with accurate depth information in the historical image, and based on the photometric invariance assumption, an image alignment technology of sparse feature points is implemented to obtain the initial relative pose between the current frame image and the historical frame image, thereby providing a better initial value for feature tracking and speeding up the feature tracking.
[0110] S102: Determine the epipolar line of the first feature point in the current frame image according to the initial relative position and posture.
[0111] After obtaining the initial relative pose between the two frames, the first feature points with depth information in the historical frame images can be roughly tracked one by one based on the epipolar line search.
[0112] like Figure 4 The figure shows the principle of epipolar line search. The 3D points on the same ray as the optical center will fall on the same position coordinates of the image, and the 3D points on this ray will fall on the epipolar line of another image. Therefore, the first feature point in the historical frame image will have a corresponding point on the epipolar line of the current frame image. In other words, when the epipolar line of the first feature point in the current frame image is found, the feature point is roughly tracked.
[0113] Specifically, the process of determining the epipolar line of the first feature point in the current frame image is as follows:
[0114] First, the depth information of the first feature point is obtained.
[0115] by Figure 3 Taking the head-mounted display device including binocular cameras shown as an example, the historical frame images include historical left-eye images captured by the left camera and historical right-eye images captured by the right camera. When the left camera and the right camera have been calibrated, that is, the intrinsic parameters and extrinsic parameters of the camera are known, at this time, the depth information of the first feature point can be determined based on the parallax of the same first feature point in the images captured by the two cameras.
[0116] Then, based on the depth information and the initial relative position, determine the epipolar line of the first feature point in the current frame image
[0117] When the initial relative position between the two frames is known, the epipolar line of the first feature point in the current frame can be determined in combination with the depth information of the first feature point.
[0118] In an embodiment of the present application, an epipolar line search technique is used to determine the epipolar line of the first feature point in the historical frame image in the current frame image, thereby achieving a rough search of a single feature point in the current frame image, eliminating the impact of the feature point depth calculation error, and increasing the accuracy and speed of feature tracking.
[0119] S103: Determine an affine transformation relationship between the historical frame image and the current frame image based on the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line.
[0120] Taking into account that the grayscale value of a single pixel may not be stable enough, after using the epipolar line search to obtain the rough position of the first feature point in the current frame image, a block matching strategy based on affine transformation is used to perform a fine search for the first feature point in the current frame image, thereby tracking the matching point with sub-pixel accuracy.
[0121] Specifically, the block matching process based on affine transformation is as follows: Figure 5 As shown, it mainly includes the following steps:
[0122] S1031: construct an image block centered on the first feature point in the historical frame image, and project the pixel points contained in the image block onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain the projection block corresponding to the image block on the epipolar line.
[0123] For a point in the historical frame image, after knowing the depth information of the point in the historical frame image and the initial relative position between the historical frame image and the current frame image, the point in the historical frame image can be projected into the current frame image. Therefore, for each pixel point in the image block corresponding to the first feature point, there is a corresponding projection point in the current frame image, and these projection points can form a projection block. Among them, based on the principle of epipolar line search, it can be known that the projection point of the first feature point in the current frame image must be located on the epipolar line.
[0124] Take a 3*3 image block as an example, Figure 6 As shown, after the image block of the first feature point in the historical frame image is projected onto the current frame image, a plurality of discrete projection points can be obtained near the polar line of the first feature point to form a projection block.
[0125] S1032: Determine the affine transformation relationship between the historical frame image and the current frame image using the least squares method according to the grayscale information of the pixel points included in the image block and the grayscale information of the projection points included in the projection block.
[0126] Assuming that the depth information of the image block in the neighborhood near the first feature point changes smoothly and is close to a plane, the position coordinates of the midpoint of the image block satisfy the affine transformation, which is expressed as follows:
[0127]
[0128] Among them, (u, v) is the position coordinate of the midpoint of the image block, (u ′ ,v ′ ) is the position coordinate of the midpoint of the projection block, A is a 2*2 matrix, also called an affine matrix, b is a 2*1 vector, and (A, b) represents the affine transformation relationship between the historical frame image and the current frame image.
[0129] After knowing the position coordinates of multiple points in the image block and the projection block, the initial values of A and b can be solved. Similarly, based on the assumption of photometric invariance, the least squares solution is performed on each image block of the first feature point to obtain the optimized A and b. The least squares optimization formula is expressed as follows:
[0130]
[0131] Where n is the number of points contained in the image block.
[0132] S104: Determine, according to the affine transformation relationship, a second feature point that matches the first feature point in the current frame image.
[0133] After obtaining the optimized A and b through Formula 7, they are substituted into Formula 6 to obtain the second feature point that matches the first feature point in the current frame image.
[0134] In an embodiment of the present application, a block matching technology based on affine transformation is used to determine the projection block of the image block of the first feature point in the historical frame image on the polar line in the current frame image according to the initial relative pose between the historical frame image and the current frame image and the depth information of the midpoint of the historical frame image. Thus, based on the grayscale information of the image block and the projection block, the affine transformation relationship for tracking the first feature point in the current frame image is determined, and a fine search for a single feature point is achieved to obtain a second matching point with sub-pixel accuracy. This method can overcome the influence of scale and rotation to a certain extent, improve the accuracy of feature tracking, and thereby improve the success rate of SLAM loopback.
[0135] S105: Determine whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to the feature descriptors of the first feature point and the second feature point.
[0136] To ensure the accuracy of feature tracking, the feature tracking result can be verified by calculating the similarity of the descriptors of the first feature point and the second feature point, thereby ensuring the accuracy of the feature point matching pair during the SLAM loop and ensuring the accuracy and robustness of positioning.
[0137] Since the two frames after loop closure may observe the same three points at different scales and angles, the scale and rotation characteristics of the features can be taken into account when verifying the feature tracking results. The specific verification process is as follows: Figure 7 As shown, it mainly includes the following steps:
[0138] S1051: Calculate the first descriptor of the first feature point.
[0139] The first descriptor is obtained by using a traditional descriptor calculation method.
[0140] S1052: According to the affine transformation relationship, a neighborhood of the second feature point in the current frame image is sampled to obtain a plurality of sampling points.
[0141] Take the 3*3 size neighborhood as an example, Figure 8 As shown, using the optimized A and b, the points in the neighborhood of the second feature point in the current frame image are resampled to a sampling block of 3*3 size.
[0142] S1053: Calculate and weight the descriptors of the multiple sampling points, and use the weighted descriptors as the descriptors of the second feature points.
[0143] The weights of the sampling points may be the same or different.
[0144] When the weights of the sampling points are different, different weights may be assigned according to the distances between each sampling point and the second feature point.
[0145] S1054: Calculate the similarity between the first descriptor and the second descriptor.
[0146] It should be noted that the embodiments of the present application do not impose any restrictive requirements on the calculation method of the descriptor similarity. For example, the Hamming distance or the 2-norm may be used.
[0147] S1055: Determine whether the similarity is greater than a pre-set similarity threshold, if so, execute S1056, otherwise execute S1057.
[0148] S1056: retain the feature point matching pair consisting of the first feature point and the second feature point.
[0149] When the similarity between the descriptors is greater than the preset similarity threshold, it indicates that the tracking result of the first feature point in the historical frame image in the current frame image is relatively accurate, that is, the second feature point in the current frame image and the first feature point are the same 3D point in space, so the feature point matching pair consisting of the first feature point and the second feature point is retained.
[0150] S1057: Eliminate the feature point matching pair consisting of the first feature point and the second feature point.
[0151] When the similarity between the descriptors is not greater than the preset similarity threshold, it indicates that the tracking result of the first feature point in the historical frame image in the current frame image is not very accurate, so the feature point matching pair consisting of the first feature point and the second feature point is eliminated.
[0152] In an embodiment of the present application, by performing neighborhood resampling on the second feature point that matches the first feature point in the current frame image, and using the weighted result of the sampling point descriptor as the descriptor of the second feature point, the influence of scale and rotation on feature matching is reduced, the accuracy of feature matching is improved, and the accuracy of SLAM loop closure is thereby improved.
[0153] S106: Perform loop correction based on the retained feature point matching pairs.
[0154] The accuracy of the feature point matching pairs retained after verification is relatively high. Therefore, when performing SLAM loopback, the accuracy of SLAM positioning can be effectively improved and the success rate of loopback can be guaranteed.
[0155] In an embodiment of the present application, after filtering out the historical frame image from the SLAM map, by determining the polar line of the first feature point in the historical frame image in the current frame image, and the corresponding projection block of the image block of the first feature point on the polar line, the feature tracking of the first feature point in the historical frame image with accurate depth information is realized in the current frame image, thereby increasing the number of feature point matching pairs between the current frame image and the historical frame image, and ensuring the success rate and robustness of the SLAM loop. On the other hand, by calculating the descriptor of the first feature point in the historical frame image and the second feature point in the current frame image, it is determined whether to retain the feature point matching pair consisting of the first feature point and the second feature point, thereby realizing the verification of the feature tracking result using the feature matching result, ensuring the accuracy of the feature point matching pair used by the SLAM loop, and thus improving the accuracy of the SLAM loop.
[0156] See also Fig. 9 , which is a system architecture diagram of the SLAM provided in the embodiment of the present application, mainly including visual odometer (front-end estimation), back-end optimization, loop detection and construction Figure 4 Part of the content. Among them, the visual odometry mainly processes the input image, calculates the transformation relationship between image frames, and completes real-time pose calculation; the back-end optimization mainly uses optimization algorithms to eliminate the errors of pose and 3D points; loop detection, also known as closed loop detection, mainly uses the similarity between images to determine whether the previous position has been reached, so as to eliminate the accumulated error; map construction builds a map corresponding to the task requirements based on the estimated trajectory. To ensure the success rate of loop detection, this part uses sparse feature point image alignment technology to calculate the initial relative pose between the current frame image and the historical frame image after selecting the historical frame image with similar scenes to the current frame image. Then, based on the initial relative pose, the polar line search technology is used to roughly track the feature points in the historical frame image in the current frame image. Based on the rough tracking results, the block matching technology of affine transformation is used to perform sub-pixel fine tracking of the feature points in the historical frame image in the current frame image, thereby increasing the number of feature point matching pairs. In addition, the feature matching results in the current frame image and the historical frame images are used to verify the feature tracking results, thereby ensuring the accuracy of the feature point matching pairs, thereby improving the positioning accuracy of SLAM and ensuring the success rate of SLAM loops.
[0157] Based on the same technical concept, an embodiment of the present application provides a SLAM loop detection device, which can implement the steps of the above-mentioned SLAM loop detection method and achieve the same technical effect.
[0158] See also Fig.10 The SLAM loop detection device includes a frame matching module 1001, an epipolar line search module 1002, a block matching module 1003, a verification module 1004 and a correction module 1005, wherein:
[0159] The frame matching module 1001 is used to select a historical frame image similar to the current frame image from the constructed SLAM map, and calculate the initial relative pose between the current frame image and the historical frame image;
[0160] The epipolar line search module 1002 is used to determine the epipolar line of the first feature point in the current frame image according to the initial relative position and posture; wherein the first feature point is any feature point extracted from the historical frame image;
[0161] The block matching module 1003 is used to determine the affine transformation relationship between the historical frame image and the current frame image based on the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line; and determine the second feature point that matches the first feature point in the current frame image based on the affine transformation relationship;
[0162] A verification module 1004 is used to determine whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to feature descriptors of the first feature point and the second feature point;
[0163] The correction module 1005 is used to perform loop correction based on the retained feature point matching pairs.
[0164] Optionally, the block matching module 1003 is specifically used for:
[0165] An image block centered on the first feature point is constructed in the historical frame image, and the pixel points contained in the image block are projected onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain a projection block corresponding to the image block on the epipolar line; wherein the projection point corresponding to the first feature point is located on the epipolar line;
[0166] According to the grayscale information of the pixel points contained in the image block and the grayscale information of the projection points contained in the projection block, the affine transformation relationship between the historical frame image and the current frame image is determined by using the least squares method.
[0167] Optionally, the verification module 1005 is specifically used for:
[0168] Calculate a first descriptor of a first feature point;
[0169] According to the affine transformation relationship, sampling is performed on the neighborhood of the second feature point in the current frame image to obtain a plurality of sampling points;
[0170] Calculate the descriptors of multiple sampling points and weight them, and use the weighted descriptors as the descriptors of the second feature points;
[0171] If the similarity between the first descriptor and the second descriptor is greater than a preset similarity threshold, the feature point matching pair consisting of the first feature point and the second feature point is retained, otherwise the feature point matching pair consisting of the first feature point and the second feature point is discarded.
[0172] Optionally, the epipolar line search module 1002 is specifically used for:
[0173] Obtaining depth information of the first feature point;
[0174] According to the depth information and the initial relative position and posture, the epipolar line of the first feature point in the current frame image is determined.
[0175] Optionally, the frame matching module 1001 is specifically used for:
[0176] Obtain multiple neighborhood points around the first feature point;
[0177] According to the grayscale information of the first feature point and multiple neighborhood points, combined with the preset initial pose value, the least squares method is used to determine the initial relative pose between the current frame image and the historical frame image.
[0178] Optionally, the least squares formula for the initial relative pose is expressed as:
[0179]
[0180] Among them, m is the number of first feature points, n is the number of neighborhood points, α is the photometric scaling factor, β is the photometric shift factor, π(·) and π(·) -1 are the forward projection function and the reverse projection function of the camera model, respectively, d old is the depth information of the first feature point in the camera coordinate system of the historical frame image, is the initial relative pose between the current frame image and the historical frame image, and I(·) is the grayscale information.
[0181] For the convenience of description, the above parts are divided into modules (or units) according to their functions and described separately. Of course, when implementing this application, the functions of each module (or unit) can be implemented in the same or multiple software or hardware.
[0182] After introducing the SLAM loop detection method and apparatus according to an exemplary embodiment of the present application, next, a positioning device according to another exemplary embodiment of the present application is introduced.
[0183] Those skilled in the art will appreciate that various aspects of the present application may be implemented as a system, method or program product. Therefore, various aspects of the present application may be specifically implemented in the following forms, namely: a complete hardware implementation, a complete software implementation (including firmware, microcode, etc.), or a combination of hardware and software, which may be collectively referred to as "circuit", "module" or "system" herein.
[0184] Based on the same inventive concept as the above method embodiment, the positioning device provided in the embodiment of the present application can be a head-mounted display device, a mobile robot, a cruise device, etc. In this embodiment, the structure of the positioning device can be as follows: Fig.11 As shown, it includes a processor 1101, a memory 1102 and a multi-eye camera 1103;
[0185] The multi-eye camera 1103 is used to collect scene images;
[0186] The memory 1102 stores a computer program, and the processor 1101 executes the steps of any one of the SLAM loop detection methods in the above embodiments according to the computer program.
[0187] In the embodiment of the present application, the memory 1102 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, and programs required to run the instant messaging function, etc.; the data storage area may store various instant messaging information and operation instruction sets, etc. The memory 1102 may be a volatile memory (volatile memory), such as a random-access memory (RAM); the memory may also be a non-volatile memory (non-volatile memory), such as a read-only memory, a flash memory (flash memory), a hard disk drive (HDD) or a solid-state drive (SSD); or the memory 1102 may be any other medium that can be used to carry or store a desired computer program in the form of an instruction or data structure and can be accessed by a computer, but is not limited thereto. The memory 1102 may be a combination of the above memories.
[0188] The processor 1101 may include one or more central processing units (CPU), GPU or a digital processing unit, etc.
[0189] In the embodiment of the present application, the specific connection medium between the multi-eye camera 1103, the memory 1102 and the processor 1101 is not limited. In the embodiment of the present application, the bus 1104 between the multi-eye camera 1103, the memory 1102 and the processor 1101 is Fig.11 The connections between the other components are described with bold lines, which are only for illustration and are not intended to be limiting. The bus 1104 can be divided into an address bus, a data bus, a control bus, etc. For ease of description, Fig.11 The diagram shows that only one thick line is used, but this does not mean that there is only one bus or only one type of bus.
[0190] It should be noted that Fig.11 It is only the necessary equipment for the positioning device to implement the SLAM loop detection in the embodiment of the present application. Not shown, the positioning device may also include the hardware of conventional positioning devices such as IMU, speakers, microphones, power supplies, buttons, etc.
[0191] The embodiment of the present application also provides a computer-readable storage medium for storing some instructions, which, when executed, can complete the steps of any one of the SLAM loop detection methods in the aforementioned embodiments.
[0192] An embodiment of the present application also provides a computer program product for storing a computer program, wherein the computer program is used to execute the steps of any one of the SLAM loop detection methods in the aforementioned embodiments.
[0193] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.
[0194] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.
[0195] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.
[0196] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.
[0197] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.
Claims
1. A SLAM loop detection method, characterized in that: include: Selecting a historical frame image similar to the current frame image from the constructed SLAM map, and calculating the initial relative pose between the current frame image and the historical frame image; Determine, according to the initial relative position and posture, an epipolar line of a first feature point in the current frame image; wherein the first feature point is any feature point extracted from the historical frame image; Determine an affine transformation relationship between the historical frame image and the current frame image according to the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line; Determine, according to the affine transformation relationship, a second feature point that matches the first feature point in the current frame image; Determining whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to feature descriptors of the first feature point and the second feature point; Loop correction is performed based on the retained feature point matching pairs.
2. The method according to claim 1, characterized in that The determining, according to the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line, the affine transformation relationship between the historical frame image and the current frame image comprises: An image block centered on the first feature point is constructed in the historical frame image, and pixel points contained in the image block are projected onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain a projection block corresponding to the image block on the epipolar line; wherein the projection point corresponding to the first feature point is located on the epipolar line; According to the grayscale information of the pixel points contained in the image block and the grayscale information of the projection points contained in the projection block, the affine transformation relationship between the historical frame image and the current frame image is determined by using the least squares method.
3. The method according to claim 1, characterized in that The determining, based on the feature descriptors of the first feature point and the second feature point, whether to retain a feature point matching pair consisting of the first feature point and the second feature point comprises: Calculating a first descriptor of the first feature point; According to the affine transformation relationship, sampling the neighborhood of the second feature point in the current frame image to obtain a plurality of sampling points; Calculating and weighting the descriptors of the plurality of sampling points, and using the weighted descriptors as the descriptors of the second feature points; If the similarity between the first descriptor and the second descriptor is greater than a preset similarity threshold, the feature point matching pair consisting of the first feature point and the second feature point is retained, otherwise the feature point matching pair consisting of the first feature point and the second feature point is discarded.
4. The method according to claim 1, characterized in that Determining the epipolar line of the first feature point in the current frame image according to the initial relative pose includes: Acquire depth information of the first feature point; Determine an epipolar line of the first feature point in the current frame image according to the depth information and the initial relative position and posture.
5. The method according to any one of claims 1 to 4, characterized in that The calculating the initial relative pose between the current frame image and the historical frame image includes: Acquire a plurality of neighborhood points around the first feature point; According to the grayscale information of the first feature point and the plurality of neighborhood points, combined with a preset initial pose value, a least squares method is used to determine an initial relative pose between the current frame image and the historical frame image.
6. The method according to claim 5, characterized in that The least squares formula of the initial relative pose is expressed as: Among them, m is the number of first feature points, n is the number of neighborhood points, α is the photometric scaling factor, β is the photometric shift factor, π(·) and π(·) -1 are the forward projection function and the reverse projection function of the camera model, respectively, d old is the depth information of the first feature point in the camera coordinate system of the historical frame image, is the initial relative pose between the current frame image and the historical frame image, and I(·) is the grayscale information.
7. A SLAM loop detection device, characterized in that: include: A frame matching module is used to select a historical frame image similar to the current frame image from the constructed SLAM map, and calculate the initial relative pose between the current frame image and the historical frame image; An epipolar line search module, used to determine an epipolar line of a first feature point in the current frame image according to the initial relative position; wherein the first feature point is any feature point extracted from the historical frame image; A block matching module, configured to determine an affine transformation relationship between the historical frame image and the current frame image based on the grayscale information of the image block of the first feature point in the historical frame image and the grayscale information of the projection block corresponding to the image block on the epipolar line; and determine a second feature point matched by the first feature point in the current frame image based on the affine transformation relationship; A verification module, used for determining whether to retain a feature point matching pair consisting of the first feature point and the second feature point according to feature descriptors of the first feature point and the second feature point; The correction module is used to perform loop correction based on the retained feature point matching pairs.
8. The device according to claim 7, characterized in that The block matching module is specifically used for: An image block centered on the first feature point is constructed in the historical frame image, and pixel points contained in the image block are projected onto the current frame image according to the initial relative posture and the depth information corresponding to the historical frame image to obtain a projection block corresponding to the image block on the epipolar line; wherein the projection point corresponding to the first feature point is located on the epipolar line; According to the grayscale information of the pixel points contained in the image block and the grayscale information of the projection points contained in the projection block, the affine transformation relationship between the historical frame image and the current frame image is determined by using the least squares method.
9. The device according to claim 8, characterized in that The verification module is specifically used for: Calculating a first descriptor of the first feature point; According to the affine transformation relationship, sampling the neighborhood of the second feature point in the current frame image to obtain a plurality of sampling points; Calculating and weighting the descriptors of the plurality of sampling points, and using the weighted descriptors as the descriptors of the second feature points; If the similarity between the first descriptor and the second descriptor is greater than a preset similarity threshold, the feature point matching pair consisting of the first feature point and the second feature point is retained, otherwise the feature point matching pair consisting of the first feature point and the second feature point is discarded.
10. A positioning device, characterized in that: It includes a processor, a memory and a multi-eye camera, wherein the multi-eye camera, the memory and the processor are connected via a bus; The multi-camera is used to collect scene images; The memory stores a computer program, and the processor executes the method according to any one of claims 1 to 6.