RGBD sensor-assisted lidar and camera joint calibration method

Abstract

The present invention discloses a laser radar and camera joint calibration method assisted by an RGBD sensor, comprising the following steps: placing an RGBD sensor at a position where the laser radar and the camera have overlapping fields of view, collecting data from each sensor; using the RGB image I collected by the RGBD sensor to obtain the RGB image data; d and the depth image D d Generate point cloud P d ; for P d And the point cloud P collected by LiDAR l Perform registration to obtain the transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 1 ; to I d and the RGB image I captured by the camera c Perform feature extraction and matching to obtain P d with I c The matching relationship of the feature points in P d with I c The feature points matched in the image are transformed from the RGBD sensor coordinate system to the camera coordinate system using the PnP algorithm. 2 According to T 1 and T 2 The transformation relationship between the laser radar coordinate system and the camera coordinate system is calculated to achieve joint calibration between the two.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-sensor joint calibration, and in particular relates to a laser radar and camera joint calibration method assisted by an RGBD sensor. Background Art

[0002] The point cloud data collected by the LiDAR can provide high-precision distance information and is not affected by lighting, but lacks color and texture information; the image data collected by the camera can provide rich color and texture information, but the image effect is easily affected by ambient lighting and cannot provide distance information. In the fields of autonomous driving, robot navigation, etc., two different sensors, LiDAR and camera, are usually used at the same time to improve the accuracy and robustness of environmental perception or navigation positioning. However, if the calibration parameters of the LiDAR and camera are inaccurate, it will cause the LiDAR and camera data to be incorrectly fused, which will greatly reduce the accuracy and robustness of environmental perception or navigation positioning. Therefore, accurate calibration of the relative posture relationship between the LiDAR and the camera is a necessary prerequisite for completing downstream tasks. Although factory calibration can achieve accurate calibration of the relative posture relationship between the LiDAR and the camera, due to the inevitable vibration and impact during the work process, the maintenance and replacement of individual sensors due to failure, etc., the sensor installation position changes, and the relative posture relationship between the sensors often needs to be recalibrated.

[0003] Calibration of the relative relationship between LiDAR and camera through manual measurement is not only cumbersome, but also introduces human errors. Since the point cloud data of LiDAR and the image data of the camera are quite different, it is very difficult to match the spatial relationship between these two types of data. The joint calibration of LiDAR and camera has attracted widespread attention.

[0004] The patent "A method for calibrating multi-line laser radar and camera (publication number CN111369630A)" extracts the corner points of the plane calibration plate from the point cloud data and image data for registration to realize the calibration of the laser radar and camera. It is necessary to know the shape of the plane calibration plate a priori and place the plane calibration plate at different positions and collect multiple sets of data to complete the extraction of the corner points of the plane calibration plate in the point cloud data. The operation steps are complicated, and the method of extracting plane corner points from the point cloud data is not robust enough; the patent "Automatic calibration method and device, computer readable storage medium, terminal (publication number CN114519681A)" extracts the corner points of the plane calibration plate from the point cloud data. The joint calibration of LiDAR and camera is completed by extracting image depth continuous line features and point cloud depth continuous line features from image data and dense point cloud data. It is only applicable to the joint calibration of high-beam LiDAR and camera, and requires sufficient structural edges in the external environment. The patent "An end-to-end camera and LiDAR calibration method for autonomous driving (publication number CN117152262A)" uses a deep learning method to perform joint calibration of camera and LiDAR. When the external environment or the sensor used is different from the training data, the calibration accuracy will be greatly reduced, and the computing power of the calibration computer is required to be high.

[0005] In summary, the joint calibration of LiDAR and camera is of great significance, but the existing methods still have problems such as cumbersome steps, lack of robustness and low accuracy. Summary of the invention

[0006] In order to solve the above technical defects in the prior art, the present invention cleverly utilizes the advantages of RGBD sensors that can both collect RGB images and obtain pixel depth measurement values ​​to assist the joint calibration of lidar and camera. The point cloud data generated by the RGBD sensor is registered with the point cloud data collected by the lidar, and the RGB image collected by the RGBD sensor is matched with the RGB image collected by the camera for feature point matching. The operation is simple, the robustness and accuracy are high, and the requirements for the external environment are not high.

[0007] This application provides a RGBD sensor-assisted laser radar and camera joint calibration method, including the following steps:

[0008] S1: Place the RGBD sensor at a position where the field of view overlaps with the lidar and the camera, and collect data from the three sensors at the same time;

[0009] S2: The first RGB image I acquired by the RGBD sensor d and the depth image D d , based on the known intrinsic parameter matrix of the RGBD sensor, generate the first point cloud P in the RGBD sensor coordinate system d ;

[0010] S3: The second point cloud P collected by the laser radar l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 1 ;

[0011] S4: From the first RGB image I d Extract the first feature point X d , the second RGB image I captured from the camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding to the three-dimensional feature points, and the second image I c The matching relationship of the corresponding two-dimensional feature points;

[0012] S5: Based on the first point cloud P d Corresponding to the three-dimensional feature points, and the second image I c Corresponding to the matching relationship of the two-dimensional feature points, the transformation matrix T from the RGBD sensor coordinate system to the camera coordinate system is obtained through the PnP algorithm 2 ;

[0013] S6: Transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 1 , and the transformation matrix T from the RGBD sensor coordinate system to the camera coordinate system 2 , calculate the transformation relationship T between the lidar coordinate system and the camera coordinate system.

[0014] Furthermore, when collecting data from three sensors in step S1, each sensor only needs to collect one frame of data;

[0015] If there is a dynamic object in the sensor's field of view, the data of each sensor needs to be collected at the same time; if there is no dynamic object in the sensor's field of view, the data of each sensor needs to be collected at different times or at the same time.

[0016] Further, the first RGB image I collected by the RGBD sensor d and the depth image D d , based on the known intrinsic parameter matrix of the RGBD sensor, generate the first point cloud P in the RGBD sensor coordinate system d ,include:

[0017] S21: The first RGB image I dThe pixel coordinates are (u i ,v i ) is numbered as k i =v i W+u i , where W is the RGB image I d width;

[0018] S22: for the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d The pixel coordinates (u i ,v i ) is invalid, the current pixel is marked as an invalid pixel;

[0019] S23: For the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d Pixel coordinate u i ,v i ) at the depth value z i If it is valid, the current pixel is marked as a candidate pixel, and the internal reference matrix F and depth value z of the RGBD sensor are known. i The pixel coordinates (u i ,v i ) is back-projected into RGBD sensor coordinates (x i ,y i ,z i ):

[0020] in

[0021] S24: Based on the first RGB image I d The RGBD sensor coordinates corresponding to all candidate pixels in the image are calculated, the distance between each candidate pixel and the origin of the RGBD sensor coordinates is calculated, and the pixel whose distance is less than the preset threshold α is marked as a valid pixel;

[0022] S25: The first RGB image I d The RGBD sensor coordinates and numbers corresponding to all valid pixels in the RGBD sensor coordinate system are combined to obtain the first point cloud P with numbers in the RGBD sensor coordinate system. d ={(x i ,y i ,z i ,k i )|i=1,2,...,m}, m is the number of valid pixels.

[0023] Furthermore, the second point cloud P collected by the laser radar l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 1 ,include:

[0024] S31: Calculate the second point cloud P l The distance between each point in and the origin of the laser radar coordinate system is calculated, and the points whose distance is less than the preset threshold α are intercepted to form the third point cloud P after preprocessing. o ;

[0025] S32: Using the normal distribution algorithm to calculate the first point cloud P d and the third point cloud P o Perform a rough registration to obtain the initial transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 0 ;

[0026] S33: With the initial transformation matrix T 0 As the initial value, the iterative closest point algorithm is used to calculate the first point cloud P d and the third point cloud P o Perform precise registration to obtain the transformation matrix T from the laser radar coordinate system to the RGBD sensor coordinate system 1 .

[0027] Further, from the first RGB image I d Extract the first feature point X d , the second RGB image I captured from the camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding 3D feature points, and the second image I c The matching relationship corresponding to the two-dimensional feature points includes:

[0028] S41: For the first RGB image I d Extract the first feature point X d And get the corresponding SIFT descriptor for the second RGB image I c Extract the second feature point X c And get the corresponding SIFT descriptor;

[0029] S42: Match the first feature point X using a fast approximate nearest neighbor algorithm based on the Hamming distance of the SIFT descriptord and the second feature point X c , get the first feature point X d and the second feature point X c The matching relationship;

[0030] S43: Using the first feature point X d and the second feature point X c The matching relationship of the second feature point X is expanded c The pixel coordinate is X c ={(k i ,u i ,v i )|i=1,2,...,n},k i is through the first feature point X d The number of pixel coordinates calculated, (u i ,v i ) is the second feature point X c The original pixel coordinates, n is the first feature point X d and the second feature point X c The number of valid matching points;

[0031] S44: Find all points in the first point cloud P d and the second feature point X c The numbers k that appear simultaneously in j , then number k j The corresponding three-dimensional point (x j ,y j ,z j ) and the two-dimensional point (u j ,v j ) form a matching pair, that is, the first point cloud P is obtained d Corresponding 3D feature points and second image I c The matching relationship of corresponding two-dimensional feature points.

[0032] Furthermore, the transformation relationship T between the laser radar coordinate system and the camera coordinate system is determined by the following method:

[0033] Transformation matrix T from the lidar coordinate system to the RGBD sensor coordinate system 1 , and the transformation matrix T from the RGBD sensor coordinate system to the camera coordinate system 2 Direct multiplication, that is, T = T 1 T 2 .

[0034] Compared with the prior art, the present invention has the following significant advantages:

[0035] 1. The present invention uses the 3D point cloud of the RGBD sensor to align with the 3D point cloud of the LiDAR, and uses the 2D image of the RGBD sensor to match with the 2D image of the camera. It does not involve the matching between heterogeneous data, reduces the requirements for the external environment, and improves the reliability and calculation speed of the algorithm. Due to the large essential differences between the 3D point cloud data of the LiDAR and the 2D image data of the camera, the current joint calibration technology of the LiDAR and the camera often needs to use the rich structural edges in the environment to find the correspondence between the point cloud and the 2D pixel points, while the present invention only needs a small number of structural features in the sensor field of view.

[0036] 2. The present invention only needs a single frame of data from each sensor to complete the calibration, and is applicable to any type of laser radar and camera joint calibration, and does not require moving the laser radar and camera to collect multiple frames of data. Since the prior art often extracts edge features from point cloud data, this requires that the point cloud data is sufficiently dense, that is, the laser radar is a non-repetitive scanning laser radar or a high-beam laser radar, or the mobile sensor is used to accumulate point cloud data.

[0037] 3. The method of the present invention has a simple and convenient operation process. The RGBD sensor does not need to be installed or fixed, and can be carried or placed at will, and is plug-and-play.

[0038] 4. The present invention can complete calibration even when the laser radar and the camera have no overlapping fields of view. It only needs to ensure that the RGBD sensor has overlapping fields of view with the laser radar and the camera respectively during calibration.

[0039] Other features and advantages of the present invention will be described in the following description, and part of them will become obvious from the description, or will be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures particularly pointed out in the written description and the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0041] Figure 1 is a flow chart of the method of the present invention;

[0042] Figure 2 This is a schematic diagram of the result of converting the laser radar point cloud into the RGBD sensor coordinate system in the embodiment;

[0043] Figure 3 Schematic diagram of feature matching results between the camera RGB image and the RGBD sensor RGB image in the embodiment;

[0044] Figure 4Schematic diagram of the result of projecting the laser radar point cloud into the camera RGB image in the embodiment. DETAILED DESCRIPTION

[0045] It is easy to understand that according to the technical solution of the present invention, without changing the essential spirit of the present invention, a person skilled in the art can imagine various embodiments of the present invention. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical solution of the present invention, and should not be regarded as the whole of the present invention or as a limitation or limitation of the technical solution of the present invention. On the contrary, the purpose of providing these embodiments is to enable those skilled in the art to understand the present invention more thoroughly. The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the innovative concept of the present invention.

[0046] It should be noted that during the execution of the method provided in the present application, the RGBD sensor is used to assist in the joint calibration of the laser radar and the camera. It does not require fixed installation and can be carried and placed at will. It only requires that the RGBD sensor have an overlapping field of view with the laser radar and the camera when collecting data.

[0047] The environmental perception and positioning system is equipped with DJI Livox Horizon laser radar and Dahua Huarui A7A20CG9 industrial camera, and uses Intel RealSense D455 depth camera as RGBD sensor to assist the joint calibration of laser radar and camera, and the calibration scene is an outdoor autonomous driving scene. It should be noted that in this embodiment, DJI Livox Horizon laser radar, Dahua Huarui A7A20CG9 industrial camera and Intel RealSense D455 depth camera are all tools provided by this application to realize specific functions, and do not constitute a limitation of this application.

[0048] like Figure 1 As shown, the RGBD sensor-assisted laser radar and camera joint calibration method includes the following steps:

[0049] S1: Place the D455 depth camera at a position where it overlaps with the Horizon LiDAR and Dahua camera, and collect data from the three sensors at the same time;

[0050] S2: The first RGB image I captured by the D455 depth camera d and the depth image D d , based on the known intrinsic parameter matrix of the D455 depth camera, generate the first point cloud P in the D455 depth camera coordinate system d ;

[0051] S3: The second point cloud P collected by Horizon LiDAR l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system 1 ;

[0052] S4: From the first RGB image I d Extract the first feature point X d , the second RGB image I collected from the Dahua camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding to the three-dimensional feature points, and the second image I c The matching relationship of the corresponding two-dimensional feature points;

[0053] S5: Based on the first point cloud P d Corresponding to the three-dimensional feature points, and the second image I c Corresponding to the matching relationship of the two-dimensional feature points, the transformation matrix T from the D455 depth camera coordinate system to the Dahua camera coordinate system is obtained through the PnP algorithm. 2 ;

[0054] S6: Transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system 1 , and the transformation matrix T from the D455 depth camera coordinate system to the Dahua camera coordinate system 2 , calculate the transformation relationship T between the Horizon lidar coordinate system and the Dahua camera coordinate system.

[0055] Specifically, a frame of data from each sensor is collected at the same time, and the first RGB image I is collected using the D455 depth camera d and the depth image D d , the image resolution is 1280x960; the second RGB image I is collected using the Dahua camera c , the image resolution is 4096x3000; the second point cloud P is collected using Horizon LiDAR l .

[0056] Specifically, the first RGB image I captured by the D455 depth camera d and the depth image D d, based on the known intrinsic parameter matrix of the D455 depth camera, generate the first point cloud P in the D455 depth camera coordinate system d ,include:

[0057] S21: The first RGB image I d The pixel coordinates are (u i ,v i ) is numbered as k i =v i W+u i , where W = 1280 is the first RGB image I d width;

[0058] S22: for the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d The pixel coordinates (u i ,v i ) is invalid, the current pixel is marked as an invalid pixel;

[0059] S23: For the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d Pixel coordinate u i ,v i ) at the depth value z i If it is valid, the current pixel is marked as a candidate pixel, and the D455 depth camera has known intrinsic parameter matrix F and depth value z. i The pixel coordinates (u i ,v i ) is back-projected into the D455 depth camera coordinates (x i ,y i ,z i ):

[0060] in

[0061] S24: Based on the first RGB image I d The D455 depth camera coordinates corresponding to all candidate pixels in the image are calculated, and the distance between each candidate pixel and the origin of the D455 depth camera coordinates is calculated, and the pixels whose distance is less than the threshold a=8m are marked as valid pixels, where the distance threshold α is pre-set according to the device indicators of the D455 depth camera (the recommended range of the device is 0.4m-8m);

[0062] S25: The first RGB image Id The D455 depth camera coordinates and serial numbers corresponding to all valid pixels in the D455 depth camera coordinate system are combined to obtain the first point cloud P with serial numbers in the D455 depth camera coordinate system. d ={(x i ,y i ,z i ,k i )|i=1,2,...,m}, m is the number of valid pixels.

[0063] Specifically, the second point cloud P collected by the Horizon LiDAR l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system 1 ,include:

[0064] S31: Calculate the second point cloud P l The distance between each point in the Horizon laser radar coordinate system and the origin is calculated, and the points whose distance is less than the preset threshold a = 8m are intercepted to form the third point cloud P after preprocessing. o ;

[0065] S32: Use the normal distribution algorithm implemented in the open source point cloud library PCL to analyze the first point cloud P d and the third point cloud P o Perform a rough registration to obtain the initial transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system 0 ;

[0066] S33: With the initial transformation matrix T 0 As the initial value, the iterative closest point algorithm implemented in the open source point cloud library PCL is used to calculate the first point cloud P d and the third point cloud P o Perform precise registration to obtain the transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system 1 .

[0067] More specifically, Figure 2 As shown, the transformation matrix T can be used 1 The third point cloud P in the Horizon lidar coordinate system o Transform to the D455 depth camera coordinate system and check the transformation matrix T 1 Is it accurate.

[0068] Specifically, from the first RGB image I d Extract the first feature point X d, the second RGB image I captured from the camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding 3D feature points, and the second image I c The matching relationship corresponding to the two-dimensional feature points includes:

[0069] S41: Using the image processing open source library OpenCV to process the first RGB image I d Extract the first SIFT feature point X d And get the corresponding SIFT descriptor, use the image processing open source library OpenCV to process the second RGB image I c Extract the second SIFT feature point X c And get the corresponding SIFT descriptor;

[0070] S42: Match the first feature point X using a fast approximate nearest neighbor algorithm based on the Hamming distance of the SIFT descriptor d and the second feature point X c , get the first feature point X d and the second feature point X c The matching relationship, such as Figure 3 As shown (feature extraction and feature matching are both performed on the original resolution image. For ease of observation, the two images are sampled to the same resolution for display after feature matching);

[0071] S43: Using the first feature point X d and the second feature point X c The matching relationship of the second feature point X is expanded c The pixel coordinate is X c ={(k i ,u i ,v i )|i=1,2,...,n},k i is through the first feature point X d The number of pixel coordinates calculated, (u i ,v i ) is the second feature point X c The original pixel coordinates, n is the first feature point X d and the second feature point X c The number of valid matching points;

[0072] S44: Find all points in the first point cloud P d and the second feature point X c The numbers k that appear simultaneously in j, then number k j The corresponding three-dimensional point (x j ,y j ,z j ) and the two-dimensional point (u j ,v j ) form a matching pair, that is, the first point cloud P is obtained d Corresponding 3D feature points and second image I c The matching relationship of corresponding two-dimensional feature points.

[0073] Specifically, the first point cloud P d Corresponding 3D feature points and second image I c The matching relationship of the corresponding two-dimensional feature points, that is, the correspondence between the three-dimensional point and the two-dimensional point, is obtained by the PnP algorithm implemented in the image processing open source library OpenCV to obtain the transformation matrix T from the D455 depth camera coordinate system to the Dahua camera coordinate system. 2 .

[0074] Specifically, the transformation matrix T from the Horizon lidar coordinate system to the D455 depth camera coordinate system is 1 , and the transformation matrix T from the D455 depth camera coordinate system to the Dahua camera coordinate system 2 , we can get the transformation matrix T=T from the Horizon LiDAR coordinate system to the Dahua camera coordinate system 1 T 2 , you can also get the transformation matrix from the Dahua camera coordinate system to the Horizon lidar coordinate system

[0075] More specifically, Figure 4 As shown in the figure, using the transformation matrix T from the Horizon laser radar coordinate system to the Dahua camera coordinate system and the internal parameter matrix of the Dahua camera, the first point cloud P of the Horizon laser radar can be l Second image I projected onto Dahua camera c Check whether the calibration result is accurate.

[0076] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0077] It should be understood that in order to simplify the present invention and help those skilled in the art understand the various aspects of the present invention, in the above description of the exemplary embodiments of the present invention, the various features of the present invention are sometimes described in a single embodiment or described with reference to a single figure. However, the present invention should not be interpreted as the features included in the exemplary embodiments are all necessary technical features of the patent claims.

[0078] It should be understood that the modules, units, components, etc. included in the device of an embodiment of the present invention can be adaptively changed to be set in a device different from the embodiment. The different modules, units, or components included in the device of the embodiment can be combined into one module, unit, or component, or they can be divided into multiple sub-modules, sub-units, or sub-components.

Claims

1. The RGBD sensor-assisted laser radar and camera joint calibration method is characterized by: The method comprises the following steps: S1: Place the RGBD sensor at a position where the field of view overlaps with the lidar and the camera, and collect data from the three sensors at the same time; S2: The first RGB image I acquired by the RGBD sensor d and the depth image D d , based on the known intrinsic parameter matrix of the RGBD sensor, generate the first point cloud P in the RGBD sensor coordinate system d ; S3: The second point cloud P collected by the laser radar l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T1 from the laser radar coordinate system to the RGBD sensor coordinate system; S4: From the first RGB image I d Extract the first feature point X d , the second RGB image I captured from the camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding to the three-dimensional feature points, and the second image I c The matching relationship of the corresponding two-dimensional feature points; S5: Based on the first point cloud P d Corresponding to the three-dimensional feature points, and the second image I c Corresponding to the matching relationship of the two-dimensional feature points, the transformation matrix T2 from the RGBD sensor coordinate system to the camera coordinate system is obtained through the PnP algorithm; S6: Calculate the transformation relationship T between the laser radar coordinate system and the camera coordinate system according to the transformation matrix T1 from the laser radar coordinate system to the RGBD sensor coordinate system and the transformation matrix T2 from the RGBD sensor coordinate system to the camera coordinate system; Among them, the first RGB image I collected by the RGBD sensor d and the depth image D d , based on the known intrinsic parameter matrix of the RGBD sensor, generate the first point cloud P in the RGBD sensor coordinate system d ,include: S21: The first RGB image I d The pixel coordinates are (u i ,v i ) is numbered as k i =v i W+u i , where W is the RGB image I d width; S22: for the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d The pixel coordinates (u i ,v i ) is invalid, the current pixel is marked as an invalid pixel; S23: For the first RGB image I d The pixel coordinates are (u i ,v i ) pixels, if the depth image D d The pixel coordinates (u i ,v i ) at the depth value z i If it is valid, the current pixel is marked as a candidate pixel, and the internal reference matrix F and depth value z of the RGBD sensor are known. i The pixel coordinates (u i ,v i ) is back-projected into RGBD sensor coordinates (x i ,y i ,z i ): in S24: Based on the first RGB image I d The RGBD sensor coordinates corresponding to all candidate pixels in the image are calculated, the distance between each candidate pixel and the origin of the RGBD sensor coordinates is calculated, and the pixel whose distance is less than the preset threshold α is marked as a valid pixel; S25: The first RGB image I d The RGBD sensor coordinates and numbers corresponding to all valid pixels in the RGBD sensor coordinate system are combined to obtain the first point cloud P with numbers in the RGBD sensor coordinate system. d ={(x i ,y i ,z i ,k i )|i=1,2,...,m}, m is the number of valid pixels; From the first RGB image I d Extract the first feature point X d , the second RGB image I captured from the camera c The second feature point X is extracted from c , get the first feature point X through feature matching d and the second feature point X c The matching relationship is obtained, and the first point cloud P is further obtained. d Corresponding 3D feature points, and the second image I c The matching relationship corresponding to the two-dimensional feature points includes: S41: For the first RGB image I d Extract the first feature point X d And get the corresponding SIFT descriptor for the second RGB image I c Extract the second feature point X c And get the corresponding SIFT descriptor; S42: Match the first feature point X using a fast approximate nearest neighbor algorithm based on the Hamming distance of the SIFT descriptor d and the second feature point X c , get the first feature point X d and the second feature point X c The matching relationship; S43: Using the first feature point X d and the second feature point X c The matching relationship of the second feature point X is expanded c The pixel coordinate is X c ={(k i ,u i ,v i )|i=1,2,...,n},k i is through the first feature point X d The number of pixel coordinates calculated, (u i ,v i ) is the second feature point X c The original pixel coordinates, n is the first feature point X d and the second feature point X c The number of valid matching points; S44: Find all points in the first point cloud P d and the second feature point X c The numbers k that appear simultaneously in j , then number k j The corresponding three-dimensional point (x j ,y j ,z j ) and the two-dimensional point (u j ,v j ) to form a matching pair, that is, the first point cloud P is obtained d Corresponding 3D feature points and second image I c The matching relationship of corresponding two-dimensional feature points.

2. The RGBD sensor-assisted laser radar and camera joint calibration method according to claim 1, characterized in that: When collecting data from three sensors in step S1, each sensor only needs to collect one frame of data; If there is a dynamic object in the sensor's field of view, the data of each sensor needs to be collected at the same time; if there is no dynamic object in the sensor's field of view, the data of each sensor needs to be collected at different times or at the same time.

3. The RGBD sensor-assisted laser radar and camera joint calibration method according to claim 1, characterized in that: The second point cloud P collected by the laser radar l Preprocessing is performed to obtain the third point cloud P o , for the third point cloud P o and the first point cloud P d Perform registration to obtain the transformation matrix T1 from the laser radar coordinate system to the RGBD sensor coordinate system, including: S31: Calculate the second point cloud P l The distance between each point in and the origin of the laser radar coordinate system is calculated, and the points whose distance is less than the preset threshold α are intercepted to form the preprocessed third point cloud P o ; S32: Using the normal distribution algorithm to calculate the first point cloud P d and the third point cloud P o Perform coarse registration to obtain the initial transformation matrix T0 from the laser radar coordinate system to the RGBD sensor coordinate system; S33: Using the initial transformation matrix T0 as the initial value, the first point cloud P is transformed using the iterative closest point algorithm. d and the third point cloud P o Perform precise registration to obtain the transformation matrix T1 from the laser radar coordinate system to the RGBD sensor coordinate system.

4. The RGBD sensor-assisted laser radar and camera joint calibration method according to claim 1, characterized in that: The transformation relationship T between the laser radar coordinate system and the camera coordinate system is determined by the following method: The transformation matrix T1 from the laser radar coordinate system to the RGBD sensor coordinate system is directly multiplied by the transformation matrix T2 from the RGBD sensor coordinate system to the camera coordinate system, that is, T=T1T2.

Images