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

Abstract

The invention provides an image data and radar point cloud data fusion SLAM method and device, and the method comprises the steps: obtaining the image data and radar point cloud data of each frame in two adjacent frames; acquiring a luminosity 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 luminosity error and the first relative pose; detecting whether the previous frame in the two adjacent frames is a key frame, and performing closed-loop detection based on the image data and the radar point cloud data corresponding to the previous frame when the previous frame is determined to be the key frame; and determining that closed-loop detection is valid, and performing global pose optimization on the second relative pose by using closed-loop constraint to obtain a target pose. According to the technical scheme, the accuracy of the output pose can be improved.

Description

Technical Field

[0001] The present application relates to the technical field of multimodal data fusion, and in particular to a SLAM method, apparatus, device and storage medium for fusing image data with radar point cloud data. Background Art

[0002] In related technologies, SLAM (Simultaneous Localization and Mapping) that fuses image data with LiDAR point cloud data typically uses point cloud data to obtain depth information from image data to enhance image-based SLAM and reduce system error drift. However, this process is loosely coupled and cannot fully utilize the image and point cloud data to constrain sensor motion. Summary of the Invention

[0003] The present application aims to solve one of the technical problems in the related art at least to a certain extent.

[0004] In the first aspect, the present application proposes a SLAM method for fusing image data with radar point cloud data, the method comprising: obtaining image data and radar point cloud data of each frame in two adjacent frames; obtaining a photometric error between the two adjacent frames; obtaining a first relative pose based on the radar point cloud data of the two adjacent frames; obtaining a second relative pose between the two adjacent frames based on the photometric error and the first relative pose; detecting whether the previous frame in the two adjacent frames is a key frame, and performing closed-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 closed-loop detection is valid, performing global pose optimization on the second relative pose using closed-loop constraints to obtain the target pose.

[0005] In one implementation, obtaining the photometric error between the 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 an optional 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] Among them, p i is the spatial point corresponding to the previous frame in the two adjacent frames, T ji is the relative pose between the two adjacent frames, T CL is the spatial transformation parameter between the camera and the lidar, I i is the previous frame of the two adjacent frames, I j is the latter frame of the two adjacent frames.

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

[0010] In one implementation, obtaining the second relative pose between the 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 and solving the photometric error and the geometric error to obtain the second relative pose.

[0011] In one implementation, the closed-loop detection based on the image data and the radar point cloud data corresponding to the previous frame includes: screening the candidate closed-loop key frames corresponding to the previous frame based on a bag-of-words model to obtain valid candidate closed-loop key frames, and performing a secondary screening of the valid candidate closed-loop key frames based on time consistency to determine whether there is a first target closed-loop key frame; and detecting the historical key frames 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 key frame corresponding to the previous frame in the historical key frames.

[0012] In the second aspect, the present application proposes a SLAM device that fuses image data and radar point cloud data, and the device includes: a first acquisition module for acquiring image data and radar point cloud data of each frame in 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 previous frame in the two adjacent frames is a key frame, and when it is determined that the previous frame is a key frame, performing closed-loop detection based on the image data and the radar point cloud data corresponding to the previous frame; a fourth processing module for determining that the closed-loop detection is valid, and using closed-loop constraints to perform global pose optimization on the second relative pose 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 an optional 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] Among them, p i is the spatial point corresponding to the previous frame in the two adjacent frames, T ji is the relative pose between the two adjacent frames, T CL is the spatial transformation parameter between the camera and the lidar, I i is the previous frame of the two adjacent frames, I j is the latter frame of the two adjacent frames.

[0017] In one implementation, the first processing module can be used to: based on the edge point 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 latter frame of the two adjacent frames to obtain a first target point corresponding to the edge point; based on the plane point 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 latter frame of the two adjacent frames to obtain a second target point corresponding to the plane point; based on the edge point and the first target point, obtain a first distance between the edge point and the corresponding matching line; based on the plane point and the second target point, obtain a second distance between the plane point and the corresponding matching surface; based on the first distance and the second distance, obtain the first relative pose.

[0018] In one implementation, the second processing module may be configured to: obtain a geometric error between the two adjacent frames based on the first relative pose; and optimize and solve 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: screen the candidate closed-loop key frames corresponding to the previous frame based on the bag-of-words model to obtain valid candidate closed-loop key frames, and perform secondary screening on the valid candidate closed-loop key frames based on time consistency to determine whether there is a first target closed-loop key frame; 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 previous frame in the historical key frames.

[0020] In a third aspect, the present 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, and the instructions are executed by the at least one processor so that the at least one processor can execute the SLAM method of fusing image data with radar point cloud data as described in the first aspect.

[0021] In a fourth aspect, the present application proposes a computer-readable storage medium for storing instructions, which, when executed, enables the method described in the first aspect to be implemented.

[0022] In a fifth aspect, the present application proposes a computer program product, comprising a computer program, which, when executed by a processor, implements the steps of the SLAM method for fusing image data with 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 the image data and radar point cloud data of the two adjacent frames, and obtain the photometric error between the two adjacent frames, so as to correct the first relative pose based on the photometric error to obtain a second relative pose, and when the previous frame of the two adjacent frames is a key frame, perform closed-loop detection based on the image data of the previous frame and the radar point cloud data, respectively, to output the pose based on the closed-loop detection results. This can improve the accuracy of the output pose.

[0024] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0026] Figure 1 1 is a flow chart of a SLAM method for fusing image data and radar point cloud data provided in an embodiment of the present application;

[0027] Figure 2 Schematic diagram of a photometric error function establishment process provided by an embodiment of the present application;

[0028] Figure 3 1 is a flow chart of another SLAM method for fusing image data with radar point cloud data provided in an embodiment of the present application;

[0029] Figure 4 1 is a flow chart of another SLAM method for fusing image data and radar point cloud data provided in an embodiment of the present application;

[0030] Figure 5 1 is a flow chart of another SLAM method for fusing image data and radar point cloud data provided in an embodiment of the present application;

[0031] Figure 6 This is a flow chart of a SLAM method for tight fusion of image and laser radar provided in an embodiment of the present application;

[0032] Figure 7 1 is a schematic structural diagram of a SLAM device for fusing image data and radar point cloud data provided in an embodiment of the present application;

[0033] Figure 8 It is a structural diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0034] The following describes in detail embodiments of the present application, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present application, and should not be construed as limiting the present application.

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

[0036] Figure 1 FIG. 1 is a flow chart of a SLAM method for fusing image data with radar point cloud data provided in an embodiment of the present 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 of each of two adjacent frames.

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

[0039] Exemplarily, based on pixel positions in two adjacent frames of corresponding image data, a difference in pixel values ​​in the two adjacent frames of corresponding image data is calculated and obtained as the photometric error.

[0040] Step S103: Acquire a first relative pose based on two adjacent frames of radar point cloud data.

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

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

[0043] Exemplarily, the photometric error between two adjacent frames is obtained based on the photometric error loss function, so as to correct the first relative pose based on the photometric error and obtain the second relative pose between the two adjacent frames.

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

[0045] In some embodiments, the second relative pose may be expressed as follows:

[0046]

[0047] Among them, ρ(r) represents the kernel function, v represents the degrees of freedom of t distribution, μ r and σ r Represent the mean and standard deviation of all residuals. r and σ r As shown below:

[0048]

[0049] Among them, N r Represents the number of residual terms.

[0050] Step S105: Detecting whether the previous frame of two adjacent frames is a key frame, and performing closed-loop detection based on the image data and radar point cloud data corresponding to the previous frame when it is determined that the previous frame is a key frame.

[0051] Exemplarily, parallax is used to determine whether the previous frame between two adjacent frames is a key frame. If the previous frame is determined to be a key frame, closed-loop detection is performed based on image data corresponding to the previous frame, and closed-loop detection is performed based on radar point cloud data corresponding to the previous frame.

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

[0053] Exemplarily, the previous frame of two adjacent frames may be the current frame.

[0054] Step S106: Determine whether 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 loop closure detection based on image data is determined to be valid, and loop closure detection based on radar point cloud data is determined to be valid, pose optimization is performed based on loop closure constraints to obtain the target pose. Map construction is performed by fusing the poses corresponding to all lidar keyframes, including the target pose.

[0056] By implementing the embodiments of the present application, a first relative pose between two adjacent frames can be obtained based on the 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 be corrected based on the photometric error to obtain a second relative pose. When the previous frame of the two adjacent frames is a key frame, closed-loop detection can be performed based on the image data and radar point cloud data of the previous frame, and the pose can be output based on the closed-loop detection results. This can improve the accuracy of the output pose.

[0057] In some embodiments, a photometric error function between two adjacent frames may be established to obtain the photometric error between the two adjacent frames based on the photometric error function. As an example, see Figure 3 , Figure 3FIG. 1 is a flow chart of another SLAM method for fusing image data with radar point cloud data provided in an embodiment of the present 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 of each of two adjacent frames.

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

[0060] Step S302: 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.

[0061] For example, I i Indicates the image corresponding to the previous frame in two adjacent frames, with P i Indicates the radar point cloud data corresponding to the previous frame in two adjacent frames, with p k =[xyz] T ∈R 3 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 between two adjacent frames is projected onto the image corresponding to the previous frame between two adjacent frames to obtain an initial sparse depth map. This initial depth map is then downsampled to obtain a sparse depth map corresponding to the previous frame between two adjacent frames.

[0062] The projection process can be specifically expressed as follows:

[0063]

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

[0065] Step S303: establishing a photometric error loss function corresponding to two adjacent frames based on the sparse depth map.

[0066] Exemplarily, the depth information of an image point in the corresponding image of the previous frame between two adjacent frames is obtained based on the sparse depth map, and the spatial point corresponding to the image point in the corresponding image of the previous frame is generated in combination with the depth information, and the spatial point is projected onto the image corresponding to the latter frame between the two adjacent frames, so as to establish a photometric error loss function based on the grayscale difference of the image block near the projection point.

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

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

[0069] Among them, p i is the spatial point corresponding to the previous frame in two adjacent frames, T ji is the relative pose between two adjacent frames, T CL is the spatial transformation parameter between the camera and the lidar, I i is the image data corresponding to the previous frame in two adjacent frames, I j is the image data corresponding to the latter frame of two adjacent frames, and a and b are calculation parameters.

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

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

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

[0073] In the embodiment of the present application, step S305 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present 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, a second relative pose between two adjacent frames is obtained.

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

[0076] Step S307: Detect whether the previous frame of two adjacent frames is a key frame, and perform closed-loop detection based on the image data and radar point cloud data corresponding to the previous frame when it is determined that the previous frame is a key frame.

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

[0078] Step S308: Determine whether 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 embodiment of the present application, step S308 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present application do not limit this and will not be described in detail.

[0080] By implementing the embodiments of the present application, a sparse depth map can be obtained based on the image data and radar point cloud data of the previous frame, and a photometric error loss function can be established based on the sparse depth map. The photometric error between two adjacent frames can be obtained based on the photometric error loss function, and the first relative pose can be corrected based on the photometric error to obtain a second relative pose. When the previous frame of the two adjacent frames is a key frame, closed-loop detection can be performed based on the image data and radar point cloud data of the previous frame, and the pose can be output based on the closed-loop detection results. This can improve 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 plane points in the radar point cloud data. As an example, see Figure 4 , Figure 4 This is a flow chart of another SLAM method for fusing image data with radar point cloud data provided by the embodiment of the present 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 of each of two adjacent frames.

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

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

[0085] In the embodiment of the present application, step S402 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present application do not limit this and will not be described in detail.

[0086] Step S403: Based on the 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 latter frame of two adjacent frames to obtain a first target point corresponding to the edge point.

[0087] Exemplarily, 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 latter frame of two adjacent frames to obtain the first target point corresponding to each edge point.

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

[0089] In the embodiment of the present application, any one of all the edge points may 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 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 ], we get a point set consisting of 2m+1 points. Calculate the centroid of the point set And the covariance matrix M:

[0091]

[0092]

[0093] Performing eigenvalue decomposition on M can obtain three eigenvalues ​​in ascending order, λ0, λ1, and λ2. Then, p is estimated by calculating the ratio of λ2 to λ1. i Curvature:

[0094]

[0095] Among them, ratio is p i If the curvature is greater than a preset threshold, the point is determined to be a plane point; if the curvature is less than or equal to the preset threshold, the point is determined to be an edge point. For edge points on the same laser line, the local minimum point is considered the main edge point. Points near the main edge point are considered to be unstable edge points and are filtered out.

[0096] Step S404: Based on the plane 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 latter frame of two adjacent frames to obtain a second target point corresponding to the plane point.

[0097] Exemplarily, based on each plane 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 latter frame of two adjacent frames to obtain the second target point corresponding to each plane point.

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

[0099] For example, the first distance can be expressed as follows:

[0100]

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

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

[0103] For example, the second distance can be expressed as follows:

[0104]

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

[0106] Step S407: Acquire a first relative posture based on the first distance and the second distance.

[0107] Exemplarily, the sum of the first distance and the second distance is minimized to obtain the first relative posture.

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

[0109]

[0110] in, is the first relative posture.

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

[0112] In the embodiment of the present application, step S408 can be implemented using any of the methods in the embodiments of the present application. The embodiments of the present application do not limit this and will not be described in detail.

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

[0114] In the embodiment of the present application, step S409 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present application do not limit this and will not be described in detail.

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

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

[0117] By implementing the embodiments of the present application, a PCA-based approximate curvature estimation algorithm can be used to obtain edge points and plane points in the radar point cloud data of the previous frame between two adjacent frames, and then perform a nearest neighbor search based on the edge points and plane points to obtain the corresponding target points. A first distance is obtained based on the edge points and the corresponding target points, and a second distance is obtained based on the plane points and the corresponding target points. Finally, a first relative pose between the two adjacent frames is obtained based on the first and second distances. This improves the accuracy of the output pose.

[0118] In some embodiments, closed loop detection can be performed on the image frame and the point cloud frame respectively, and a map can be constructed based on the closed loop detection results. As an example, see Figure 5 , Figure 5 This is a flow chart of another SLAM method for fusing image data with radar point cloud data provided by the embodiment of the present 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 of each of two adjacent frames.

[0120] In the embodiment of the present application, step S501 can be implemented in any of the ways in the embodiments of the present application. The embodiments of the present 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 embodiment of the present application, step S502 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present application do not limit this and will not be described in detail.

[0123] Step S503: Acquire a first relative pose based on two adjacent frames of radar point cloud data.

[0124] In the embodiment of the present application, step S503 can be implemented by any of the methods in the embodiments of the present application. The embodiments of the present 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, a second relative pose between two adjacent frames is obtained.

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

[0127] Step S505: Based on the bag-of-words model, the candidate closed-loop key frames corresponding to the previous frame are screened to obtain valid candidate closed-loop key frames, and the valid candidate closed-loop key frames are screened again based on time consistency to determine whether the first target closed-loop key frame exists.

[0128] For example, several image keyframes before the previous frame are obtained, and the bag-of-words model is used to calculate the bag-of-words vector of each image keyframe, and then the normalized similarity score of the bag-of-words vector 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 closed-loop keyframes, and invalid frames in the candidate closed-loop keyframes are deleted through temporal consistency to determine whether the first target closed-loop keyframe exists.

[0129] As an example, the similarity score is calculated as follows:

[0130]

[0131] Among them, η(v c ,v q ) is the similarity score, v q Represents the bag-of-words vector of the current query keyframe, v c The bag-of-words vector representing the candidate closed-loop keyframe, v l The bag-of-words vector representing the previous keyframe.

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

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

[0134] As an example, the judgment criteria for loop detection are as follows: the distance from the current lidar key frame is less than a preset threshold; the time from the position of the last loop frame is greater than a preset duration; the time difference from the current lidar key frame is greater than a preset duration.

[0135] For example, let the set of all candidate closed-loop lidar keyframes be denoted as C. A sparse local map M_0 is created 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 created 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 scores. If the highest matching score is greater than or equal to a preset threshold, it is determined that a second target closed-loop keyframe corresponding to the subsequent frame exists in the historical keyframes.

[0136] Step S507: If the closed-loop detection is determined to be effective, the closed-loop constraint is used to perform global pose optimization to obtain the target pose.

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

[0138] By implementing the embodiments of the present application, a first relative pose between two adjacent frames can be obtained based on the 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 be corrected based on the photometric error to obtain a second relative pose. When the previous frame of the two adjacent frames is a key frame, closed-loop detection can be performed based on the image data and radar point cloud data of the previous frame, and the pose can be output based on the closed-loop detection results. This can improve the accuracy of the output pose.

[0139] See Figure 6 , Figure 6 This is a flow chart of a SLAM method for tightly integrating image and laser radar provided in an embodiment of the present application. Figure 6As shown, the method can obtain the photometric error between the two adjacent frames based on the image data and radar point cloud data of the two adjacent frames; perform feature matching based on the radar point cloud data of the two adjacent frames to obtain a first relative pose; perform joint optimization based on the photometric error and the first relative pose to obtain a second relative pose between the two adjacent frames; and detect whether the previous frame of the two adjacent frames is a key frame. When the previous frame is determined to be a key frame, a closed-loop detection is performed based on the image data and the radar point cloud data corresponding to the previous frame. When the closed-loop detection is determined to be valid, a global pose optimization is performed on the second relative pose using closed-loop constraints to obtain a target pose, and a map is constructed based on the target pose. Alternatively, when the closed-loop detection is determined to be invalid, the map is constructed using the second relative pose.

[0140] See Figure 7 , Figure 7 Schematic diagram of the structure of a SLAM device for fusing image data and radar point cloud data provided in an embodiment of the present application. Figure 7 As shown, the device 700 includes: a first acquisition module 701, used to obtain image data and radar point cloud data of each frame in two adjacent frames; a second acquisition module 702, used to obtain the photometric error between the two adjacent frames; a first processing module 703, used to obtain a first relative pose based on the radar point cloud data of the two adjacent frames; a second processing module 704, used to obtain a second relative pose between the 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 the two adjacent frames, and when the previous frame is determined to be a key frame, perform closed-loop detection based on the image data and radar point cloud data corresponding to the previous frame; a fourth processing module 706, used to determine whether the closed-loop detection is valid, use the closed-loop constraint to perform global pose optimization on the second relative pose, and 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 a photometric error based on the photometric error loss function.

[0142] In an optional 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] Among them, p i is the spatial point corresponding to the previous frame in two adjacent frames, T ji is the relative pose between two adjacent frames, T CL is the spatial transformation parameter between the camera and the lidar, I i is the image data corresponding to the previous frame in two adjacent frames, I j is the image data corresponding to the latter frame of two adjacent frames, and a and b are calculation parameters.

[0145] In one implementation, the first processing module 703 can be used to: based on the edge point 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 latter frame of two adjacent frames to obtain a first target point corresponding to the edge point; based on the plane point 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 latter frame of two adjacent frames to obtain a second target point corresponding to the plane point; based on the edge point and the first target point, obtain a first distance between the edge point and the corresponding matching line; based on the plane point and the second target point, obtain a second distance between the plane point and the corresponding matching surface; based on the first distance and the second distance, obtain a first relative pose.

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

[0147] In one implementation, the third processing module 705 can be used to: screen the candidate closed-loop key frames corresponding to the previous frame based on the bag-of-words model to obtain valid candidate closed-loop key frames, and perform secondary screening on the valid candidate closed-loop key frames based on time consistency to determine whether there is a first target closed-loop key frame; 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 previous frame in the historical key frames.

[0148] The device of the embodiment of the present application can obtain a first relative pose between two adjacent frames based on the image data and radar point cloud data of the two adjacent frames, and obtain the photometric error between the two adjacent frames. The first relative pose is corrected based on the photometric error to obtain a second relative pose. When the previous frame of the two adjacent frames is a key frame, closed-loop detection is performed based on the image data of the previous frame and the radar point cloud data, and the pose is output based on the closed-loop detection results. This can improve the accuracy of the output pose.

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

[0150] In order to implement the above embodiment, the present application also proposes an electronic device. Figure 8 , Figure 8 Schematic diagram of the structure of the electronic device provided in the embodiment of the present application. 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-executable instructions; the processor 801 executes the computer-executable instructions stored in the memory to implement the method provided in the aforementioned embodiment.

[0151] In order to implement the above embodiments, the present application also proposes a computer-readable storage medium, in which computer-executable instructions are stored. When the computer-executable instructions are executed by a processor, they are used to implement the methods provided by the above embodiments.

[0152] In order to implement the above embodiments, the present application also proposes a computer program product, including a computer program, which implements the methods provided by the above embodiments when executed by a processor.

[0153] In the description of this application, unless otherwise specified, " / " means or, for example, A / B can mean A or B; "and / or" in this article is only a way to describe the association relationship of associated objects, indicating that there can be three relationships, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone.

[0154] In the descriptions of the foregoing embodiments, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are mutually inconsistent.

[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 the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. Throughout the description of this application, "plurality" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0156] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a custom logical function or process, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present application belong.

[0157] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing the 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 (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program 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 the following: an electrical connection with one or more wires (electronic devices), a portable computer disk cartridge (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and programmable read-only memory (EPROM or flash memory), fiber optic devices, and a portable compact disc read-only memory (CDROM). Furthermore, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium and then editing, interpreting or processing it in another suitable manner if necessary, and then storing it in a computer memory.

[0158] It should be understood that various parts of the present 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 a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used to implement: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0159] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

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

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

Claims

1. A SLAM method for fusing image data and radar point cloud data, characterized in that: include: Obtain image data and radar point cloud data of each of two adjacent frames; Obtaining a photometric error between the two adjacent frames; Acquire a first relative pose based on the radar point cloud data of the two adjacent frames; Acquire a second relative pose between the two adjacent frames based on the photometric error and the first relative pose; detecting whether a previous frame of the two adjacent frames is a key frame, and performing closed-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; 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.

2. The method according to claim 1, wherein The obtaining of the photometric error between the 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; The photometric error is obtained based on the photometric error loss function.

3. The method according to claim 2, wherein The photometric error loss function can be expressed as follows: r(p i )=I j (π(T ji *T CL *p i ))-(aI i (π(T cL *p i ))+b) Among them, p i is the spatial point corresponding to the previous frame in the two adjacent frames, T ji is the relative pose between the two adjacent frames, T CL is the spatial transformation parameter between the camera and the lidar, I i is the image data corresponding to the previous frame in the two adjacent frames, I j is the image data corresponding to the latter frame of the two adjacent frames, and a and b are calculation parameters.

4. The method according to claim 1, wherein The acquiring a first relative pose based on the radar point cloud data of the two adjacent frames includes: Based on an 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 latter frame among the two adjacent frames to obtain a first target point corresponding to the edge point; Based on the plane 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 latter frame in the two adjacent frames to obtain a second target point corresponding to the plane point; Based on the edge point and the first target point, obtaining a first distance between the edge point and a corresponding matching line; Based on the plane point and the second target point, obtaining a second distance between the plane point and the corresponding matching surface; The first relative posture is acquired based on the first distance and the second distance.

5. The method according to claim 1, wherein The acquiring, based on the photometric error and the first relative pose, a second relative pose between the two adjacent frames, comprises: Acquire a geometric error between the two adjacent frames based on the first relative pose; The photometric error and the geometric error are optimized and solved to obtain the second relative pose.

6. The method according to claim 1, wherein The performing closed-loop detection based on the image data corresponding to the previous frame and the radar point cloud data includes: Screening the candidate closed-loop key frames corresponding to the previous frame based on the bag-of-words model to obtain valid candidate closed-loop key frames, and performing a secondary screening on the valid candidate closed-loop key frames based on temporal consistency to determine whether a first target closed-loop key frame exists; Based on the radar point cloud data of the previous frame, a historical key frame corresponding to the previous frame is detected to determine whether a second target closed-loop key frame corresponding to the previous frame exists in the historical key frame.

7. A SLAM device that fuses image data and radar point cloud data, characterized in that: include: A first acquisition module is used to acquire image data and radar point cloud data of each of two adjacent frames; A second acquisition module is used to obtain the photometric error between the two adjacent frames; A first processing module is configured to obtain a first relative pose based on the radar point cloud data of the two adjacent frames; A second processing module is configured to obtain a second relative pose between the two adjacent frames based on the photometric error and the first relative pose; a third processing module, configured to detect whether a previous frame of the two adjacent frames is a key frame, and, when it is determined that the previous frame is a key frame, perform closed-loop detection based on the image data and the radar point cloud data corresponding to the previous frame; The fourth processing module is used to determine whether the closed-loop detection is effective, and use the closed-loop constraint to perform global pose optimization on the second relative pose to obtain a target pose.

8. An electronic device, characterized in that: include: a processor, and a memory communicatively connected to the processor; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory to implement the method according to any one of claims 1 to 6.

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

10. A computer program product, characterized in that The invention comprises a computer program, which implements the method according to any one of claims 1 to 6 when being executed by a processor.

Images