Slam method and device for fusing image data and radar point cloud data

Abstract

The application provides a SLAM method and device for fusing image data and radar point cloud data. The method comprises the following steps: acquiring image data and radar point cloud data of each frame in two adjacent frames; acquiring photometric errors between the two adjacent frames; acquiring a first relative pose based on the radar point cloud data of the two adjacent frames; acquiring a second relative pose between the two adjacent frames based on the photometric errors and the first relative pose; detecting whether a previous frame in the two adjacent frames is a key frame, and performing loop detection based on the image data and the radar point cloud data corresponding to the previous frame when it is determined that the previous frame is a key frame; determining that the loop detection is valid, performing global pose optimization on the second relative pose by using loop constraints, and obtaining a target pose. The technical scheme of the application can improve the accuracy of the output pose.

Description

Technical Field

[0001] This application relates to the field of multimodal data fusion technology, and in particular to a SLAM method, apparatus, device and storage medium for fusing image data and radar point cloud data. Background Technology

[0002] In related technologies, SLAM (Simultaneous Localization and Mapping), which fuses image data and LiDAR point cloud data, typically acquires depth data from image data based on point cloud data to enhance image-based SLAM and reduce system error drift. However, the above processing is loosely coupled and cannot fully utilize image data and point cloud data to constrain sensor motion. Summary of the Invention

[0003] This application aims to at least partially address one of the technical problems in the related art.

[0004] In a first aspect, this application proposes a SLAM method for fusing image data and radar point cloud data. The method includes: acquiring image data and radar point cloud data for each of two adjacent frames; acquiring the photometric error between the two adjacent frames; acquiring a first relative pose based on the radar point cloud data of the two adjacent frames; acquiring a second relative pose between the two adjacent frames based on the photometric error and the first relative pose; detecting whether the preceding frame in the two adjacent frames is a keyframe, and when the preceding frame is determined to be a keyframe, performing loop closure detection based on the image data and radar point cloud data corresponding to the preceding frame respectively; determining that the loop closure detection is valid, and using loop closure constraints to perform global pose optimization on the second relative pose to obtain the target pose.

[0005] In one implementation, obtaining the photometric error between two adjacent frames includes: projecting the radar point cloud data corresponding to the previous frame onto the image data corresponding to the previous frame to obtain a sparse depth map; establishing a photometric error loss function corresponding to the two adjacent frames based on the sparse depth map; and obtaining the photometric error based on the photometric error loss function.

[0006] In one alternative implementation, the photometric error loss function can be expressed as follows:

[0007] r(p i ) = I j (π(T ji *T CL *p i ))-(aI i (π(T CL *p i ))+b)

[0008] Where, p i T is the spatial point corresponding to the previous frame in the two adjacent frames. ji T represents the relative pose between two adjacent frames. CL I represents the spatial transformation parameters between the camera and the lidar. i For the preceding frame among the two adjacent frames, I j It is the next frame in the two adjacent frames.

[0009] In one implementation, obtaining the first relative pose based on the radar point cloud data of the two adjacent frames includes: based on the edge points in the radar point cloud data corresponding to the previous frame, performing a nearest neighbor search in the radar point cloud data corresponding to the next frame of the two adjacent frames to obtain a first target point corresponding to the edge points; based on the planar points in the radar point cloud data corresponding to the previous frame, performing a nearest neighbor search in the radar point cloud data corresponding to the next frame of the two adjacent frames to obtain a second target point corresponding to the planar points; based on the edge points and the first target point, obtaining a first distance between the edge points and the corresponding matching line; based on the planar points and the second target point, obtaining a second distance between the planar points and the corresponding matching surface; and based on the first distance and the second distance, obtaining the first relative pose.

[0010] In one implementation, obtaining the second relative pose between two adjacent frames based on the photometric error and the first relative pose includes: obtaining the geometric error between the two adjacent frames based on the first relative pose; and optimizing the photometric error and the geometric error to obtain the second relative pose.

[0011] In one implementation, the loop closure detection based on the image data and radar point cloud data corresponding to the previous frame includes: filtering candidate loop closure keyframes corresponding to the previous frame based on a bag-of-words model to obtain valid candidate loop closure keyframes, and performing a secondary filtering of the valid candidate loop closure keyframes based on time consistency to determine whether a first target loop closure keyframe exists; and detecting historical keyframes corresponding to the previous frame based on the radar point cloud data of the previous frame to determine whether a second target loop closure keyframe corresponding to the previous frame exists in the historical keyframes.

[0012] Secondly, this application proposes a SLAM device for fusing image data and radar point cloud data. The device includes: a first acquisition module for acquiring image data and radar point cloud data for each of two adjacent frames; a second acquisition module for acquiring the photometric error between the two adjacent frames; a first processing module for acquiring a first relative pose based on the radar point cloud data of the two adjacent frames; a second processing module for acquiring a second relative pose between the two adjacent frames based on the photometric error and the first relative pose; a third processing module for detecting whether the preceding frame in the two adjacent frames is a keyframe, and performing loop closure detection based on the image data and radar point cloud data corresponding to the preceding frame when the preceding frame is determined to be a keyframe; and a fourth processing module for determining that the loop closure detection is valid, and performing global pose optimization on the second relative pose using loop closure constraints to obtain the target pose.

[0013] In one implementation, the second acquisition module can be used to: project the radar point cloud data corresponding to the previous frame onto the image data corresponding to the previous frame to obtain a sparse depth map; establish a photometric error loss function corresponding to the two adjacent frames based on the sparse depth map; and obtain the photometric error based on the photometric error loss function.

[0014] In one alternative implementation, the photometric error loss function can be expressed as follows:

[0015] r(p i ) = I j (π(T ji *T CL *p i ))-(aI i (π(T CL *p i ))+b)

[0016] Where, p i T is the spatial point corresponding to the previous frame in the two adjacent frames. ji T represents the relative pose between two adjacent frames. CL I represents the spatial transformation parameters between the camera and the lidar. i For the preceding frame among the two adjacent frames, I j It is the next frame in the two adjacent frames.

[0017] In one implementation, the first processing module can be used to: perform a nearest neighbor search in the radar point cloud data corresponding to the radar point cloud data in the next frame of the two adjacent frames based on the edge points in the radar point cloud data corresponding to the previous frame, to obtain a first target point corresponding to the edge points; perform a nearest neighbor search in the radar point cloud data corresponding to the radar point cloud data in the next frame of the two adjacent frames based on the planar points in the radar point cloud data corresponding to the previous frame, to obtain a second target point corresponding to the planar points; obtain a first distance between the edge points and the corresponding matching line based on the edge points and the first target point; obtain a second distance between the planar points and the corresponding matching surface based on the planar points and the second target point; and obtain a first relative pose based on the first distance and the second distance.

[0018] In one implementation, the second processing module can be used to: obtain the geometric error between two adjacent frames based on the first relative pose; optimize the photometric error and the geometric error to obtain the second relative pose.

[0019] In one implementation, the third processing module can be used to: filter candidate closed-loop keyframes corresponding to the previous frame based on the bag-of-words model to obtain valid candidate closed-loop keyframes, and perform secondary filtering on the valid candidate closed-loop keyframes based on time consistency to determine whether a first target closed-loop keyframe exists; and detect historical keyframes corresponding to the previous frame based on the radar point cloud data of the previous frame to determine whether a second target closed-loop keyframe corresponding to the previous frame exists in the historical keyframes.

[0020] Thirdly, this application proposes an electronic device comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the SLAM method of fusing image data and radar point cloud data as described in the first aspect.

[0021] Fourthly, this application proposes a computer-readable storage medium for storing instructions that, when executed, cause the method described in the first aspect to be implemented.

[0022] Fifthly, this application proposes a computer program product, including a computer program that, when executed by a processor, implements the steps of the SLAM method for fusing image data and radar point cloud data as described in the first aspect.

[0023] The SLAM method, apparatus, device, and storage medium for fusing image data and radar point cloud data provided in this application can obtain a first relative pose between two adjacent frames based on image data and radar point cloud data, and obtain the photometric error between the two adjacent frames. The first relative pose is then corrected based on the photometric error to obtain a second relative pose. When the preceding frame is a keyframe in the two adjacent frames, loop closure detection is performed based on the image data of the preceding frame and the radar point cloud data, and the pose is output based on the loop closure detection result. This improves the accuracy of the output pose.

[0024] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0025] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0026] Figure 1 This is a flowchart illustrating a SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application.

[0027] Figure 2 This is a schematic diagram of a photometric error function establishment process provided in an embodiment of this application;

[0028] Figure 3 This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application.

[0029] Figure 4 This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in this application embodiment;

[0030] Figure 5 This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in this application embodiment;

[0031] Figure 6 This is a schematic flowchart of an image and lidar tight fusion SLAM method provided in an embodiment of this application;

[0032] Figure 7 This is a schematic diagram of the structure of a SLAM device for fusing image data and radar point cloud data according to an embodiment of this application;

[0033] Figure 8 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0034] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain this application, and should not be construed as limiting this application.

[0035] The following describes, with reference to the accompanying drawings, a method and apparatus for fusing image data and radar point cloud data in SLAM (Simultaneous Localization and Mapping) according to embodiments of this application.

[0036] Figure 1 This is a flowchart illustrating a SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application. Figure 1 As shown, the method may include, but is not limited to, the following steps:

[0037] Step S101: Acquire image data and radar point cloud data for each of two adjacent frames.

[0038] Step S102: Obtain the photometric error between two adjacent frames.

[0039] For example, the difference in pixel values ​​between two adjacent frames of corresponding image data is calculated as the photometric error based on the pixel positions in the corresponding image data of two adjacent frames.

[0040] Step S103: Obtain the first relative pose based on radar point cloud data from two adjacent frames.

[0041] For example, point cloud matching and pose estimation are performed based on radar point cloud data from two adjacent frames to obtain the first relative pose between the two adjacent frames.

[0042] Step S104: Based on the photometric error and the first relative pose, obtain the second relative pose between two adjacent frames.

[0043] For example, the photometric error between two adjacent frames is obtained based on the photometric error loss function, and the first relative pose is corrected based on the photometric error to obtain the second relative pose between the two adjacent frames.

[0044] As an example, please see Figure 2 , Figure 2 This is a schematic diagram of a photometric error function establishment process provided in an embodiment of this application.

[0045] In some embodiments, the second relative pose can be represented as follows:

[0046]

[0047] Where ρ(r) represents the kernel function, v represents the degrees of freedom of the t distribution, and μ r and σ r denoted by μ, representing the mean and standard deviation of all residuals, respectively. r and σ r As shown below:

[0048]

[0049] Where, N r This indicates the number of residual terms.

[0050] Step S105: Detect whether the previous frame in two adjacent frames is a key frame, and when the previous frame is determined to be a key frame, perform loop closure detection based on the image data and radar point cloud data corresponding to the previous frame respectively.

[0051] For example, parallax is used to determine whether the previous frame is a key frame in two adjacent frames. If the previous frame is determined to be a key frame, loop closure detection is performed based on the image data corresponding to the previous frame, and loop closure detection is also performed based on the radar point cloud data corresponding to the previous frame.

[0052] For example, parallax is used to determine whether the previous frame is a keyframe in two adjacent frames. If it is determined that the previous frame is not a keyframe, the pose is directly output.

[0053] For example, the previous frame in two adjacent frames can be the current frame.

[0054] Step S106: Determine that the closed-loop detection is effective, and use the closed-loop constraint to perform global pose optimization on the second relative pose to obtain the target pose.

[0055] For example, if it is determined that loop closure detection based on image data is effective, and loop closure detection based on radar point cloud data is also effective, then pose optimization is performed based on loop closure constraints to obtain the target pose. A map is then constructed by fusing the poses corresponding to all LiDAR keyframes, including the target pose.

[0056] By implementing the embodiments of this application, a first relative pose between two adjacent frames can be obtained based on image data and radar point cloud data of the two adjacent frames, and the photometric error between the two adjacent frames can be obtained. The first relative pose can then be corrected based on the photometric error to obtain a second relative pose. Furthermore, when the preceding frame is a keyframe in the two adjacent frames, loop closure detection is performed based on the image data and radar point cloud data of the preceding frame, and the pose is output based on the loop closure detection result. This improves the accuracy of the output pose.

[0057] In some embodiments, a photometric error function can be established between two adjacent frames to obtain the photometric error between the two adjacent frames. As an example, please refer to [link to example]. Figure 3 , Figure 3This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application. Figure 3 As shown, the method may include, but is not limited to, the following steps:

[0058] Step S301: Acquire image data and radar point cloud data for each of two adjacent frames.

[0059] In the embodiments of this application, step S301 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0060] Step S302: Project the radar point cloud data corresponding to the previous frame onto the image data corresponding to the previous frame to obtain a sparse depth map.

[0061] For example, with I i This represents the image corresponding to the previous frame in two adjacent frames, denoted by P. i This represents the radar point cloud data corresponding to the previous frame in two adjacent frames, denoted by p. k =[xyz] T ∈R 3 This represents a spatial point in the radar point cloud data. Based on the camera's imaging model, the spatial point in the radar point cloud data corresponding to the previous frame in two adjacent frames is projected onto the image corresponding to the previous frame in two adjacent frames to obtain an initial sparse depth map. This initial depth map is then downsampled to obtain the sparse depth map corresponding to the previous frame in two adjacent frames.

[0062] The projection process can be represented as follows:

[0063]

[0064] Among them, u k =[uv] T ∈R 2 This represents the coordinates of a pixel in the corresponding image data of the previous frame, π represents the camera model, and f x f y and c x c y T represents the camera's focal length and principal point, respectively. CL =[R CL t CL ] represents the spatial transformation parameters between the camera and the lidar.

[0065] Step S303: Establish the photometric error loss function for two adjacent frames based on the sparse depth map.

[0066] For example, depth information of image points in the corresponding image of the previous frame is obtained based on a sparse depth map, and spatial points corresponding to the image points in the corresponding image of the previous frame are generated by combining the depth information. The spatial points are then projected onto the corresponding image of the next frame in the two adjacent frames, and a photometric error loss function is established based on the grayscale difference of the image blocks near the projection point.

[0067] In some embodiments, the above photometric error loss function can be expressed as follows:

[0068] r(p i ) = I j (π(T ji *T CL *p i ))-(aI i (π(T CL *p i ))+b)

[0069] Where, p i T represents the spatial point corresponding to the previous frame in two adjacent frames. ji T represents the relative pose between two adjacent frames. CL I represents the spatial transformation parameters between the camera and the lidar. i For the image data corresponding to the previous frame in two adjacent frames, I j represents the image data corresponding to the next frame in two adjacent frames, where a and b are calculation parameters.

[0070] Step S304: Obtain the photometric error based on the photometric error loss function.

[0071] In the embodiments of this application, step S304 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0072] Step S305: Obtain the first relative pose based on radar point cloud data from two adjacent frames.

[0073] In the embodiments of this application, step S305 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0074] Step S306: Based on the photometric error and the first relative pose, obtain the second relative pose between two adjacent frames.

[0075] In the embodiments of this application, step S306 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0076] Step S307: Detect whether the previous frame in two adjacent frames is a key frame, and when the previous frame is determined to be a key frame, perform loop closure detection based on the image data and radar point cloud data corresponding to the previous frame respectively.

[0077] In the embodiments of this application, step S307 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0078] Step S308: Determine that the closed-loop detection is effective, and use the closed-loop constraint to perform global pose optimization on the second relative pose to obtain the target pose.

[0079] In the embodiments of this application, step S308 can be implemented in any of the ways described in the various embodiments of this application. The embodiments of this application do not limit this, nor will they be described in detail.

[0080] By implementing the embodiments of this application, a sparse depth map can be obtained based on the image data and radar point cloud data of the previous frame. A photometric error loss function is then established based on the sparse depth map, and the photometric error between two adjacent frames is obtained based on the photometric error loss function. The first relative pose is then corrected based on the photometric error to obtain a second relative pose. Furthermore, when the previous frame is a keyframe in the two adjacent frames, loop closure detection is performed based on the image data and radar point cloud data of the previous frame, and the pose is output based on the loop closure detection result. This improves the accuracy of the output pose.

[0081] In some embodiments, the first relative pose between two adjacent frames can be obtained based on edge points and planar points in radar point cloud data. As an example, see [link to example]. Figure 4 , Figure 4 This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application. Figure 4 As shown, the method may include, but is not limited to, the following steps:

[0082] Step S401: Acquire image data and radar point cloud data for each of two adjacent frames.

[0083] In the embodiments of this application, step S401 can be implemented in any of the ways described in the embodiments of this application. The embodiments of this application do not limit this, nor will they be described in detail.

[0084] Step S402: Obtain the photometric error between two adjacent frames.

[0085] In the embodiments of this application, step S402 can be implemented in any of the ways described in the embodiments of this application. The embodiments of this application do not limit this, nor will they be described in detail.

[0086] Step S403: Based on the edge points in the radar point cloud data corresponding to the previous frame, perform a nearest neighbor search in the radar point cloud data corresponding to the next frame in two adjacent frames to obtain the first target point corresponding to the edge point.

[0087] For example, based on each edge point in the radar point cloud data corresponding to the previous frame, a nearest neighbor search is performed in the radar point cloud data corresponding to the next frame in two adjacent frames to obtain the first target point corresponding to each edge point.

[0088] In one alternative implementation, an approximate curvature estimation algorithm based on PCA (Principal Component Analysis) can be used to obtain all edge points in the radar point cloud data of the previous frame in two adjacent frames.

[0089] In the embodiments of this application, any one of all the edge points can be used as an edge point.

[0090] For example, the edge point extraction steps are as follows: For each point p in the previous frame of the radar point cloud... i Find its m left neighbors on the same laser line [p i-m , ..., p i-1 ] and m right neighbors [p i+1 , ..., p i+m This yields a point set consisting of 2m+1 points. The centroid of this point set is then calculated. Sum of covariance matrix M:

[0091]

[0092]

[0093] Eigenvalue decomposition of M yields three eigenvalues, λ0, λ1, and λ2, arranged in ascending order. Then, p is estimated by calculating the ratio of λ2 to λ1. i curvature:

[0094]

[0095] Where, ratio is p i The curvature of the point is considered. If the curvature is greater than a preset threshold, the point is identified as a planar point; if the curvature is less than or equal to the preset threshold, the point is identified as an edge point. For edge points on the same laser line, the local minimum points are considered as primary edge points. Points near the primary edge points are considered unstable edge points and are filtered out.

[0096] Step S404: Based on the planar points in the radar point cloud data of the previous frame, perform a nearest neighbor search in the radar point cloud data of the next frame in two adjacent frames to obtain the second target point corresponding to the planar points.

[0097] For example, based on each planar point in the radar point cloud data of the previous frame, a nearest neighbor search is performed in the radar point cloud data of the next frame in two adjacent frames to obtain the second target point corresponding to each planar point.

[0098] Step S405: Based on the edge point and the first target point, obtain the first distance between the edge point and the corresponding matching line.

[0099] For example, the first distance mentioned above can be represented as follows:

[0100]

[0101] in, The first distance, This is the first relative pose. For a plane point, and This is the first target point.

[0102] Step S406: Based on the plane point and the second target point, obtain the second distance between the plane point and the corresponding matching surface.

[0103] For example, the second distance mentioned above can be represented as follows:

[0104]

[0105] in, The second distance, and This is the second target point.

[0106] Step S407: Obtain the first relative pose based on the first distance and the second distance.

[0107] For example, the sum of the first distance and the second distance is minimized to obtain the first relative pose.

[0108] For example, the first relative pose can be represented as follows:

[0109]

[0110] in, This is the first relative pose.

[0111] Step S408: Based on the photometric error and the first relative pose, obtain the second relative pose between two adjacent frames.

[0112] In the embodiments of this application, step S408 can be implemented in any of the ways described in the embodiments of this application. The embodiments of this application do not limit this, nor will they be described in detail.

[0113] Step S409: Detect the previous frame in two adjacent frames, and when the previous frame is determined to be a key frame, perform loop closure detection based on the image data and radar point cloud data corresponding to the previous frame.

[0114] In the embodiments of this application, step S409 can be implemented in any of the ways described in the embodiments of this application. The embodiments of this application do not limit this, nor will they be described in detail.

[0115] Step S4010: Determine that the closed-loop detection is valid, and use the closed-loop constraint to perform global pose optimization on the second relative pose.

[0116] In the embodiments of this application, step S4010 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0117] By implementing the embodiments of this application, an approximate curvature estimation algorithm based on PCA can be used to obtain edge points and planar points in the radar point cloud data of the previous frame in two adjacent frames. Nearest neighbor searches are then performed based on these edge points and planar points to obtain corresponding target points. A first distance is obtained based on the edge points and their corresponding target points, and a second distance is obtained based on the planar points and their corresponding target points. A first relative pose between two adjacent frames is then obtained based on the first and second distances, thereby improving the accuracy of the output pose.

[0118] In some embodiments, loop closure detection can be performed on image frames and point cloud frames separately, and map construction can be performed based on the loop closure detection results. For an example, please refer to... Figure 5 , Figure 5 This is a flowchart illustrating another SLAM method for fusing image data and radar point cloud data provided in an embodiment of this application. Figure 5 As shown, the method may include, but is not limited to, the following steps:

[0119] Step S501: Acquire image data and radar point cloud data for each of two adjacent frames.

[0120] In the embodiments of this application, step S501 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0121] Step S502: Obtain the photometric error between two adjacent frames.

[0122] In the embodiments of this application, step S502 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0123] Step S503: Obtain the first relative pose based on radar point cloud data from two adjacent frames.

[0124] In the embodiments of this application, step S503 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0125] Step S504: Based on the photometric error and the first relative pose, obtain the second relative pose between two adjacent frames.

[0126] In the embodiments of this application, step S504 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0127] Step S505: Based on the bag-of-words model, filter the candidate closed-loop keyframes corresponding to the previous frame to obtain valid candidate closed-loop keyframes, and perform a second filtering on the valid candidate closed-loop keyframes based on time consistency to determine whether the first target closed-loop keyframe exists.

[0128] For example, several image keyframes preceding the previous frame are acquired, and the bag-of-words vector for each image keyframe is calculated using a bag-of-words model. Then, a normalized similarity score for the bag-of-words vectors of each image keyframe is calculated. Keyframes with similarity scores greater than or equal to a preset score threshold (e.g., 0.7) are selected as candidate loop-closing keyframes. Invalid frames in the candidate loop-closing keyframes are removed through temporal consistency to determine whether a first target loop-closing keyframe exists.

[0129] As an example, the formula for calculating similarity scores is as follows:

[0130]

[0131] Wherein, η(v) c ,v q ) represents the similarity score, v q v represents the bag-of-words vector of the current query keyframe. c The bag-of-words vector representing the candidate loop closure keyframe, v l This represents the bag-of-words vector of the previous keyframe.

[0132] Step S506: Based on the radar point cloud data of the previous frame, detect the historical key frames corresponding to the previous frame to determine whether there is a second target closed-loop key frame corresponding to the next frame in the historical key frames.

[0133] For example, based on the radar point cloud data corresponding to the previous frame, loop closure detection is performed in combination with historical key frames to determine whether there are any frames in the historical frames that can form a loop with the previous frame.

[0134] As an example, the criteria for loop closure detection are as follows: the distance from the current LiDAR keyframe is less than a preset threshold; the time from the last loop closure frame is greater than a preset duration; and the time difference from the current LiDAR keyframe is greater than a preset duration.

[0135] For example, let C be the set of all candidate closed-loop LiDAR keyframes. A sparse local map M_0 is built using the previous frame and its adjacent LiDAR keyframes in two adjacent frames. Then, a series of sparse local maps [M_1…M_n] are built using the set of local candidate frames. The NDT (Normal Distributions Transform) algorithm is used to match M_0 with each of [M_1…M_n] to obtain the corresponding matching score. If the highest matching score is greater than or equal to a preset threshold, it is determined that a second target closed-loop keyframe exists in the historical keyframes.

[0136] Step S507: If the closed-loop detection is confirmed to be valid, then global pose optimization is performed using the closed-loop constraints to obtain the target pose.

[0137] In the embodiments of this application, step S507 can be implemented in any of the embodiments of this application. The embodiments of this application do not limit this and will not be described in detail.

[0138] By implementing the embodiments of this application, a first relative pose between two adjacent frames can be obtained based on image data and radar point cloud data of the two adjacent frames, and the photometric error between the two adjacent frames can be obtained. The first relative pose can then be corrected based on the photometric error to obtain a second relative pose. Furthermore, when the preceding frame is a keyframe in the two adjacent frames, loop closure detection is performed based on the image data and radar point cloud data of the preceding frame, and the pose is output based on the loop closure detection result. This improves the accuracy of the output pose.

[0139] Please see Figure 6 , Figure 6 This is a schematic flowchart of an image-lidar tight fusion SLAM method provided in an embodiment of this application. Figure 6As shown, this method can obtain the photometric error between two adjacent frames based on image data and radar point cloud data; obtain a first relative pose by performing feature matching based on the radar point cloud data of the two adjacent frames; obtain a second relative pose by performing joint optimization based on the photometric error and the first relative pose; detect whether the previous frame in the two adjacent frames is a keyframe, and if the previous frame is determined to be a keyframe, perform loop closure detection based on the image data and radar point cloud data corresponding to the previous frame respectively; if the loop closure detection is valid, use loop closure constraints to perform global pose optimization on the second relative pose to obtain the target pose, and construct a map based on the target pose. Alternatively, if the loop closure detection is invalid, use the second relative pose to construct a map.

[0140] Please see Figure 7 , Figure 7 This is a schematic diagram of the structure of a SLAM device for fusing image data and radar point cloud data, provided in an embodiment of this application. Figure 7 As shown, the device 700 includes: a first acquisition module 701, used to acquire image data and radar point cloud data of each of two adjacent frames; a second acquisition module 702, used to acquire photometric error between two adjacent frames; a first processing module 703, used to acquire a first relative pose based on the radar point cloud data of two adjacent frames; a second processing module 704, used to acquire a second relative pose between two adjacent frames based on the photometric error and the first relative pose; a third processing module 705, used to detect the previous frame in two adjacent frames, and when the previous frame is determined to be a key frame, to perform loop closure detection based on the image data and radar point cloud data corresponding to the previous frame respectively; and a fourth processing module 706, used to determine that the loop closure detection is valid, and to perform global pose optimization on the second relative pose using loop closure constraints to obtain the target pose.

[0141] In one implementation, the second acquisition module 702 can be used to: project the radar point cloud data corresponding to the previous frame onto the image data corresponding to the previous frame to obtain a sparse depth map; establish a photometric error loss function corresponding to two adjacent frames based on the sparse depth map; and obtain the photometric error based on the photometric error loss function.

[0142] In one alternative implementation, the photometric error loss function can be expressed as follows:

[0143] r(p i ) = I j (π(T ji *T CL *p i ))-(aI i (π(T CL *p i ))+b)

[0144] Where, p i T represents the spatial point corresponding to the previous frame in two adjacent frames. ji T represents the relative pose between two adjacent frames. CL I represents the spatial transformation parameters between the camera and the lidar. i For the image data corresponding to the previous frame in two adjacent frames, I j represents the image data corresponding to the next frame in two adjacent frames, where a and b are calculation parameters.

[0145] In one implementation, the first processing module 703 can be used to: perform a nearest neighbor search in the radar point cloud data of the next frame in two adjacent frames based on edge points in the radar point cloud data of the previous frame, to obtain a first target point corresponding to the edge point; perform a nearest neighbor search in the radar point cloud data of the next frame in two adjacent frames based on planar points in the radar point cloud data of the previous frame, to obtain a second target point corresponding to the planar point; obtain a first distance between the edge point and the corresponding matching line based on the edge point and the first target point; obtain a second distance between the planar point and the corresponding matching surface based on the planar point and the second target point; and obtain a first relative pose based on the first distance and the second distance.

[0146] In one implementation, the second processing module 704 can be used to: obtain the geometric error between two adjacent frames based on the first relative pose; optimize the photometric error and geometric error to obtain the second relative pose.

[0147] In one implementation, the third processing module 705 can be used to: filter candidate closed-loop keyframes corresponding to the previous frame based on the bag-of-words model to obtain valid candidate closed-loop keyframes, and perform secondary filtering on the valid candidate closed-loop keyframes based on time consistency to determine whether there is a first target closed-loop keyframe; and detect historical keyframes corresponding to the previous frame based on the radar point cloud data of the previous frame to determine whether there is a second target closed-loop keyframe corresponding to the previous frame in the historical keyframes.

[0148] The apparatus of this application embodiment can obtain a first relative pose between two adjacent frames based on image data and radar point cloud data, and obtain the photometric error between the two adjacent frames. The first relative pose is then corrected based on the photometric error to obtain a second relative pose. When the preceding frame is a keyframe in the two adjacent frames, loop closure detection is performed based on the image data of the preceding frame and the radar point cloud data, respectively, and the pose is output based on the loop closure detection result. This improves the accuracy of the output pose.

[0149] It should be noted that the foregoing explanation of the SLAM method embodiment for fusing image data and radar point cloud data also applies to the SLAM device for fusing image data and radar point cloud data in this embodiment, and will not be repeated here.

[0150] To implement the above embodiments, this application also proposes an electronic device. Please see [link to relevant documentation]. Figure 8 , Figure 8 This is a schematic diagram of the structure of the electronic device provided in an embodiment of this application. For example... Figure 8 As shown, the electronic device 800 includes: a processor 801, and a memory 802 communicatively connected to the processor 801; the memory 802 stores computer execution instructions; the processor 801 executes the computer execution instructions stored in the memory to implement the method provided in the foregoing embodiments.

[0151] To implement the above embodiments, this application also proposes a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, are used to implement the methods provided in the foregoing embodiments.

[0152] To implement the above embodiments, this application also proposes a computer program product, including a computer program that, when executed by a processor, implements the methods provided in the foregoing embodiments.

[0153] In the description of this application, unless otherwise stated, " / " means "or", for example, A / B can mean A or B; "and / or" in this document is merely a description of the relationship between related objects, indicating that there can be three relationships, for example, A and / or B can mean: A exists alone, A and B exist simultaneously, and B exists alone.

[0154] In the foregoing descriptions of the embodiments, the terms "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0155] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0156] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0157] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0158] It should be understood that various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0159] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0160] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0161] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application.

Claims

1. A SLAM method for fusing image data and radar point cloud data, characterized in that, include: Acquire image data and radar point cloud data for each of two adjacent frames; Project the radar point cloud data corresponding to the previous frame onto the image data corresponding to the previous frame to obtain a sparse depth map; Based on the sparse depth map, establish the photometric error loss function for the two adjacent frames; The photometric error between two adjacent frames is obtained based on the photometric error loss function, which is expressed as follows: in, The spatial point corresponding to the previous frame in the two adjacent frames. The relative pose between two adjacent frames. These are the spatial transformation parameters between the camera and the lidar. The image data corresponding to the previous frame in the two adjacent frames. The image data corresponding to the second frame in the two adjacent frames. and For calculation parameters, π represents the camera model; Based on the edge points and planar points in the radar point cloud data corresponding to the previous frame, a nearest neighbor search is performed in the radar point cloud data corresponding to the next frame in the two adjacent frames to obtain the first distance between the edge point and the corresponding matching line, and the second distance between the planar point and the corresponding matching surface. Based on the first distance and the second distance, the first relative pose is obtained. The geometric error between the two adjacent frames is obtained based on the first relative pose. The photometric error and the geometric error are optimized and solved to obtain the second relative pose between two adjacent frames, wherein the second relative pose is represented as follows: Among them, P i This represents the radar point cloud data corresponding to the previous frame in two adjacent frames. This represents a spatial point in radar point cloud data. The first distance, This is the second distance; Represents the kernel function. express Degrees of freedom of distribution and Let represent the mean and standard deviation of all residuals, respectively; Detect whether the preceding frame in two adjacent frames is a key frame, and when the preceding frame is determined to be a key frame, perform loop closure detection based on the image data and radar point cloud data corresponding to the preceding frame respectively; Once the closed-loop detection is confirmed to be effective, the closed-loop constraints are used to perform global pose optimization on the second relative pose to obtain the target pose.

2. The method as described in claim 1, characterized in that, The step of performing a nearest neighbor search on the radar point cloud data in the next frame of two adjacent frames, based on the edge points and planar points in the radar point cloud data corresponding to the previous frame, to obtain the first distance between the edge point and the corresponding matching line, and the second distance between the planar point and the corresponding matching surface, includes: Based on the edge points in the radar point cloud data corresponding to the previous frame, a nearest neighbor search is performed in the radar point cloud data corresponding to the next frame in the two adjacent frames to obtain the first target point corresponding to the edge point. Based on the planar point in the radar point cloud data corresponding to the previous frame, a nearest neighbor search is performed in the radar point cloud data corresponding to the next frame in the two adjacent frames to obtain the second target point corresponding to the planar point. Based on the edge point and the first target point, obtain the first distance between the edge point and the corresponding matching line; Based on the plane point and the second target point, obtain the second distance between the plane point and the corresponding matching surface.

3. The method as described in claim 1, characterized in that, The loop closure detection based on the image data and radar point cloud data corresponding to the previous frame includes: Based on the bag-of-words model, the candidate closed-loop keyframes corresponding to the previous frame are filtered to obtain valid candidate closed-loop keyframes. Then, based on time consistency, the valid candidate closed-loop keyframes are filtered a second time to determine whether the first target closed-loop keyframe exists. Based on the radar point cloud data of the previous frame, the historical key frames corresponding to the previous frame are detected to determine whether there is a second target closed-loop key frame corresponding to the previous frame in the historical key frames.

4. A SLAM device for fusing image data and radar point cloud data, characterized in that, The apparatus is used to implement the SLAM method for fusing image data and radar point cloud data as described in claim 1, and the apparatus includes: The first acquisition module is used to acquire image data and radar point cloud data for each of two adjacent frames; The second acquisition module is used to acquire the photometric error between two adjacent frames; The first processing module is used to perform nearest neighbor search in the radar point cloud data in the next frame of the two adjacent frames based on the edge points and planar points in the radar point cloud data corresponding to the previous frame, to obtain the first distance between the edge point and the corresponding matching line and the second distance between the planar point and the corresponding matching surface, and to obtain the first relative pose based on the first distance and the second distance. The second processing module is used to obtain the geometric error between the two adjacent frames based on the first relative pose; optimize the solution of the photometric error and the geometric error to obtain the second relative pose between the two adjacent frames. The third processing module is used to detect whether the previous frame in the two adjacent frames is a key frame, and when the previous frame is determined to be a key frame, to perform closed-loop detection based on the image data and radar point cloud data corresponding to the previous frame respectively. The fourth processing module is used to determine that the closed-loop detection is valid, and to perform global pose optimization on the second relative pose using closed-loop constraints to obtain the target pose.

5. An electronic device, characterized in that, include: A processor, and a memory communicatively connected to the processor; The memory stores computer-executed instructions; The processor executes computer execution instructions stored in the memory to implement the method as described in any one of claims 1 to 3.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the method as described in any one of claims 1 to 3.

7. A computer program product, characterized in that, It includes a computer program that, when executed by a processor, implements the method as described in any one of claims 1 to 3.

Images