A mapping method and system integrating semi-direct vision and radar odometry

Abstract

The present invention discloses a mapping method and system that integrates semi-direct vision and radar odometry. The method includes obtaining a pixel correspondence relationship between a current frame and a key frame; using a high-frequency camera to obtain image frames within a preset time period, processing the image frames to obtain an initial pose of the image frames, and optimizing the initial pose to a first optimized pose according to the correspondence relationship; dividing the image frames into key frames and non-key frames, optimizing the first optimized pose of the image frames to a second optimized pose according to a reprojection error, and performing multi-frame joint optimization on the second optimized pose of the key frames to form a third optimized pose of the key frames; integrating the second optimized pose of the non-key frames with the third optimized pose of the key frames to generate a visual pose; aligning the low-frequency radar with the high-frequency camera frequency to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera, using the visual pose as the input pose of the radar to perform residual constraints on the reconstructed radar point cloud, and outputting the final pose and map.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular to a mapping method and system integrating semi-direct vision and radar odometry. Background Art

[0002] Visual odometry and radar odometry are two widely used solutions for 6-DOF (Degrees-of-Freedom) pose estimation and mapping, and are fundamental technologies for many robotics and computer vision applications, such as self-driving cars and autonomous navigation. Using an RGB (Red-Green-Blue) camera or radar as the sole input sensor for odometry has complementary advantages and disadvantages. Visual odometry can output poses and map points at a high frequency (i.e., 30-60Hz), but its accuracy is inferior to radar odometry due to its poor robustness to blurry images and low texture content. Radar odometry can provide more accurate pose estimation and mapping than visual odometry, but its output frequency is limited by the low frequency of the input point cloud (e.g., 10Hz).

[0003] There is currently a lot of advanced work combining the complementary strengths of vision and radar sensors to achieve high-frequency output and more accurate pose estimation and mapping. State-of-the-art visual-radar odometry can be divided into two categories: loosely coupled and tightly coupled. Loosely coupled methods, such as DEVO (A real-time method for depth-enhanced visual odometry), LIMO (Lidar monocular visual odometry), and DVL-SLAM (sparse depth-enhanced direct visual lidar SLAM), only use the three-dimensional point cloud from the radar to provide depth measurements for visual odometry, ignoring the radar's advantage in accurate pose estimation. Tightly coupled systems can better utilize the complementary strengths of vision and radar. The most eye-catching work on tightly coupled methods is V-LOAM (Visual lidar odometry and mapping: Low drift, robust, and fast), in which the depth provided by the radar enables the visual odometry to run at a high frequency, and the radar odometry optimizes the pose obtained by the visual odometry at a low frequency, thereby improving the pose accuracy.

[0004] However, due to the high frequency of visual odometry, many camera poses are not improved by the radar module in V-LOAM. In addition, V-LOAM uses a feature-based approach for its visual module, which inevitably leads to 3D-2D depth correlation errors. Direct methods using visual odometry can potentially avoid 3D-2D depth correlation errors. However, compared to feature-based methods, direct methods rely on minimizing photometric errors to estimate poses, are prone to falling into local minima, and are sensitive to photometric changes and camera extrinsics.

[0005] In view of this, overcoming the defects of the prior art is an urgent problem to be solved in this technical field. Summary of the Invention

[0006] The problem to be solved by the present invention is to solve the problem of 3D-2D depth correlation error caused by the tight coupling method and the photometric error of the direct method.

[0007] The present invention adopts the following technical solutions:

[0008] In a first aspect, the present invention proposes a mapping method that integrates semi-direct vision and radar odometry, comprising:

[0009] Preferably, obtaining a pixel correspondence between the current frame and the key frame;

[0010] Acquiring all image frames within a preset time period using a high-frequency camera, processing the image frames to obtain initial poses corresponding to the image frames, and optimizing the initial poses to first optimized poses based on the pixel correspondences;

[0011] The image frames are divided into key frames and non-key frames, the first optimized pose of the image frame is optimized to the second optimized pose according to the reprojection error, and the second optimized pose of the key frame is jointly optimized by multiple frames to form the third optimized pose of the key frame;

[0012] Integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose;

[0013] The low-frequency radar and high-frequency camera frequencies are aligned according to the frequency of the visual pose to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera. The visual pose is used as the input pose of the radar to perform residual constraints on the reconstructed radar point cloud to output the final pose and map.

[0014] Preferably, before obtaining the pixel correspondence between the current frame and the key frame, the method further includes constructing a local depth map, which specifically includes:

[0015] Acquire radar point clouds and image frames within a preset time period, and acquire all key frames within the image frames, wherein the key frames are image frames with the same timestamp as the radar point clouds;

[0016] Selectively extract depth points or non-depth points within key frames according to a preset strategy;

[0017] The depth points and the non-depth points are mapped to form a local depth map.

[0018] Preferably, before extracting the depth points and non-depth points in the radar point cloud, the radar point cloud is preprocessed, specifically including:

[0019] Based on the radar point cloud, a ground segmentation method is used to detect candidate ground points;

[0020] Fitting the candidate ground points to obtain an accurate ground point cloud, obtaining a refined ground point cloud of the current frame based on the accurate ground point cloud, and calculating a first percentage of the refined ground point cloud to the radar point cloud;

[0021] If the first percentage is above a first threshold, extracting depth points and non-depth points with high gradients;

[0022] If the first percentage is not higher than the first threshold, a step of further differentiating the ground point cloud of the current frame is performed.

[0023] Preferably, the step of further distinguishing the refined ground point cloud of the current frame specifically includes:

[0024] using a fast segmentation method to distinguish the point cloud with irregular structure information in the refined ground point cloud of the current frame, and obtaining a second percentage of the point cloud with irregular structure information and the refined ground point cloud;

[0025] If the second percentage is higher than the second threshold, depth points and non-depth points are extracted; if the second percentage is not higher than the second threshold, depth points are extracted.

[0026] Preferably, the extracting depth points specifically includes:

[0027] Acquire a key frame and a radar point cloud with the same timestamp as the key frame, and project the radar point cloud onto the key frame to acquire a point extraction image;

[0028] The point extraction image is divided into multiple image blocks Ti of preset size, and the gradient threshold th (T i );

[0029] Divide the point extraction image into image blocks m of size n×n evenly i , and extract each image block m i All the depth points within form a subset S of depth points i ; Wherein, the value of n is determined by the expected number of points extracted from the point extraction image;

[0030] Calculate subset S i The gradient of all depth points in the , and get the depth point p with the largest gradient h , determine the depth point p h The corresponding image block T p ;

[0031] If the depth point p h The gradient is higher than the gradient threshold th(T p ), then the depth point p h Add to as a newly extracted depth point.

[0032] Preferably, the step of establishing the reprojection error to change the first optimized pose of the image frame to the second optimized pose specifically includes:

[0033] Get all keyframes within a preset time period and extract the depth points and non-depth points in all keyframes;

[0034] A reprojection error model is established to process the depth points and non-depth points in the key frame to output a second optimized pose after eliminating the accumulated errors of depth points and high-gradient non-depth points.

[0035] Preferably, obtaining the pixel correspondence between the current frame and the key frame specifically includes:

[0036] Get all points in the local depth map;

[0037] In a traversal manner, all points in the local depth map are projected into the current frame to obtain the pixel correspondence between the current frame and the key frame.

[0038] Preferably, the low-frequency radar and the high-frequency camera frequencies are aligned according to the frequency of the visual pose to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera, specifically including:

[0039] According to the low-frequency radar cycle and the high-frequency camera cycle, obtaining time points of image frames captured by the high-frequency camera corresponding to at least two cycles of the low-frequency radar, wherein the two cycles are a first cycle and a second cycle that are consecutive;

[0040] The end portion of the first cycle is divided into the same time as the low-frequency radar cycle, and the end portion of the first cycle is compensated for the same time as the low-frequency radar cycle to form a third cycle.

[0041] The end of the first cycle is divided into a time equal to twice the period of the low-frequency radar, and the tail of the first cycle is compensated with a time equal to twice the period of the low-frequency radar to reorganize into a fourth cycle;

[0042] Recursive segmentation is adopted until the first cycle and the second cycle are segmented to form the Nth cycle, where N ≥ 4;

[0043] The radar is used to perform reconstruction scans according to the third cycle, the fourth cycle, ..., and the Nth cycle to adjust the frequency of the radar point cloud to the same as that of the high-frequency camera.

[0044] Preferably, the obtaining of the reconstructed radar point cloud having the same frequency as the high-frequency camera and performing residual constraint on the reconstructed radar point cloud specifically includes:

[0045] Get any point A in the reconstructed radar point cloud and the initial pose corresponding to point A;

[0046] Project point A from the radar coordinate system to the world coordinate system to obtain point A′;

[0047] Find a preset number of points near point A′ in the local depth map and fit them into a plane, calculate the normal vector of the plane, and establish a residual model from point A to the plane;

[0048] The residual model is used to calculate the distance from point A to the plane and minimize the distance to output the final pose and map.

[0049] In a second aspect, relative to the mapping method for fusing semi-direct vision and radar odometry in the first aspect, the present invention further proposes a mapping system for fusing semi-direct vision and radar odometry, the mapping system being applicable to the mapping method for fusing semi-direct vision and radar odometry in the first aspect, the mapping system comprising a point matching and propagation module, a direct pose estimation module, a pose refinement module, a sliding window module based on BA, an integrated conversion module, a scanning reconstruction module, and an adaptive optimization module;

[0050] The point matching and propagation module is used to obtain the pixel correspondence between the current frame and the key frame, and optimize the initial pose to a first optimized pose;

[0051] The direct pose estimation module is used to obtain the initial pose of the image;

[0052] The posture refinement module is used to optimize the first optimized posture into a second optimized posture;

[0053] The sliding window module based on BA is used to perform multi-frame joint optimization on the second optimized pose of the key frame to form a third optimized pose of the key frame;

[0054] The integrated conversion module is used to integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose;

[0055] The scanning and reconstruction module is used to align the low-frequency radar with the high-frequency camera frequency;

[0056] The adaptive optimization module is used to perform residual constraints on the radar point cloud to output the final pose and map.

[0057] Compared with the prior art, the beneficial effects of the embodiments of the present invention are as follows: the present invention forms a tightly coupled system with a high-frequency camera and a low-frequency radar, and reconstructs the original low-frequency radar point cloud input into a high-frequency radar point cloud with the same frequency as the camera through a scanning and reconstruction module, thereby solving the problem of different frequencies of the high-frequency camera input and the low-frequency radar input. The new point extraction module solves the problem of deep correlation and matching between the high-frequency camera and the low-frequency radar, thereby absorbing the respective advantages of the camera and radar odometry to achieve the purpose of outputting higher frequency and more accurate posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] To more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort.

[0059] Figure 1 This is a flow chart of a mapping method that integrates semi-direct vision and radar odometry, provided by an embodiment of the present invention;

[0060] Figure 2 1 is a schematic diagram of the structure of a mapping system that integrates semi-direct vision and radar odometry, provided by an embodiment of the present invention;

[0061] Figure 3 is a flow chart of a method for constructing a local depth map provided by an embodiment of the present invention;

[0062] Figure 4 is a flow chart of a method for point cloud preprocessing provided by an embodiment of the present invention;

[0063] Figure 5 This is a flowchart of the steps for further distinguishing the refined ground point cloud of the current frame in point cloud preprocessing provided by an embodiment of the present invention;

[0064] Figure 6 is a flow chart of a method for extracting depth points provided by an embodiment of the present invention;

[0065] Figure 7 is a flow chart of a method for establishing a reprojection error to convert a first optimized pose of an image frame into a second optimized pose provided by an embodiment of the present invention;

[0066] Figure 82 is a schematic diagram of a structure for obtaining pixel correspondence between a current frame and a key frame provided by an embodiment of the present invention;

[0067] Figure 9 This is a flow chart of a method for aligning the frequencies of a low-frequency radar and a high-frequency camera to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera, provided by an embodiment of the present invention;

[0068] Figure 10 This is a simplified diagram of the process of aligning the frequencies of a low-frequency radar and a high-frequency camera to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera, according to an embodiment of the present invention.

[0069] Figure 11 This is a flow chart of a method for performing residual constraints on a reconstructed radar point cloud provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0070] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0071] In the description of the present invention, the terms "inside", "outside", "longitudinal", "lateral", "upper", "lower", "top", "bottom", etc. indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and do not require that the present invention must be constructed and operated in a specific orientation. Therefore, they should not be understood as limitations on the present invention.

[0072] In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0073] Embodiment 1:

[0074] Embodiment 1 of the present invention provides a mapping method that integrates semi-direct vision and radar odometer. Figure 1 Shown, including:

[0075] Step 201: Obtain the pixel correspondence between the current frame and the key frame.

[0076] The mapping method of the embodiment of the present invention uses a high-frequency camera and a low-frequency radar as input. It should be understood that for high-frequency cameras and low-frequency radars, high frequency and low frequency are relative. The high frequency and low frequency in the embodiment of the present invention simply mean that the camera frequency used is higher than the radar scanning frequency. In actual use, the camera frequency used is usually an integer multiple of the radar scanning frequency, for example, a 60 Hz high-frequency camera and a 10 Hz low-frequency radar.

[0077] The embodiment of the present invention defines the point cloud obtained by a 360° scan of a low-frequency radar as one scan, resulting in a set of radar point sets. An image frame is defined as an image acquired by each shot of a high-frequency camera. Since the acquisition frequency of the camera of the embodiment of the present invention (e.g., 60 Hz) is higher than the acquisition frequency of the radar (e.g., 10 Hz), only one image frame corresponds to a set of radar point sets with the same timestamp within one radar cycle, and no other image frames have radar point sets corresponding to the same timestamp. The embodiment of the present invention defines a key frame as an image frame that has a radar point set corresponding to the same timestamp, and an image frame that does not have a radar point set corresponding to the same timestamp is a non-key frame. The embodiment of the present invention uses a point matching and propagation module to obtain the pixel correspondence between the current frame and the key frame. The specific method for obtaining the pixel correspondence between the current frame and the key frame of the embodiment of the present invention will be described in detail later and will not be described here.

[0078] Step 202: using a high-frequency camera to acquire all image frames within a preset time period, processing the image frames to acquire initial poses corresponding to the image frames, and optimizing the initial poses to first optimized poses according to the pixel correspondence.

[0079] like Figure 2 As shown, the embodiment of the present invention uses a direct pose estimation module to process the image frames captured by the high-frequency camera to obtain the initial pose of the current frame; then, through the pixel correspondence between the current frame and the key frame obtained by the point matching and propagation module, the initial pose of the image frame is optimized and the initial pose is optimized to a first optimized pose. The preset time of the embodiment of the present invention is determined according to actual conditions. Specifically, the direct pose estimation module calculates the photometric error and minimizes the photometric error to obtain an initial pose; the photometric error calculated by the direct pose estimation module is calculated as follows:

[0080]

[0081] Among them, ci refers to the current frame, kn refers to the latest key frame, ||·|| γ refers to the Huber norm, I(·) refers to the pixel intensity, ac and bc are hardware-related parameters that affect the illumination transfer function during the optimization process, and ωu is a weight that reduces pixels with high image gradients and can be calculated as follows (con is a constant):

[0082]

[0083] u is The two-dimensional pixel of the high gradient depth point extracted, u' is the projection of u in ci, and the calculation result is:

[0084]

[0085] Among them, d u is the depth of u, N u Indicates the 8-dot pattern of u, It is a mode point exist Projection on .

[0086] Step 203: Divide the image frame into key frames and non-key frames, optimize the first optimized pose of the image frame to a second optimized pose according to the reprojection error, and perform multi-frame joint optimization on the second optimized pose of the key frame to form a third optimized pose of the key frame.

[0087] like Figure 2 As described above, the embodiment of the present invention utilizes a pose refinement module to process an image frame (which may be a keyframe or a non-keyframe) based on the reprojection error, and optimizes the first optimized pose of the image frame to a second optimized pose. Then, based on the definition of a keyframe, the image frame is divided into a keyframe and a non-keyframe. For the keyframe, since there is a large error between the pixels of each keyframe, it is necessary to further optimize the cumulative error brought by all keyframes. A sliding window module based on Balanced Arithmetic (BA) is used to perform multi-frame joint optimization on all keyframes to obtain a third optimized pose with a more optimized keyframe.

[0088] Step 204: Integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose.

[0089] After eliminating the errors between the pixels of each key frame, the embodiment of the present invention uses the integrated conversion module (see Figure 2 ) integrates the third optimized pose of the key frame with the second optimized pose of the non-key frame to generate a visual pose. The integrated conversion module of the embodiment of the present invention uses the conversion formula The second optimized pose of the non-key frame obtained by the pose refinement module and the third optimized pose of the key frame obtained by the BA-based sliding window module are integrated to obtain the integrated pose of the intermediate variable. Then by Converted to visual pose, where is the most recent keyframe k n The visual pose at this time is sent to the lidar odometry module as the input pose of the radar. Specifically, for each key frame k n After performing point reverse matching, the embodiment of the present invention can find the extraction point (i.e., the point input by the new point extraction module) in the sliding window. ) on other key frames (i.e., kj∈K). Then, the embodiment of the present invention performs a sliding window based on BA to optimize

[0090]

[0091] in, is the dominant frame k h ,point and pi in target frame k j (Right now ) is a single reprojection residual determined by three elements of the two-dimensional correspondence on . The above formula is to jointly minimize all reprojection residuals in the sliding window, forming a multi-frame joint optimization to improve the accuracy and consistency of the pose.

[0092] Step 205: Align the low-frequency radar with the high-frequency camera frequency according to the frequency of the visual pose, obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera, and use the visual pose as the input pose of the radar to perform residual constraints on the reconstructed radar point cloud to output the final pose and map.

[0093] The embodiment of the present invention utilizes a scanning reconstruction module (see Figure 2 ) obtain the header of the visual pose data packet, and through the frequency information in the visual pose header, use the segmentation and reorganization method to adjust the frequency of the low-frequency radar to be equal to the frequency of the high-frequency camera. After the frequency of the low-frequency radar is adjusted, the frequency of the reorganized radar point cloud obtained by the adjusted radar is the same as the frequency of the camera. Based on the properties of the radar, it is impossible to obtain the same correspondence as the camera, so a more accurate posture is required as the initial posture of the input radar. The embodiment of the present invention uses the visual posture that has been optimized multiple times as the radar input posture, and then performs residual constraints on the reorganized radar point cloud to output the final posture and map, so that the posture and map finally output by the embodiment of the present invention are more accurate, and the output posture is changed from low frequency to high frequency.

[0094] The present invention combines a high-frequency camera and a low-frequency radar into a tightly coupled system, solving the problems of different high-frequency camera inputs and low-frequency radar inputs, as well as the deep correlation and matching between the high-frequency camera and the low-frequency radar. It can absorb the respective advantages of the camera and radar odometer to achieve the purpose of outputting higher frequency and more accurate posture.

[0095] In order to illustrate the complete solution of the present invention, the specific details of the present invention are explained in detail below. Before obtaining the pixel correspondence between the current frame and the key frame, the embodiment of the present invention also includes constructing a local depth map, such as Figure 3 As shown, specifically including:

[0096] Step 301: Acquire radar point clouds and image frames within a preset time period, and acquire all key frames in the image frames, wherein the key frames are image frames with the same timestamp as the radar point clouds.

[0097] Key frames in the embodiments of the present invention are image frames with the same timestamp as the radar point cloud. This means that the same timestamp corresponds to the moment when the camera in the embodiment of the present invention completes an image acquisition and the radar completes a 360° scan. Within a preset time period, the high-frequency camera captures one image frame per cycle (corresponding to the camera cycle), and the corresponding radar completes a 360° scan per cycle (corresponding to the radar cycle). Key frames within the preset time period can be retrieved using the timestamps. The preset time in the embodiments of the present invention is set based on actual conditions.

[0098] Step 302: selectively extract depth points or non-depth points in the key frame according to a preset strategy.

[0099] Because the frequency of the low-frequency radar in this embodiment of the present invention is lower than that of the high-frequency camera, when the radar point cloud is input into the system, it must correspond to a corresponding image frame (refer to the definition of keyframes above), and this image frame is a keyframe. This embodiment of the present invention processes the low-frequency input radar point cloud through a point cloud preprocessing module. Based on the point cloud preprocessing results, a preset strategy is selected. Depth points and non-depth points are then extracted from the low-frequency radar point cloud and the image frames captured by the high-frequency camera according to the selected preset strategy.

[0100] Step 303: Map the depth points and the non-depth points to form a local depth map.

[0101] After obtaining the depth points and non-depth points in the low-frequency radar point cloud and the image frame obtained by the high-frequency camera, a local depth map is formed through mapping, and the corresponding data is also stored in the local depth map.

[0102] For the embodiment of the present invention, before extracting the depth points and non-depth points in the radar point cloud, the radar point cloud is also pre-processed to control the extraction strategy of step 302 according to the situation of the radar point cloud. Figure 4 As shown, specifically including:

[0103] Step 401: Based on the radar point cloud, a ground segmentation method is used to detect candidate ground points.

[0104] According to the embodiment of the present invention, a ground segmentation method is used based on the original low-frequency input point cloud (for example, the 10 Hz input point cloud Si) to roughly detect candidate ground points, wherein the ground is usually a textureless area.

[0105] Step 402: Fit the candidate ground points to obtain a precise ground point cloud, obtain a refined ground point cloud of the current frame based on the precise ground point cloud, and calculate a first percentage of the refined ground point cloud to the radar point cloud.

[0106] This embodiment of the present invention fits the precise ground point cloud generated by RANSAC to obtain a refined ground point cloud for the current frame. A first percentage value is obtained by calculating the ratio of the refined ground point cloud to the radar point cloud. A larger first percentage indicates that more lidar points are located in texture-free areas.

[0107] Step 403: If the first percentage is higher than the first threshold, extract the depth points and the non-depth points with high gradient; if the first percentage is not higher than the first threshold, perform a step of further distinguishing the ground point cloud of the current frame.

[0108] The embodiment of the present invention sets a first threshold (for example, in the embodiment of the present invention, the first threshold is set to 0.8), and compares the first threshold with the first percentage. When the first percentage is higher than the set first threshold, it means that the number of points where the lidar point falls in the texture-free area exceeds the set threshold. In addition to extracting depth points, additional high-gradient non-depth points are also extracted to ensure that visual tracking is obvious. In addition, the embodiment of the present invention further refines the situation when the first percentage is not higher than the set first threshold, that is, the number of points where the lidar point falls in the texture-free area does not exceed the set threshold. The step of further distinguishing the refined ground point cloud of the current frame is as follows: Figure 5 As shown, specifically including:

[0109] Step 4031: Use a fast segmentation method to distinguish the point cloud with irregular structure information in the refined ground point cloud of the current frame, and obtain a second percentage of the point cloud with irregular structure information to the refined ground point cloud.

[0110] In the embodiment of the present invention, a segmentation method is used to obtain a point cloud of irregular structure information, and the point cloud of the irregular structure information is compared with the refined ground point cloud to obtain a second percentage.

[0111] Step 4032: If the second percentage is higher than the second threshold, extract the depth points and non-depth points; if the second percentage is not higher than the second threshold, extract the depth points.

[0112] In the embodiment of the present invention, for the point cloud of irregular structure information, the projection of the irregular structure is usually located on leaves or grass, and its depth is very similar to the surrounding pixels, and visual tracking is not obvious. The embodiment of the present invention further sets a second threshold (for example, in the embodiment of the present invention, the second threshold is set to 0.5). When the second percentage is higher than the set second threshold, that is, the points of irregular structure account for a large proportion, in addition to extracting depth points, it is also necessary to extract additional high-gradient non-depth points. When the embodiment of the present invention extracts additional high-gradient non-depth points and adds them to the depth point set. The non-depth points dominate the convergence of the sliding window optimization and assist visual tracking. At the same time, the depth points in the depth point set are mainly used to obtain an accurate measurement scale; if the second percentage is not higher than the second threshold, it means that the irregular points account for a small proportion and the regular points account for a large proportion, and then the depth points are extracted.

[0113] For the embodiment of the present invention, the formula d u =[T l c p l ] z The calculated depth value is of high precision, and the depth value remains unchanged while the program is running. If most of the lidar point cloud can be projected onto an area with rich visual texture on the image, then the high-gradient depth points extracted from the new point extraction module are sufficient for pose tracking. However, certain areas of a scene are often textureless or highly similar (for example, the ground and leaves). If most of the depth points are located in these textureless areas, then estimating the pose based solely on the depth points may become inaccurate. In this case, an embodiment of the present invention uses high-gradient non-depth points in the textured area to assist pose tracking. Although the depth values ​​of the non-depth points are obtained through depth filtering and are not as accurate as the depth values ​​of the depth points, the non-depth points still contain valid visual information and therefore help improve the accuracy of pose tracking.

[0114] The following describes the depth point extraction process of the embodiment of the present invention. The depth point extraction of the embodiment of the present invention is generally divided into two steps: first, the point cloud of the key frame kn is projected onto the image to obtain the corresponding depth point set, which is defined as Then from Extract a subset of uniformly distributed high gradient image points For the embodiment of the present invention, the depth point is extracted, such as Figure 6 As shown, specifically including:

[0115] Step 501: Obtain a key frame and a radar point cloud with the same timestamp as the key frame, and project the radar point cloud onto the key frame to obtain a point extraction image; divide the point extraction image into a plurality of image blocks T of a preset size i , according to each image block Ti The intensity histogram is calculated for each image block T i The corresponding gradient threshold th(T i ).

[0116] It can be understood that for the embodiment of the present invention, both the extraction of depth points and the extraction of non-depth points are for key frames (non-key frames enter the integrated conversion module directly after entering the pose refinement, and no depth points and non-depth points are extracted, see Figure 2 Before extracting depth points (or non-depth points), the radar point cloud must first be projected onto a keyframe with the same timestamp to obtain a point extraction image. The point extraction image is then processed into image blocks of a preset size (e.g., 32×32), and the gradient threshold of each image block is then obtained. The preset size in the embodiment of the present invention is set according to actual needs.

[0117] Step 502: Divide the point extraction image into image blocks m of size n×n i , and extract each image block m i All the depth points within form a subset S of depth points i ; wherein the value of n is determined by the expected number of points extracted from the point extraction image.

[0118] In the embodiment of the present invention, n is inversely proportional to the expected number of points (depth points) extracted from each point extraction image. The initial value of n is given by Calculate, where w and h are the width and height of the input image respectively, and num_expert is the number of points that the embodiment of the present invention wants to extract from each frame.

[0119] Step 503: Calculate the subset S i The gradient of all depth points in the , and get the depth point p with the largest gradient h , determine the depth point p h The corresponding image block T p ; If the depth point p h The gradient is higher than the gradient threshold th(T p ), then the depth point p h Add to as a newly extracted depth point.

[0120] Among them, the embodiment of the present invention T p is a block of a preset size in step 501. The embodiment of the present invention also proposes a method for extracting non-depth points, specifically: in the first step, a point extraction image is evenly divided into different blocks {T1, T2, ..., T q}, then according to T i The intensity histogram is calculated for each block Ti The corresponding gradient threshold th(T i ). The second step is to divide the image into n×n blocks evenly {m1,m2,…,m l}, for each block m i , the embodiment of the present invention extracts all Non-depth points form a subset S i Then calculate S i The gradient of all points in , and define the point corresponding to the maximum gradient as If p h The gradient is higher than th(T p )(T p is u h The embodiment of the present invention adds ph to the 32×32 block where as a newly extracted high-gradient non-depth point.

[0121] The embodiment of the present invention establishes a reprojection error to convert the first optimized pose of the image frame into the second optimized pose, such as Figure 7 As shown, specifically including:

[0122] Step 601: Acquire all key frames within a preset time period, and extract depth points and non-depth points in all key frames.

[0123] The method of obtaining key frames within a preset time period, as well as depth points and non-depth points within the key frames, has been described in detail above and will not be repeated here.

[0124] Step 602: Establish a reprojection error model, process the depth points and non-depth points in the key frame, and output a second optimized pose after eliminating the accumulated errors of the depth points and the high-gradient non-depth points.

[0125] The embodiment of the present invention establishes a reprojection error model through the posture refinement module, and after obtaining the pixel correspondence between the current frame and the key frame, further optimizes the first optimized posture of the current frame through the reprojection error model.

[0126]

[0127] in is the correspondence between the point ps on ci, K is the set of all key frames in the sliding window, and Pkm is the set of points extracted from km.

[0128] The embodiment of the present invention provides for obtaining pixel correspondences between the current frame and the key frame, specifically including: obtaining all points in the local depth map; and projecting all points in the local depth map into the current frame in a traversal manner to obtain pixel correspondences between the current frame and the key frame.

[0129] In the process of constructing a local depth map, the embodiment of the present invention stores depth points and waste depth points in the local depth map, and uses a traversal method to project the depth points and non-depth points in the local depth map into the current frame one by one to obtain the pixel correspondence between the current frame and the key frame. The depth points and non-depth points stored in the local depth map are all points in the key frame. The embodiment of the present invention uses a point matching and propagation module to obtain the correspondence between the current frame and the key frame, specifically: first, project the points in the local depth map into the current frame to obtain the initial pose Then, the embodiment of the present invention takes the corresponding relationship previously matched by p1 from the key frame kn closest to the current frame ci And considering the influence of photometric parameters, by aligning and initial position To optimize Coordinates:

[0130]

[0131]

[0132]

[0133] is the initial coordinate system To the current frame coordinate system Transformation matrix, The dominant frame coordinate system To the initial coordinate system The transformation matrix, I(·) is the pixel intensity, A1 is the affine warp matrix, and ac and bc are hardware-related parameters that affect the illumination transfer function during the optimization process.

[0134] like Figure 8 As shown, when obtaining the pixel correspondence between the current frame and the key frame, the embodiment of the present invention uses As an intermediate, instead of aligning u1 and because (i.e., kn) and the current frame ci are close enough so that their local resolutions are similar. Figure 2 As shown, Neighborhood and The neighborhood of is more similar than u1. Therefore, by and Alignment, the embodiment of the present invention can obtain a more accurate pixel correspondence between u1 and ci. Naturally, after ci is selected as the new key frame kn+1, when the next camera frame ci+1 arrives, the embodiment of the present invention can align (Right now )and Find the corresponding relationship of p1 in ci+1.

[0135] The embodiment of the present invention aligns the low-frequency radar with the high-frequency camera frequency according to the frequency of the visual posture, and obtains the reconstructed radar point cloud with the same frequency as the high-frequency camera, such as Figure 9 As shown, specifically including:

[0136] Step 701: According to the low-frequency radar cycle and the high-frequency camera cycle, obtain the time points of the image frames captured by the high-frequency camera corresponding to at least two cycles of the low-frequency radar, wherein the two cycles are a first cycle and a second cycle that are consecutive.

[0137] The purpose of aligning the low-frequency radar and high-frequency camera frequencies in this embodiment of the present invention is to convert the low-frequency radar into a high-frequency radar (equal to the frequency of the high-frequency camera), reconstruct the radar point cloud through the high-frequency radar output, and then process the reconstructed radar point cloud to achieve the purpose of outputting high-frequency pose and map.

[0138] Step 702: Split the end of the first cycle to the same time as the low-frequency radar cycle, and compensate the end of the first cycle with the same time as the low-frequency radar cycle to reorganize into a third cycle; split the end of the first cycle to the same time as twice the low-frequency radar cycle, and compensate the end of the first cycle with the same time as twice the low-frequency radar cycle to reorganize into a fourth cycle.

[0139] In this embodiment of the present invention, the first and second cycles of the low-frequency radar are first segmented into several cycles of the high-frequency camera. The cycles are then reassembled and the time intervals within adjacent reassembled cycles are set to be the same as the cycle of the high-frequency camera (e.g., the time interval between the third and fourth cycles). The reassembled cycles are used as the radar scanning cycles, and the time intervals between adjacent reassembled cycles are used as the radar scanning intervals, thereby adjusting the frequency of the radar point cloud to be the same as that of the high-frequency camera.

[0140] Step 703: Recursively segment the image until the first and second cycles are segmented to form the Nth cycle, where N ≥ 4. The radar is then used to reconstruct and scan the image according to the third, fourth, and Nth cycles to adjust the frequency of the radar point cloud to the same as that of the high-frequency camera.

[0141] The embodiment of the present invention divides the time within the low-frequency radar cycle into several high-frequency camera cycles, and then reorganizes them, and adjusts the frequency of the radar point cloud to the same as that of the high-frequency camera by resetting the initial moment of the radar scan. For ease of understanding, the following is an analysis of a specific example to illustrate the method and process of aligning the low-frequency radar and high-frequency camera frequencies in the embodiment of the present invention. The embodiment of the present invention uses a scanning and reconstruction module to align the low-frequency radar and high-frequency camera frequencies. Assuming that the frequency of the radar is 10HZ and the frequency of the high-frequency camera is 60HZ, as shown in FIG. Figure 10 As shown, for a low-frequency radar scan S j :The scan starts from t j_1 Start, to t j End, in [t j-1 , t j ] During this period, the camera captured a total of 6 images, and the 6 camera images correspond to t i+1 , t i+2 , t i+3 , t i+4 , t i+5 and t i+6 Shoot. Only in t i+6 The camera image has the same timestamp as the radar point cloud (the image frame at this time is the key frame). Based on the characteristics of continuous acquisition of the lidar over a period of time, it can be defined that the radar is [t i+1 ,t i+7 ]The captured point cloud is used as the reconstructed scan S i+7 , which corresponds to the camera at t i+7 Similarly, the embodiment of the present invention can also obtain the reconstructed scan S i+8 , S i+9 ,…,S i+17 , corresponding to the camera at t i+8 , t i+9 ,…,t i+17 The image at the moment , so that the radar obtains a reconstructed radar scan input with the same frequency as the camera input.

[0142] In the embodiment of the present invention, after aligning the frequency of the low-frequency radar with the frequency of the high-frequency camera by the segmentation and recombination method, the adaptive optimization module is used to process the reconstructed radar point cloud. The residual constraint is applied to the reconstructed radar point cloud, such as Figure 11 As shown, specifically including:

[0143] Step 801: Obtain any point A in the reconstructed radar point cloud and the initial pose corresponding to point A; project point A from the radar coordinate system to the world coordinate system to obtain point A′.

[0144] Definition of the embodiment of the present invention (·) w is the world coordinate system, (·) c is the camera coordinate system, (·) l is the radar coordinate system, and is defined is the camera coordinate system (·) c To radar coordinate system (·) l The transformation matrix can be used to transform the radar and camera coordinate systems into the world coordinate system, and then obtain the world coordinates corresponding to the reconstructed radar point cloud.

[0145] Step 802: Find a preset number of points nearest to point A′ in the local depth map and fit them into a plane, calculate the normal vector of the plane, and establish a residual model from point A to the plane.

[0146] In the embodiment of the present invention, a residual model is constructed in the local depth map, and points in the local depth map are projected one by one into a plane for analysis.

[0147] Step 803: Calculate the distance from point A to the plane using the residual model and minimize the distance to output the final pose and map.

[0148] The adaptive scan-to-map optimization module of this embodiment of the present invention automatically determines whether to optimize three horizontal DOFs (i.e., yaw, front, and right) or six DOFs based on the geometric constraints on the vertical direction of the reconstructed scan Si. Specifically, this embodiment of the present invention checks whether vertically oriented faces are evenly distributed on the same plane. If nearly all point-to-vertical faces lie on the same plane (typically the base plane), the residuals they provide are difficult and well constrained in the vertical direction. In this case, this embodiment of the present invention optimizes only the three horizontal DOF poses. As each reconstructed scan Si arrives, this embodiment uses the point cloud preprocessing module to detect the ground surface and calculate the normal vector of the ground surface nG. When constructing the point-to-plane residual for pose estimation, the surface ε around the target point p is calculated and the normal n of ε is calculated. If the directions of n and nG are very similar, this embodiment considers p as a ground point candidate. This embodiment calculates the number of ground point candidates N in Si and the number of points M located on the ground surface. The percentage of ground surface curved to vertical surfaces is then calculated as M / N. If M / N is higher than a preset first threshold (e.g., set to 0.8), the vertical geometric constraint is considered poor. In this case, only the three horizontal DOFs (i.e., lateral, forward, and right) of the current reconstruction scan Si are optimized. Otherwise, the full six DOFs are scanned.

[0149] The present invention combines a high-frequency camera with a low-frequency radar to form a tightly coupled system, resolving the issues of different inputs from the high-frequency camera and low-frequency radar, as well as the deep correlation and matching between the high-frequency camera and low-frequency radar. This system leverages the respective strengths of the camera and radar odometry to achieve higher-frequency and more accurate pose output. The mapping system of the present invention comprises two main components: a semi-direct depth-enhanced visual odometry and a radar odometry. Leveraging the respective strengths of the camera and lidar, the system achieves deep correlation between camera images and radar point clouds. The radar scan reconstruction module increases the radar frequency, improving both output frequency and pose accuracy. On the public datasets KITTI, KITTI-360, and KITTI-CARLA, the method of the present invention demonstrates greater robustness and accuracy compared to existing visual-radar odometry systems.

[0150] Example 2:

[0151] Compared with the mapping method of the first embodiment of the present invention that integrates semi-direct vision and radar odometer, the second embodiment of the present invention further proposes a mapping system that integrates semi-direct vision and radar odometer, such as Figure 2 As shown, the method includes: a point matching and propagation module, a direct pose estimation module, a pose refinement module, a BA-based sliding window module, an integrated conversion module, a scanning reconstruction module and an adaptive optimization module.

[0152] The point matching and propagation module is used to obtain the pixel correspondence between the current frame and the key frame, and optimize the initial pose to the first optimized pose; the direct pose estimation module is used to obtain the initial pose of the image; the pose refinement module is used to optimize the first optimized pose to the second optimized pose; the BA-based sliding window module is used to perform multi-frame joint optimization on the second optimized pose of the key frame to form the third optimized pose of the key frame; the integrated conversion module is used to integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose; the scanning reconstruction module is used to align the low-frequency radar with the high-frequency camera frequency; the adaptive optimization module is used to perform residual constraints on the radar point cloud to output the final pose and map.

[0153] For details about the working mechanism of each module and the mapping process, please refer to Example 1, which will not be repeated here.

[0154] In addition, the mapping system of an embodiment of the present invention also includes a new point extraction module and a point cloud preprocessing module; the new point extraction constructs a local depth map through the depth points and non-depth points of the key frames, which is used for pose refinement and BA-based sliding windows, and the point cloud preprocessing module is used to determine the method for extracting key frames.

[0155] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A mapping method integrating semi-direct vision and radar odometry, characterized in that: include: Get the pixel correspondence between the current frame and the key frame; Acquiring all image frames within a preset time period using a high-frequency camera, processing the image frames to obtain initial poses corresponding to the image frames, and optimizing the initial poses to first optimized poses based on the pixel correspondences; The image frames are divided into key frames and non-key frames, the first optimized pose of the image frame is optimized to the second optimized pose according to the reprojection error, and the second optimized pose of the key frame is jointly optimized by multiple frames to form the third optimized pose of the key frame; Integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose; The low-frequency radar and high-frequency camera frequencies are aligned according to the frequency of the visual pose to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera. The visual pose is used as the input pose of the radar to perform residual constraints on the reconstructed radar point cloud to output the final pose and map.

2. The mapping method integrating semi-direct vision and radar odometry according to claim 1, characterized in that: Before obtaining the pixel correspondence between the current frame and the key frame, the method further includes constructing a local depth map, specifically including: Acquire radar point clouds and image frames within a preset time period, and acquire all key frames within the image frames, wherein the key frames are image frames with the same timestamp as the radar point clouds; Selectively extract depth points or non-depth points within key frames according to a preset strategy; The depth points and the non-depth points are mapped to form a local depth map.

3. The mapping method integrating semi-direct vision and radar odometry according to claim 2, characterized in that: Before extracting the depth points and non-depth points in the radar point cloud, the radar point cloud is preprocessed, including: Based on the radar point cloud, a ground segmentation method is used to detect candidate ground points; Fitting the candidate ground points to obtain an accurate ground point cloud, obtaining a refined ground point cloud of the current frame based on the accurate ground point cloud, and calculating a first percentage of the refined ground point cloud to the radar point cloud; If the first percentage is above a first threshold, extracting depth points and non-depth points with high gradients; If the first percentage is not higher than the first threshold, a step of further differentiating the ground point cloud of the current frame is performed.

4. The mapping method integrating semi-direct vision and radar odometry according to claim 3, characterized in that: The step of further distinguishing the refined ground point cloud of the current frame specifically includes: using a fast segmentation method to distinguish the point cloud with irregular structure information in the refined ground point cloud of the current frame, and obtaining a second percentage of the point cloud with irregular structure information and the refined ground point cloud; If the second percentage is higher than the second threshold, depth points and non-depth points are extracted; if the second percentage is not higher than the second threshold, depth points are extracted.

5. The mapping method integrating semi-direct vision and radar odometry according to claim 3, characterized in that: The extraction of depth points specifically includes: Acquire a key frame and a radar point cloud with the same timestamp as the key frame, and project the radar point cloud onto the key frame to acquire a point extraction image; Divide the point extraction image into multiple image blocks of preset size T i , according to each image block T i The intensity histogram is calculated for each image block T i The corresponding gradient threshold th(T i ); Divide the point extraction image into image blocks m of size n×n evenly i , and extract each image block m i All the depth points within form a subset S of depth points i ; Wherein, the value of n is determined by the expected number of points extracted from the point extraction image; Calculate subset S i The gradient of all depth points in the , and get the depth point p with the largest gradient h , determine the depth point p h The corresponding image block T p ; If the depth point p h The gradient is higher than the gradient threshold th(T p ), then the depth point p h Add to as a newly extracted depth point.

6. The mapping method integrating semi-direct vision and radar odometry according to claim 2, characterized in that: The step of changing the first optimized pose of the image frame to the second optimized pose according to the reprojection error specifically includes: Get all keyframes within a preset time period and extract the depth points and non-depth points in all keyframes; A reprojection error model is established to process the depth points and non-depth points in the key frame to output a second optimized pose after eliminating the accumulated errors of depth points and high-gradient non-depth points.

7. The mapping method integrating semi-direct vision and radar odometry according to claim 2, characterized in that: The obtaining of the pixel correspondence between the current frame and the key frame specifically includes: Get all points in the local depth map; In a traversal manner, all points in the local depth map are projected into the current frame to obtain the pixel correspondence between the current frame and the key frame.

8. The mapping method integrating semi-direct vision and radar odometry according to claim 2, characterized in that: Align the low-frequency radar with the high-frequency camera frequency according to the frequency of the visual pose to obtain a reconstructed radar point cloud with the same frequency as the high-frequency camera. Specifically, the following steps are performed: According to the low-frequency radar cycle and the high-frequency camera cycle, obtaining time points of image frames captured by the high-frequency camera corresponding to at least two cycles of the low-frequency radar, wherein the two cycles are a first cycle and a second cycle that are consecutive; The end portion of the first cycle is divided into the same time as the low-frequency radar cycle, and the end portion of the first cycle is compensated for the same time as the low-frequency radar cycle to form a third cycle. The end of the first cycle is divided into a time equal to twice the period of the low-frequency radar, and the tail of the first cycle is compensated with a time equal to twice the period of the low-frequency radar to reorganize into a fourth cycle; Recursive segmentation is adopted until the first cycle and the second cycle are segmented to form the Nth cycle, where N ≥ 4; The radar is used to perform reconstruction scans according to the third cycle, the fourth cycle, ..., and the Nth cycle to adjust the frequency of the radar point cloud to the same as that of the high-frequency camera.

9. The mapping method integrating semi-direct vision and radar odometry according to claim 1, characterized in that: The performing residual constraint on the reconstructed radar point cloud specifically includes: Get any point A in the reconstructed radar point cloud and the initial pose corresponding to point A; Project point A from the radar coordinate system to the world coordinate system to obtain point A′; Find a preset number of points near point A′ in the local depth map and fit them into a plane, calculate the normal vector of the plane, and establish a residual model from point A to the plane; The residual model is used to calculate the distance from point A to the plane and minimize the distance to output the final pose and map.

10. A mapping system integrating semi-direct vision and radar odometry, characterized in that: The mapping method for integrating semi-direct vision and radar odometry according to any one of claims 1 to 9, wherein the mapping system comprises a point matching and propagation module, a direct pose estimation module, a pose refinement module, a sliding window module based on BA, an integrated conversion module, a scanning reconstruction module, and an adaptive optimization module; The point matching and propagation module is used to obtain the pixel correspondence between the current frame and the key frame, and optimize the initial pose to a first optimized pose; The direct pose estimation module is used to obtain the initial pose of the image; The posture refinement module is used to optimize the first optimized posture into a second optimized posture; The sliding window module based on BA is used to perform multi-frame joint optimization on the second optimized pose of the key frame to form a third optimized pose of the key frame; The integrated conversion module is used to integrate the second optimized pose of the non-key frame with the third optimized pose of the key frame to generate a visual pose; The scanning and reconstruction module is used to align the low-frequency radar with the high-frequency camera frequency; The adaptive optimization module is used to perform residual constraints on the radar point cloud to output the final pose and map.

Images