External parameter registration and inter-frame loading method based on laser radar and camera

Abstract

The invention provides an external parameter registration and inter-frame loading method based on a laser radar and a camera, and the method comprises the steps: obtaining point cloud data collected by the laser radar and image data collected by the camera, and carrying out the fitting and optimization of the point cloud data, obtaining a feature point coordinate value of the calibration object in the laser radar coordinate system and a feature linear equation of the calibration object in the laser radar coordinate system; determining feature point coordinate values of a calibration object in the image data under a camera coordinate system; determining a characteristic linear equation of the calibration object in a camera coordinate system; determining an error function of intersection point matching constraint based on the feature point coordinate values under the laser radar coordinate system and the camera coordinate system; based on the characteristic linear equations under the laser radar coordinate system and the camera coordinate system, determining an error function of the reprojection constraint; and determining an external parameter registration result of the laser radar and the camera through an iterative nearest point registration algorithm based on the error function of the intersection point matching constraint and the error function of the reprojection constraint.

Description

Technical Field

[0001] The present invention relates to the field of machine learning technology, and in particular to an external parameter registration and inter-frame loading method based on a laser radar and a camera. Background Art

[0002] With the continuous changes in sensors and various related technologies, most modern environmental perception systems are not limited to the use of just one sensor, but instead combine multiple sensors to form a joint sensor system based on demand.

[0003] Although multiple sensors can provide people with richer multi-dimensional information for perceiving the environment, before that, the problem of external parameter calibration of multi-sensor fusion must be solved first, that is, how to accurately align the data obtained by different sensors to ensure the accuracy and consistency of the information.

[0004] It can be seen that in the related art, when a plurality of different sensors are integrated and used, there is a technical problem that the external parameters cannot be aligned. Summary of the invention

[0005] The present invention provides an external parameter registration and inter-frame loading method based on laser radar and camera, which is used to solve the defect in the related art that the external parameters cannot be registered when a plurality of different sensors are integrated and used, and achieve a high-precision external parameter registration result.

[0006] The present invention provides an external parameter registration and inter-frame loading method based on a laser radar and a camera, wherein the laser radar and the camera have the same calibration object, and the method comprises the following steps.

[0007] The point cloud data collected by the laser radar and the image data collected by the camera are obtained, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include the calibration object; based on a random sampling consistency algorithm and a least squares method based on vertical distance, the point cloud data are fitted and optimized to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system; according to a checkerboard corner point detection algorithm based on seed growth, the feature point coordinate values ​​of the calibration object in the image data in the camera coordinate system are determined; the image data are color-coded. Color segmentation and straight line fitting are used to determine the characteristic straight line equation of the calibration object in the camera coordinate system; based on the characteristic point coordinate values ​​of the calibration object in the lidar coordinate system and the characteristic point coordinate values ​​of the calibration object in the camera coordinate system, an error function of the intersection matching constraint is determined; based on the characteristic straight line equation of the calibration object in the lidar coordinate system and the characteristic straight line equation of the calibration object in the camera coordinate system, an error function of the reprojection constraint is determined; and the external parameter registration result of the lidar and the camera is determined based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative nearest point registration algorithm.

[0008] According to a method for external parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, the calibration object includes: three pyramids with checkerboard patterns attached to the sides and a two-dimensional checkerboard calibration plate.

[0009] According to the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, the point cloud data is fitted and optimized based on the random sampling consistency algorithm and the least square method based on the vertical distance to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system, including: iteratively segmenting the point cloud data of the calibration object based on the random sampling consistency algorithm to obtain the initial values ​​of the fitting plane equations of four planes; taking the sum of the squares of the distances from the origin to the four planes as the objective function of the least squares method, optimizing the initial values ​​of the fitting plane equations of the four planes to obtain the plane equation parameters of the four planes; conditionally filtering based on the sum of the squares of the distances of all the inner points of the four planes in the height direction to determine the plane equation parameters of the two-dimensional chessboard calibration plate plane in the four planes; performing an intersection operation based on the plane equation parameters of the four planes and the plane equation parameters of the two-dimensional chessboard calibration plate plane to determine the feature point coordinate values ​​of the four intersection points of the four planes and the feature straight line equations of the three edges of the pyramid.

[0010] According to a method for external parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, after determining the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative nearest point registration algorithm, the method also includes: performing weighted summation on the error function of the intersection matching constraint and the error function of the reprojection constraint to obtain a total error function; performing iterative nearest point registration on the point cloud data based on the total error function to obtain a spatial variation matrix between all adjacent frames; and aligning the spatial variation matrix between all adjacent frames with the spatial variation matrix of the first frame to obtain a global feature point cloud of the laser radar.

[0011] According to a method for external parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, after aligning the spatial variation matrix between all adjacent frames with the spatial variation matrix of the first frame to obtain the global feature point cloud of the laser radar, the method further includes: based on the global feature point cloud, registering and aligning the point clouds of each frame in the point cloud data to perform loop optimization on the point clouds of each frame.

[0012] According to a method for external parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, after determining the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative nearest point registration algorithm, the method further includes: mapping the point cloud data to the image data based on the external parameter registration result to obtain a colored three-dimensional point cloud scene reconstruction result.

[0013] The present invention also provides an external parameter alignment and inter-frame loading device based on laser radar and camera, comprising the following modules: an acquisition module, used to acquire the point cloud data collected by the laser radar and the image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include the calibration object; a fitting and optimization module, used to fit and optimize the point cloud data based on a random sampling consistency algorithm and a least squares method based on vertical distance, to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system; a first determination module, used to determine the feature point coordinate values ​​of the calibration object in the image data in the camera coordinate system according to a checkerboard corner point detection algorithm based on seed growth point coordinate value; a second determination module, used to perform color segmentation and straight line fitting on the image data, and determine the characteristic straight line equation of the calibration object in the camera coordinate system; a matching constraint module, used to determine the error function of the intersection matching constraint based on the characteristic point coordinate value of the calibration object in the lidar coordinate system and the characteristic point coordinate value of the calibration object in the camera coordinate system; a projection constraint module, used to determine the error function of the reprojection constraint based on the characteristic straight line equation of the calibration object in the lidar coordinate system and the characteristic straight line equation of the calibration object in the camera coordinate system; an external parameter configuration module, used to determine the external parameter registration result of the lidar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative closest point registration algorithm.

[0014] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the external parameter registration and inter-frame loading method based on laser radar and camera as described in any one of the above methods is implemented.

[0015] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for external parameter registration and inter-frame loading based on a laser radar and a camera as described in any one of the above is implemented.

[0016] The present invention also provides a computer program product, including a computer program, which, when executed by a processor, implements any of the above-mentioned external parameter alignment and inter-frame loading methods based on laser radar and camera.

[0017] The external parameter alignment and inter-frame loading method based on laser radar and camera provided by the present invention uses the same calibration object and collects the point cloud data of the laser radar and the image data of the camera at the same time, thereby ensuring the synchronization and consistency of the data and providing a high-precision basis for subsequent alignment; the point cloud data is fitted and optimized using a random sampling consistency algorithm and a least squares method based on vertical distance, thereby improving the accuracy of the feature point coordinates and the feature straight line equation of the calibration object in the laser radar coordinate system; a checkerboard corner point detection algorithm based on seed growth is used to determine the feature point coordinates in the image data, and the image data is subjected to color segmentation and straight line fitting to determine the feature straight line equation of the calibration object in the camera coordinate system, thereby further improving the accuracy of the feature point and feature straight line equation in the camera coordinate system; the two error functions of intersection matching constraint and reprojection constraint are combined, and the external parameter alignment between the laser radar and the camera is optimized through these two constraint conditions. The intersection matching constraint uses the consistency of feature point coordinates to reduce errors, while the reprojection constraint uses the consistency of feature line equations to further correct the registration results. It can make full use of the information in point cloud data and image data to improve the robustness and accuracy of registration; thereby solving the technical problem in related technologies that external parameters cannot be registered when fusing multiple different sensors. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced one by one below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0019] Figure 1 It is a schematic diagram of the overall framework of the external parameter alignment and inter-frame loading method based on laser radar and camera provided by the present invention.

[0020] Figure 2 It is a flow chart of the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention.

[0021] Figure 3 It is a schematic diagram of the external parameter calibration data scanning method provided by the present invention.

[0022] Figure 4 It is a schematic diagram of corresponding frame extraction and interpolation provided by the present invention.

[0023] Figure 5 It is a RANSAC single feature plane segmentation and fitting flow chart provided by the present invention.

[0024] Figure 6It is a schematic diagram of the principle of sub-pixel corner point detection provided by the present invention.

[0025] Figure 7 It is a schematic diagram of the laser radar coordinate system, camera coordinate system and chessboard coordinate system provided by the present invention.

[0026] Figure 8 It is a schematic diagram of plane error sorting provided by the present invention.

[0027] Fig. 9 It is a schematic diagram of loop detection provided by the present invention.

[0028] Fig.10 This is a schematic diagram of the global point cloud solution provided by the present invention.

[0029] Fig.11 It is a schematic diagram of loop optimization provided by the present invention.

[0030] Fig.12 It is a schematic diagram of visualizing the calibration object point cloud and plane segmentation effect provided by the present invention.

[0031] Fig.13 It is a structural schematic diagram of the external parameter alignment and inter-frame loading device based on laser radar and camera provided by the present invention.

[0032] Fig.14 It is a schematic diagram of the physical structure of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0033] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0034] With the continuous development of digital technology, people are increasingly experiencing the convenience brought by 3D reconstruction technology in their lives. People can visit scenic spots online, preview house decoration, and other operations on their mobile phones. This virtual reality is inseparable from the support of 3D scene reconstruction technology. 3D reconstruction technology has already existed in every corner of people's lives without knowing it. It not only plays an indispensable role in intelligent driving, robots, metaverse, game development, etc., but also penetrates deeper and deeper into the fields of virtual attractions, online decoration, architectural design, and cultural heritage protection. In addition, in key fields such as medical care and military industry, 3D reconstruction technology has also become an indispensable and important part. 3D reconstruction technology provides people with a high degree of restoration of real scenes, achieving a more realistic, efficient and convenient experience.

[0035] In layman's terms, 3D reconstruction technology is to use machines to replace human eyes to observe the surrounding environment and save it as 3D data that is closer to the real world to meet people's various needs. The evaluation dimensions of 3D reconstruction need to be comprehensively considered from the aspects of reconstruction accuracy, authenticity and real-time. 3D reconstruction technology is inseparable from various sensors used to replace human eyes to perceive the world, including traditional cameras, global positioning systems, and recently increasingly popular laser radars. With the continuous change of sensors and various related technologies, most modern environmental perception systems are not limited to using only one sensor, but are combined into a joint sensor system based on the needs of multiple sensors. As a sensor with a long history of development, although the camera has many limitations compared to emerging sensors, it still becomes a frequent visitor in the joint sensor system due to its advantages such as fast and convenient, color information, low cost, and mature technology. With the popularity of intelligent driving, laser radar has gradually entered the public's field of vision. With its advantages such as high precision, strong environmental adaptability, and long perception distance, it has been applied to more and more technical fields. As the two mainstream sensors that can stand alone, the joint perception of laser radar and camera sensors has become an increasingly mainstream choice. Therefore, this paper combines laser radar and camera sensors as the sensor system used in this study. In addition, most of the current multi-sensor perception systems are complex in design and have many limitations. Therefore, the sensor system in this article will focus on lightweight and portability while ensuring accuracy.

[0036] Although multiple sensors can provide people with richer multi-dimensional information for perceiving the environment, before that, the problem of external parameter calibration of multi-sensor fusion must be solved first, that is, how to accurately align the data obtained by different sensors to ensure the accuracy and consistency of the information. In addition, when using cameras and lidar to solve the problem of three-dimensional reconstruction, it is also necessary to consider the registration problem between different frames of data from the same sensor. Although there are already a large number of research methods for these two key issues, there is still no absolutely mainstream or unified solution. In most cases, the two are regarded as two independent problems that cannot provide convenience for each other.

[0037] A comprehensive solution for both sensor calibration and 3D reconstruction registration problems can not only significantly improve the portability of operations, but also simplify some repetitive steps such as feature extraction.

[0038] In addition, in an embodiment of the present invention, based on the idea of ​​loop closure detection, the global point cloud obtained by inter-frame registration of multiple frames of data is used as a benchmark, and the point clouds of each frame are aligned to achieve global loop closure optimization of the point clouds of each frame; based on the inter-frame registration of the lidar point cloud, the ICP algorithm is used to obtain the three-dimensional point cloud scene reconstruction result; each frame of the point cloud is projected into the corresponding image frame, and bilinear interpolation is used to obtain the color information of each point cloud to complete the task of reconstructing the three-dimensional scene with colored point clouds.

[0039] refer to Figure 1 , Figure 1 is a schematic diagram of the overall framework of the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, such as Figure 1 Said method comprises the following aspects: In order to link the extrinsic calibration problem of LiDAR and camera with the joint 3D scene reconstruction problem, a calibration object system that can be used for both sensor calibration and 3D reconstruction registration is designed. It consists of a pyramid calibration object with checkerboard patterns on three sides and a 2D checkerboard calibration plate. The scan data only needs to be held by one person, and the extrinsic calibration scan can be converted to a 3D scene reconstruction scan by simply rotating the scanning system 90 degrees. In addition, sharing a calibration object system allows extrinsic calibration and 3D reconstruction to share some feature point extraction and registration methods, reducing the complexity of the algorithm.

[0040] In order to improve the accuracy and robustness of the calibration of the laser radar and camera extrinsic parameters, a variety of strategies and methods are used. In addition to using the intersection of four checkerboard planes as the feature points required for registration, the reprojection error of the three-dimensional edge to the image coordinate system is also introduced as an additional constraint. In the feature extraction stage, the RANSAC algorithm is used in combination with the least squares method based on the vertical distance to accurately fit the point cloud plane. The RANSAC algorithm is used to filter out reliable point cloud samples from the original data, and the least squares method based on the vertical distance fits these samples more accurately to obtain a more accurate plane equation. This method not only improves the accuracy of feature extraction, but also enhances its robustness, providing more accurate and stable features for subsequent registration. The feature registration process is divided into three stages. First, the coarse registration stage based on the least squares method provides an initial and roughly correct alignment relationship for the entire registration process. Then, the geometric constraint relationship between the three sides of the pyramid is used to optimize the feature plane fitting. Finally, the ICP registration algorithm is used to accurately solve the extrinsic parameter matrix of the laser radar and the camera.

[0041] In the joint 3D scene reconstruction of LiDAR and camera, we first introduce a global optimization method based on loop detection to address the cumulative error caused by the registration between adjacent frames and the random error of a single set of data. By using the global point cloud registered by multiple frames of data, we perform global loop optimization on each frame of point cloud. This not only improves the accuracy of calibration and reconstruction, but also ensures the consistency of the global scene. Finally, the point cloud is mapped to the image according to the external parameter matrix, the point cloud color information is obtained, and a high-precision colored 3D point cloud scene reconstruction result is obtained. This not only makes the reconstruction result more vivid and realistic, but also provides rich information for possible applications such as environmental perception and object recognition.

[0042] Optionally, the external parameter alignment and inter-frame loading method based on laser radar and camera in the embodiment of the present application can be executed by a server, or by a terminal device, or jointly by a server and a terminal device, taking the terminal executing the external parameter alignment and inter-frame loading method based on laser radar and camera in this embodiment as an example.

[0043] Figure 2 is a flow chart of the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, such as Figure 2 As shown, the method includes the following: Step 201, obtaining point cloud data collected by the laser radar and image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include calibration objects.

[0044] In the embodiment of the present invention, during the data scanning process using the laser radar and the camera, it is necessary to place the calibration object in the overlapping area of ​​the field of view of the laser radar and the camera, and ensure that the feature information in the data of the two can be easily extracted.

[0045] refer to Figure 3 , Figure 3 It is a schematic diagram of the external parameter calibration data scanning method provided by the present invention.

[0046] like Figure 3 As shown, the scanning system is rotated around the blue dotted line to obtain multiple sets of data for subsequent global optimization. This method is convenient for scanning and can segment the calibration object by the coordinate value of the point cloud for subsequent operations.

[0047] refer to Figure 4 , Figure 4 It is a schematic diagram of corresponding frame extraction and interpolation provided by the present invention.

[0048] The external calibration of LiDAR and camera aims to determine the spatial transformation relationship between the two sensor coordinate systems so that they can be correctly aligned to achieve multi-sensor perception of the environment. In the registration process, the point cloud and image to be registered must be scanned at the same time. The commonly used corresponding frame extraction method is time synchronization. However, since the LiDAR frame rate is not consistent with the camera, and the timestamps of most frames of the two cannot be completely matched, there are always more or less errors. You can refer to Figure 4 .

[0049] Since the LiDAR frame rate is lower than the camera frame rate, it is more accurate to find matching image frames through LiDAR point cloud frames. The method is divided into the following steps: (1) First, find the image frames closest to the timestamp of the lidar point cloud frame and calibrate the camera pose matrix of the two image frames through the checkerboard , and , To solve.

[0050] (2) The time interpolation factor is calculated by the time difference between two image frames to interpolate the rotation matrix and the translation matrix. The translation matrix interpolation can be easily obtained using linear interpolation. The formula is as follows: in and , They are the timestamps of the lidar point cloud frame and its two adjacent image frames. It is the linear interpolation result of the image frame translation matrix.

[0051] (3) For rotation matrix interpolation, Euler angle linear interpolation is a simple and convenient interpolation method. However, due to the inherent properties of Euler angles, using three angles to describe rotation will cause the gimbal lock problem in some cases, such as when the pitch angle is close to 90 degrees. Therefore, a more robust quaternion interpolation method is used. Quaternion interpolation is divided into quaternion linear interpolation and quaternion spherical interpolation. The use of quaternion spherical interpolation ensures the continuity of the interpolation result on the sphere, and can obtain a smoother interpolation result. The first step is to convert the rotation matrix of the two image frames into quaternion representation: in, is the real part (or scalar part) of the quaternion, is the imaginary part (or vector portion) of the quaternion.

[0052] in is a quaternion representation, is the trace of the rotation matrix, is the element of the rotation matrix. Then perform spherical interpolation on it: in is the quaternion representation of the rotation matrix of the two image frames, is the quaternion representation of the interpolated rotation matrix, is the interpolation factor, for and The angle of Convert to rotation matrix representation to get the rotation matrix of the image frame after interpolation .

[0053] Step 202, based on the random sampling consistency algorithm and the least square method based on the vertical distance, the point cloud data is fitted and optimized to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system.

[0054] According to the external parameter alignment and inter-frame loading method based on laser radar and camera provided by the present invention, the calibration object includes: three pyramids with checkerboard patterns on the sides and a two-dimensional checkerboard calibration plate.

[0055] According to the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, the point cloud data is fitted and optimized based on the random sampling consistency algorithm and the least square method based on the vertical distance, and the characteristic point coordinate values ​​of the calibration object in the laser radar coordinate system and the characteristic straight line equation of the calibration object in the laser radar coordinate system are obtained, including: The point cloud data of the calibration object is iteratively segmented based on the random sampling consistency algorithm to obtain the initial values ​​of the fitting plane equations of the four planes; The sum of the squares of the distances from the origin to the four planes is used as the objective function of the least squares method, and the initial values ​​of the fitted plane equations of the four planes are optimized to obtain the plane equation parameters of the four planes; Conditional filtering is performed based on the sum of the squares of the distances of all the internal points in the four planes in the height direction to determine the plane equation parameters of the two-dimensional chessboard calibration plate plane in the four planes; Based on the intersection operation of the plane equation parameters of the four planes and the plane equation parameters of the two-dimensional chessboard calibration plate plane, the characteristic point coordinate values ​​of the four intersection points of the four planes and the characteristic straight line equations of the three edges of the pyramid are determined.

[0056] refer to Figure 5 , Figure 5 It is a RANSAC single feature plane segmentation and fitting flow chart provided by the present invention.

[0057] The steps for iteratively splitting the four planes are as follows: Step 1: Randomly select three points from the calibration object point cloud to form a plane equation.

[0058] Step 2: Calculate the distance from all points in the point cloud to the plane and mark the distances less than the threshold. The points are points in the plane. The number of points in the plane is counted. If it is greater than the threshold , then set the plane as the pre-selected plane C and record the number of points n in the plane. Otherwise, return to step 1 and reselect three points.

[0059] Step three, randomly select three points from the interior points of the pre-selected plane C to form a new plane equation C', calculate and count the number of interior points n' of the new plane equation C', if the number of interior points n' of the new plane is greater than the number of interior points n of the pre-selected plane, then update the pre-selected plane to the new plane C', and update the set of interior points of the pre-selected plane.

[0060] Step 4: Repeat step 3 until the maximum number of iterations N is reached, save the final pre-selected plane and the corresponding points in the plane, and separate them from the calibration object point cloud.

[0061] Step 5: Repeat steps 1 to 4 until all four planes of the calibration object are fitted and segmented.

[0062] In the whole method, the parameters that need to be set are the maximum number of iterations N, the distance threshold of the points in the plane And the threshold of the number of interior points of the valid plane It should be noted that if the distance threshold of the plane point If it is set too small, the RANSAC algorithm will distinguish between inliers and outliers more strictly, and only when the distance between the point and the fitting plane is very close will it be considered an inlier.

[0063] Since the accuracy of the RANSAC algorithm is highly dependent on the setting of parameters, and its randomness will affect the stability of the algorithm, and it may be affected by noise, outliers, and local extreme points, causing the results to deviate from expectations. To this end, the present invention sets relatively conservative parameters for RANSAC, only segments the point clouds of the four planes, and roughly fits the initial values ​​of the plane equations, and then uses the least squares method to accurately fit and optimize the planes.

[0064] For the traditional ordinary least squares method based on z-axis distance residual, the general expression of the plane equation is: Ax+By+Cz+D=0 Where A, B, C are the components of the plane normal vector, and D is the offset of the plane, which can be transformed into: So, using all n point coordinates ( The x and y in the equation are used as independent variables, and the sum of squared errors between the estimated value and the actual value of z is used as the loss function of the least squares method. The parameters that minimize the loss function are solved. , and ,Right now: Use the least squares method to find the plane parameters that minimize the loss function , and After that, the expression of the plane equation can be obtained. However, the loss function of this method assumes that the x and y values ​​of the point are accurate, while the coordinates of the actual midpoint contain errors on all three coordinate axes, so the results of this method are not accurate and robust enough. In addition, using only the z value residual as the loss function will make the result easily affected by the plane angle. When the angle between the plane to be fitted and the xy plane is larger, the z value residual will also become larger overall, thus affecting the fitting accuracy.

[0065] Therefore, in the embodiment of the present invention, the residual sum of squares of the vertical distance between the point and the plane is used as the loss function, and the plane equation roughly fitted by RANSAC is optimized in combination with the unit normal vector constraint of the plane equation, and the method is as follows: For the plane equation, (A, B, C) represents the normal vector of the plane, (x, y, z) is the coordinates of a point in space, and D represents the distance from the plane to the origin. It can be expressed as: Furthermore, every plane equation can be written in terms of a unit normal vector, that is, Therefore, three parameters a, b, c are introduced to establish the relationship with the plane equation parameters A, B, C, D: Then the distance formula from point to plane can be expressed as: Distance The sum of squares is used as the objective function of the least squares method, and the coordinates of all n points are Substitute and solve the values ​​of parameters a, b, and c when the objective function is minimized, that is: The point cloud plane parameters obtained from the RANSAC algorithm are used as the initial parameters, and the least squares optimization is performed to obtain the parameters a, b, and c, and then the plane equation parameters A, B, C, and D are solved in reverse.

[0066] For the four plane equations obtained after fitting and optimization, since the angle between the two-dimensional checkerboard calibration plate plane and the ground is the smallest, conditional filtering is first used to separate the two-dimensional checkerboard calibration plate plane from the four planes according to the sum of the squares of the distances of all the inner points of each plane in the height direction. Then, the four intersection points of the four planes and the three edge equations of the pyramid are solved by plane intersection, thus completing the extraction of feature points and feature lines in the lidar point cloud.

[0067] Step 203: Determine the coordinate values ​​of the feature points of the calibration object in the image data in the camera coordinate system according to the checkerboard corner point detection algorithm based on seed growth.

[0068] To extract the checkerboard plane in the image, we first need to detect the black and white checkerboard corners in the image. The steps of the traditional checkerboard corner detection algorithm are as follows: Firstly, the image is converted into black and white binary using the average adaptive thresholding method to highlight the rectangular structure of the checkerboard.

[0069] The white pixels in the binary image are expanded to separate the black grids, which is beneficial for the subsequent corner point position estimation.

[0070] Detect all black quadrilateral regions from the binary image and remove interfering quadrilaterals based on geometric constraints.

[0071] For each quadrilateral, if there are four adjacent quadrilaterals around it, it is regarded as a quadrilateral inside the chessboard. If there are only two adjacent quadrilaterals around it, it is regarded as a quadrilateral at the four corners of the chessboard. If it has no adjacent quadrilaterals, it is regarded as an interference item and is removed.

[0072] According to the known length and width of the chessboard, the quadrilaterals are sorted to obtain their positions, that is, the number of rows and columns, and the number of quadrilaterals is checked to see if it meets the known number. If so, the midpoint of the two nearest vertices of the two adjacent quadrilaterals is calculated as the corner point and optimized. Otherwise, the detection fails.

[0073] Although this chessboard corner point detection method is simple to implement and fast, it has poor stability and environmental adaptability. The number of chessboard rows and columns needs to be given in advance, and detection is prone to failure when the chessboard is tilted at a large angle.

[0074] The checkerboard corner point detection algorithm based on seed growth can be applied to the case of detecting multiple checkerboards at one time. The method steps of the embodiment of the present invention are as follows: Step 1: Roughly extract the corner points by using Gaussian filtering and non-maximum suppression method.

[0075] First, for the pixel-level features of the checkerboard corners, three different scales are used The Gaussian filter kernel is constructed with the value to blur the image and highlight the features of the corner points of the chessboard.

[0076] Then the grayscale image is convolved using a Gaussian filter kernel. Corresponding to the four result images, the pixels of the four result images are averaged to obtain the average image of the result images. Since in the average image, the value of the flat area after convolution is basically unchanged, while in the corner or edge area, using different convolution kernels will result in different values ​​in the edge direction, the difference image obtained by calculating the difference between the four result images and their average image can highlight the approximate position range of the corner points.

[0077] Finally, the non-maximum suppression algorithm is used to find the local maximum in the local area of ​​each corner point, and then the neighborhood of the local maximum is checked to see if there is a value greater than it. If so, it is marked as a failure. At the same time, the boundary detection condition must be set so that the maximum value is not a local maximum on the boundary. If the local maximum value is not marked as a failure and the value is greater than the threshold set in advance, the position of the local maximum value is marked as a candidate corner point position.

[0078] Step 2: Sub-pixel corner detection optimization.

[0079] refer to Figure 6 , Figure 6 It is a schematic diagram of the principle of sub-pixel corner point detection provided by the present invention.

[0080] like Figure 6 As shown in the figure, the camera and lidar external parameter calibration requires high accuracy, so it is necessary to optimize the sub-pixel detection of pixel-level corner points. For the pixel points on the black and white chessboard. First, let point c be the precise corner point in the ideal case. is the pixel-level corner point obtained in the previous step in the actual experiment. For the pixel in the neighborhood of the ideal precise corner point c, let its neighborhood point set ), where points located in the flat area of ​​the black and white grid are such as Gradient is 0, so the vector and Gradient The dot product of must be 0. And the points at the intersection of black and white grids are and , their gradient vector and are perpendicular to the boundary line, so their gradient vectors are the same as the vectors and The dot product is also 0. In general, for an ideal corner point c, the gradient vectors of all points in its neighborhood are With vector The dot product is 0, that is: In actual situations, the corner points with errors , the gradient of all points in the flat area inside the black and white grid in its neighborhood is still 0. However, it is different from the ideal corner point c in that for the point located at the junction of the black and white grid and , since its gradient vector , and , Not perpendicular, so the dot product is not 0, that is: Therefore, in order to convert the actual pixel-level corner points Approaching the ideal corner point c, choose to use Neighborhood point set ), the sum of the squares of the vector dot products of all points in the point set is used as the loss function, and the least squares method is used to Perform iterative optimization, namely: Step 3: Seed growth at the corner points of the chessboard.

[0081] For the seed corner point, first extract the nine corner points in its neighborhood as the initial 2*2 chessboard, and then gradually grow from the edge to the surrounding areas. When growing, the third corner point is predicted by two adjacent corner points, and the distance between the predicted point and the nearest corner point is used as an important basis for judgment. The score is as follows: in is based on and Predicted growth corners, It is the distance If the score is greater than the set threshold, the growth is successful, and the score is added to the overall score of the chessboard for subsequent comparison of overlapping chessboards.

[0082] After detecting the complete chessboard, the edge corners of the chessboard are marked so that the edge corners are avoided when selecting the seed corners, which can reduce the number of iterations and thus the amount of calculation. In addition, in the actual chessboard growth, two chessboards may be stuck together. Since the number and size of the chessboards in the image are known, the chessboards with sticky phenomenon can be identified by detecting the number and size of the chessboards, and then separated according to the known chessboard size.

[0083] After obtaining the pixel coordinates of the corner points of the chessboard, the direct linear transformation (DLT) algorithm is used to solve the pose transformation relationship between each chessboard coordinate system and the camera coordinate system. For each chessboard, its own coordinate system is set to , is the chessboard grid number, the chessboard grid plane position The plane is 0=Z plane, the origin Located at the top left corner of the chessboard.

[0084] The same point in pixel coordinates With the checkerboard coordinate system The coordinate transformation relationship between them is as follows: The 3*4 matrix is is the camera’s intrinsic parameter matrix, and It is the rotation matrix and translation matrix between the chessboard coordinate system and the camera coordinate system.

[0085] Since the size of the chessboard is known, according to the definition of the chessboard coordinate system, the three-dimensional coordinates of all corner points on the chessboard in the chessboard coordinate system can be obtained. , the z values ​​of these three-dimensional coordinates are all 0. Then, combining the above formula, we can get the coordinate transformation relationship between the pixel coordinate system and the chessboard coordinate system of the chessboard corner point: Right now: in and , respectively represent the coordinates of the chessboard corner points in the chessboard coordinate system and the camera coordinate system, M is the camera's intrinsic parameter matrix, , , is the rotation matrix between the chessboard coordinate system and the camera coordinate system The weight, is the translation matrix between the chessboard coordinate system and the camera coordinate system, is the scale factor.

[0086] Define the homography matrix H: but: Substituting each set of coordinates of the corner points into the equation: By substituting the coordinates of all corner points on the chessboard, we can obtain the homography matrix H and the rotation matrix The two components of and , combined with the inherent constraint properties of the rotation vector, refer to the following formula, and you can decompose the rotation matrix between the checkerboard coordinate system and the camera 3D coordinate system from H and translation matrix .

[0087] After obtaining the pose transformation relationship between the four chessboard plane coordinate systems and the camera's three-dimensional coordinate system, we can easily get the plane functions of the four chessboard planes in the camera's three-dimensional coordinate system, and then obtain the coordinate values ​​of the intersection of the four chessboard planes, that is, the feature points in the camera coordinate system.

[0088] Step 204 , performing color segmentation and straight line fitting on the image data to determine the characteristic straight line equation of the calibration object in the camera coordinate system.

[0089] Set the three edges of the pyramid to red, and extract the equations of the three edges in the image by color segmentation and straight line fitting. The specific steps are as follows: Color segmentation: Convert the image's color space from RGB to HSV, and extract the red area in the image through conditional filtering, leaving the rest of the area black.

[0090] Use the RANSAC algorithm to extract the equations of three edges. First, randomly select two red pixels and fit them into a straight line equation. Determine whether the number of red pixels passing through the straight line equation is greater than the threshold n. If it is greater, update the straight line to the pre-selected straight line, and mark all points whose distance from the straight line is less than the threshold d as inner pixels. Then randomly select two pixels from the inner pixels of the pre-selected straight line to fit a new straight line equation. If the inner pixel of the new straight line equation is greater than the pre-selected straight line, update it to the pre-selected straight line, otherwise continue to iterate to the specified number of times. Finally, save the fitted straight line equation and its inner pixel points, and set all the inner pixel points of the straight line to black. Then use the same method to extract the other two straight lines to complete the extraction of the pyramid feature edges in the image.

[0091] Step 205 , determining an error function of the intersection matching constraint based on the coordinate values ​​of the feature points of the calibration object in the laser radar coordinate system and the coordinate values ​​of the feature points of the calibration object in the camera coordinate system.

[0092] refer to Figure 7 , Figure 7 It is a schematic diagram of the laser radar coordinate system, camera coordinate system and chessboard coordinate system provided by the present invention.

[0093] The coordinate transformation relationship of the same point in the laser radar and camera coordinate systems is as follows: in and Indicates the coordinates of the point in the camera and lidar coordinate systems. and These are the rotation matrix and translation matrix between the laser radar and camera coordinate systems that need to be solved, that is, the external parameters.

[0094] Use point set and Represent the intersection points of the four checkerboard planes in the camera and lidar coordinate systems. Through the geometric properties of the calibration object, we can easily find two sets of corresponding points, which are the apex of the pyramid and the point farthest from the two-dimensional checkerboard calibration plate. Assume that these two sets of corresponding points are as well as , then there are only two possibilities for the correspondence between the other two points in the two point sets: or When the correspondence between two point sets C and all points in P is known, the least squares method can be used to solve the external parameters. The steps are as follows: (1) Calculate the centroid of two point sets C and L: in, represents the centroid of the point set L, represents the centroid of the point set C, represents the intersection of four checkerboard planes in the camera coordinate system, Represents the intersection of four checkerboard planes in the lidar coordinate system.

[0095] (2) Centralization of point set. Subtract the centroid of the point set from the points in the point set to obtain the decentralized point set. and : (3) For the matrix Using singular value decomposition (SVD), the initial rotation matrix can be solved , and then solve the translation matrix .

[0096] (4) Distance residual calculation. Calculation point set All points in and The residual sum of squares of the distance between the corresponding points in the point set C after transformation: (5) By calculating the correspondence between two possible point sets and , the distance residual sum of squares under the two point set correspondences are compared, where the set of data with a lower distance residual sum of squares is the correct point set correspondence, and its corresponding rotation and translation matrix and are the initial external parameter estimates.

[0097] The laser radar point cloud data has different degrees of discreteness in the parallel and perpendicular directions of the laser beam, and the scanning direction of the calibration object plane is different, so the number of points in the planes of several planes will be different, resulting in different fitting errors of each plane. Therefore, the geometric constraint relationship of the three sides of the pyramid being perpendicular to each other is used to optimize the three point cloud plane equations of the laser radar.

[0098] refer to Figure 8 , Figure 8 It is a schematic diagram of plane error sorting provided by the present invention.

[0099] The main optimization idea is to correct the plane with the largest error by using the two planes with smaller errors. Therefore, we first need to sort the error sizes of the planes, refer to Figure 8 The error of the plane is judged by the absolute value of the angle difference of the three edges of the pyramid after mapping by the initial external parameters.

[0100] The specific steps for plane correction are as follows: (1) Determine the plane error ranking. First, the three pyramid edges in the three-dimensional point cloud space are converted into the two-dimensional image space using the initial values ​​of the external parameters of the lidar and camera and the camera internal parameters obtained in the previous step. For example, Figure 7 In the figure, OA', OB', and OC' are three edges obtained after mapping. The plane errors are ranked by comparing the sizes of the three error angles: ,but and The coplanar OAC is considered to be the plane with the smallest error, so the error order of the three planes is .

[0101] (2) Correct the parameters of the plane with the largest error by using the two planes with smaller errors. Represents three planes with errors from small to large , , The unit normal vector of . According to the geometric relationship between the perpendicular pyramid planes, use Perform cross product to get Corrected unit normal vector .

[0102] (3) Repeat steps (1) and (2) to modify the parameters of the three planes so that the sum of the squares of the inner products of the unit normal vectors of the three planes is 0.

[0103] (4) Based on the corrected three plane equations, recalculate the optimized feature points and feature edges.

[0104] refer to Fig. 9 , Fig. 9 It is a schematic diagram of loop detection provided by the present invention.

[0105] In point cloud ICP registration, a small number of points may lead to the uniqueness problem of the registration result, that is, there are multiple different transformation matrices that can align two point clouds within a given error range. Therefore, the four feature points are expanded to ten feature points. The expansion method is as follows: Fig. 9 , select equidistant points on the extension lines of the three edges of the pyramid.

[0106] The ICP registration algorithm mainly uses the idea of ​​iterative optimization, taking the sum of the squares of the distances between the two sets of feature points as the error function, and continuously optimizing the rigid posture transformation between the two point sets by adjusting the correspondence between the points in the two point sets to achieve the maximum matching result. In order to reduce the impact of the accumulated errors in the previous steps and improve the registration accuracy of the algorithm, especially the accuracy of the rotation matrix in the external parameter matrix, the reprojection angle error constraint of the ridges of the intersecting planes of the pyramid point cloud to the image coordinate system is also introduced. The specific registration steps are as follows: The error function is established. The error function includes two constraints: the feature plane intersection matching constraint and the pyramid three edge reprojection constraint.

[0107] Among them, the feature plane intersection matching constraints include: The feature point clouds in the laser radar and camera coordinate systems are defined as , , first find the laser radar feature point cloud Each point in In the camera feature point cloud The nearest point in is taken as the matching point. Then the external parameter estimation matrix between the matching points is calculated by the formula and The residual sum of squares of the distance after mapping is used as the error function of this part: Step 206 , determining the error function of the reprojection constraint based on the characteristic straight line equation of the calibration object in the laser radar coordinate system and the characteristic straight line equation of the calibration object in the camera coordinate system.

[0108] Among them, the reprojection constraints of the three edges of the pyramid include: Through feature extraction, the straight line function of the three edges of the pyramid in the laser radar coordinate system and the straight line parameters in the two-dimensional image are obtained. The edge equation in the laser radar coordinate system is projected into the two-dimensional image, and its slope is recorded as , the slope of the edge line in the two-dimensional image is recorded as , the sum of the squares of the differences in the slopes of the two is taken as the error function of the constraint, where j is the sequence number of the three edges: Finally, the two error functions are weighted to obtain the final total error function: Step 207, determining the external parameter registration result of the laser radar and the camera by iterating the closest point registration algorithm based on the error function of the intersection matching constraint and the error function of the reprojection constraint.

[0109] By focusing on the two points in the above content and The nearest neighbor point is adjusted continuously to iteratively optimize the rotation matrix between the two sensor coordinate systems. and translation matrix , until the target number of iterations is reached or the error L reaches the preset threshold. Finally, the optimized high-precision external parameter matrix T is obtained: Since the results of a single set of registrations are unstable and limited, and the lidar point cloud frames and the corresponding camera image frames are matched by timestamp interpolation, in order to improve the robustness of the registration, multiple sets of lidar and camera corresponding frames are used to solve the global external parameters. The basic idea of ​​this method is as follows: First, based on the multi-frame point cloud registration and multi-frame image data registration, the global lidar feature point cloud and the global image feature point cloud are solved respectively. Then, the two global feature point clouds are registered using the ICP algorithm to obtain a globally stable registration result. However, it should be noted that when solving the global feature point cloud, since it is based on the inter-frame registration of all adjacent frames, cumulative errors will occur, affecting the accuracy of the global feature point cloud. In this regard, the idea of ​​loop detection is introduced, and all single-frame point clouds are loop-optimized based on the global feature point cloud to reduce the cumulative error. Finally, the lidar and camera global feature point clouds after loop optimization are registered to solve accurate and robust global external parameters. The entire global extrinsic parameter matrix solution method can be mainly divided into three steps: global feature point cloud solution, frame point cloud loop optimization and global extrinsic parameter matrix solution.

[0110] According to the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, after determining the external parameter registration result of laser radar and camera based on the error function of intersection matching constraint and the error function of reprojection constraint by iterative closest point registration algorithm, the above method further includes: Perform weighted summation on the error function of the intersection matching constraint and the error function of the reprojection constraint to obtain the total error function; Iteratively perform the closest point registration on the point cloud data based on the total error function to obtain the spatial variation matrix between all adjacent frames; The spatial variation matrix between all adjacent frames is aligned with the spatial variation matrix of the first frame to obtain the global feature point cloud of the lidar.

[0111] refer to Fig.10 , Fig.10 This is a schematic diagram of the global point cloud solution provided by the present invention.

[0112] Whether it is a lidar or a camera, by registering adjacent frames, all frames are aligned to the first frame to obtain a global point cloud. Fig.10 Since the frame rate of the lidar is 10 frames per second, and the camera data is the corresponding camera frame obtained by interpolating the timestamp of the lidar point cloud frame, the spatial transformation between adjacent frames of the two is very small. Therefore, when the ICP algorithm is used to align adjacent frames, there is no need to calculate the initial value, but to solve it directly.

[0113] In the process of solving the global feature point cloud of LiDAR, For the i-th frame of the LiDAR point cloud, the error function performs ICP registration on all adjacent frames, thereby solving the spatial transformation matrix between all adjacent frames. Align all to the first frame through the space transformation matrix , get the global point cloud of the lidar . Similarly, solve the global point cloud of the camera.

[0114] According to the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, after aligning the spatial variation matrix between all adjacent frames with the spatial variation matrix of the first frame to obtain the global feature point cloud of the laser radar, the method further includes: Based on the global feature point cloud, each frame of the point cloud in the point cloud data is registered and aligned to perform loop closure optimization on each frame of the point cloud.

[0115] In the process of solving the global point cloud in the previous step, since the spatial transformation matrix solved between each two adjacent frames inevitably contains a certain error, The further away the frame is, the more it will align to When the error of the spatial transformation matrix of all previous adjacent frames is accumulated, the error becomes larger and larger. In order to reduce the impact of the accumulated error on the distant frames, the idea of ​​loop detection is adopted.

[0116] Specifically, since the global point cloud is obtained by solving all frame point clouds together, it reflects the overall characteristics of all frame point cloud data. Therefore, the global point cloud is used as a benchmark to optimize the registration of each frame point cloud. This effectively reduces the cumulative error and improves the accuracy and stability of subsequent overall registration.

[0117] refer to Fig.11 , Fig.11 It is a schematic diagram of loop optimization provided by the present invention.

[0118] refer to Fig.12 , Fig.12 It is a schematic diagram of visualizing the calibration object point cloud and plane segmentation effect provided by the present invention.

[0119] like Fig.11 As shown in the figure, a is the ideal case of loop optimization, and b is arrive The effect of cumulative error is shown. The idea of ​​loop closure detection is to replace adjacent frames with more similar historical frames when calculating the pose transformation, so as to reduce the cumulative error. Fig.12 As shown in FIG. 1 , by registering the solved global point cloud with each frame point cloud, the purpose of optimizing the cumulative error is achieved.

[0120] According to the external parameter registration and inter-frame loading method based on laser radar and camera provided by the present invention, after the point clouds of each frame in the point cloud data are registered and aligned based on the global feature point cloud to perform loop optimization on each frame point cloud, the above method also includes: solving the global external parameter matrix.

[0121] When solving the global external parameter matrix, the ICP algorithm is used to accurately align the lidar and camera global point clouds after global loop closure optimization. In order to ensure that the ICP algorithm can have a more accurate starting point, the alignment result between the first frame lidar feature point cloud and the camera feature point cloud of the corresponding timestamp is specially selected as the initial value, which provides strong support for subsequent iterative optimization.

[0122] Specifically, we first use the initial external parameter estimation method based on the least squares method to estimate the first frame point of the lidar. The first frame point cloud of the camera with the corresponding timestamp Perform registration and obtain the initial value of the external parameter. Then use it to form the global point cloud of the laser radar and camera and The feature points of all frames constitute the global feature point cloud, denoted as and , the ICP algorithm is used to perform global registration on the two. The specific registration steps are as follows: Step 1: First find the global feature point cloud of the LiDAR Each point in In the camera global feature point cloud The nearest neighbor point in as a corresponding point.

[0123] Step 2: Since the global feature point cloud is composed of n frames of feature point clouds, each frame contains ten feature points, the global feature point clouds of the laser radar and the camera contain 10n points respectively. The correspondence between these 10n points has been selected in (1). Substitute these 10 sets of corresponding points into the loss function of the formula, use the least squares method to align the two global feature point clouds, and solve the rotation and translation matrices that minimize the error function E. and As the external parameter matrix result of this iteration.

[0124] Step 3: Substitute the result obtained in step 2 into and The global feature point cloud of the LiDAR before this iteration , so that it is consistent with the camera global feature point cloud to align.

[0125] Step 4: Repeat (1), (2), (3) until the preset threshold or the maximum number of iterations is reached to obtain the final global external parameter matrix and .

[0126] Since lidar point clouds have the advantages of high precision and are not easily affected by the environment, the lidar point cloud data is mainly used to reconstruct the surrounding scenes. Then, for each frame of lidar point cloud, its corresponding camera image frame is found, and the previously solved external parameters of the lidar and camera are used to project all points into the image to obtain the color information of the points, thereby completing the high-precision lidar and camera colored point cloud scene reconstruction task.

[0127] For the data scanning in the joint scene reconstruction of LiDAR and camera, a scanning method similar to that of the calibration module is adopted, that is, the LiDAR and camera are located directly above the calibration object system. The difference is that in the calibration task, the camera is always facing downward to ensure that the image has a complete calibration object system, while in the reconstruction task, the camera faces sideways to provide color information of the surrounding scene for reconstruction. The specific steps include the following: Step 1: LiDAR multi-frame point cloud registration.

[0128] For the multi-frame point cloud of the LiDAR, the point cloud feature extraction method is used. The four feature plane point clouds are segmented and roughly fitted by using the random sampling consensus (RANSAC) algorithm. Then, the point cloud plane is accurately fitted by the least squares method based on the vertical distance residual and the unit normal vector constraint of the plane equation to obtain the equations of the four feature planes. The four intersection points of the feature planes and the six extension points on the three edges of the pyramid are used as feature points to align each adjacent point cloud frame. The ICP algorithm is used for alignment. The specific principle is the same as the global point cloud solution of the LiDAR. After the alignment is completed, all frames are aligned to the first frame to obtain the global point cloud. After that, the global loop optimization method of each frame point cloud is used to optimize the loop of each frame point cloud with the help of the global point cloud to reduce the impact of the cumulative error caused by the alignment between adjacent frames.

[0129] Step 2: solve the point cloud color.

[0130] For each frame of LiDAR point cloud, first find the camera image frame with the corresponding timestamp, and then solve the coordinates of all points in the LiDAR point cloud in the corresponding image frame after mapping according to the formula.

[0131] in is the coordinate of the point in the laser radar point cloud, (u,v) is the coordinate of the point in the image, M is the camera intrinsic parameter matrix, and To solve the external parameters of the globally optimized lidar and camera, that is, the rotation matrix and translation matrix between the two sensor coordinate systems.

[0132] Since the required (u,v) coordinates are not integers, interpolation is required to solve the color information of the point. By performing linear interpolation in two directions on the color values ​​of the RGB three channels, the color values ​​of the non-integer coordinates (u,v) are obtained, thereby obtaining the color information of the three-dimensional point cloud. The specific steps are as follows: First, label the four integer coordinates closest to (u,v) as , , , , perform linear interpolation in the u direction: Then perform linear interpolation in the v direction: Where C represents the color value, is the final bilinear interpolation result.

[0133] Step 3: Downsample the colored point cloud 3D scene reconstruction result.

[0134] Considering that the data contains a large number of LiDAR point cloud frames when scanning and reconstructing the surrounding scene, and the number of points in each point cloud frame is more than 60,000, the final color point cloud scene reconstruction result contains millions of points, and the reconstruction result is too dense and redundant. In order to reduce the number of point clouds without affecting the reconstruction quality of the scene too much, point cloud voxel filtering is selected as the point cloud downsampling method. This method retains the characteristics and shape of the point cloud while having a high computing speed. At an appropriate voxel size, it can achieve high-quality point cloud downsampling effects.

[0135] First, according to the maximum and minimum values ​​of all points in the point cloud in the three-axis directions, the point cloud space is divided into voxel grids of fixed size. For each voxel grid, the centroid of all points in the voxel grid is calculated using formula (4-5) as the representative of the midpoint of the voxel grid.

[0136] in represents the centroid of all points in the voxel grid with coordinates (i,j,k), represents the coordinates of each point in the voxel grid, is the total number of points in the voxel grid.

[0137] Finally, all points except the centroid of the voxel are deleted to complete the downsampling of the reconstruction result of the color point cloud scene.

[0138] The external parameter alignment and inter-frame loading device based on laser radar and camera provided by the present invention is described below. The external parameter alignment and inter-frame loading device based on laser radar and camera described below and the external parameter alignment and inter-frame loading method based on laser radar and camera described above can be referenced to each other.

[0139] refer to Fig.13 , Fig.13 It is a structural schematic diagram of the external parameter alignment and inter-frame loading device based on laser radar and camera provided by the present invention.

[0140] An acquisition module 1301 is used to acquire point cloud data collected by the laser radar and image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include calibration objects; The fitting and optimization module 1302 is used to fit and optimize the point cloud data based on the random sampling consistency algorithm and the least square method based on the vertical distance, so as to obtain the coordinate values ​​of the feature points of the calibration object in the laser radar coordinate system and the characteristic straight line equation of the calibration object in the laser radar coordinate system; A first determination module 1303 is used to determine the coordinate values ​​of feature points of the calibration object in the image data in the camera coordinate system according to a checkerboard corner point detection algorithm based on seed growth; The second determination module 1304 is used to perform color segmentation and straight line fitting on the image data to determine the characteristic straight line equation of the calibration object in the camera coordinate system; A matching constraint module 1305 is used to determine an error function of an intersection matching constraint based on the coordinate values ​​of the feature points of the calibration object in the laser radar coordinate system and the coordinate values ​​of the feature points of the calibration object in the camera coordinate system; A projection constraint module 1306 is used to determine an error function of a reprojection constraint based on a characteristic straight line equation of the calibration object in the laser radar coordinate system and a characteristic straight line equation of the calibration object in the camera coordinate system; The external parameter configuration module 1307 is used to determine the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative closest point registration algorithm.

[0141] Specifically, the above-mentioned external parameter alignment and inter-frame loading device based on laser radar and camera provided by the present invention can implement all the method steps implemented by the above-mentioned external parameter alignment and inter-frame loading method embodiment based on laser radar and camera, and can achieve the same technical effect. The parts and beneficial effects of this embodiment that are the same as the method embodiment will not be described in detail here.

[0142] Fig.14 is a schematic diagram of the physical structure of the electronic device provided by the present invention, such as Fig.14 As shown, the electronic device may include: a processor 1410, a communications interface 1420, a memory 1430 and a communication bus 1440, wherein the processor 1410, the communications interface 1420 and the memory 1430 communicate with each other through the communication bus 1440. The processor 1410 may call the logic instructions in the memory 1430 to execute the external parameter registration and inter-frame loading method based on the laser radar and the camera, the method comprising: obtaining the point cloud data collected by the laser radar and the image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include calibration objects; fitting and optimizing the point cloud data based on the random sampling consistency algorithm and the least squares method based on the vertical distance, and obtaining the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system; determining the image corner point detection algorithm based on the seed growth. The feature point coordinates of the calibration object in the image data in the camera coordinate system; perform color segmentation and straight line fitting on the image data to determine the feature line equation of the calibration object in the camera coordinate system; determine the error function of the intersection matching constraint based on the feature point coordinates of the calibration object in the lidar coordinate system and the feature point coordinates of the calibration object in the camera coordinate system; determine the error function of the reprojection constraint based on the feature line equation of the calibration object in the lidar coordinate system and the feature line equation of the calibration object in the camera coordinate system; determine the external parameter registration result of the lidar and camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through the iterative closest point registration algorithm.

[0143] In addition, the logic instructions in the above-mentioned memory 1430 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a disk or an optical disk.

[0144] On the other hand, the present invention also provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the external parameter alignment and inter-frame loading method based on laser radar and camera provided by the above methods. The method includes: obtaining point cloud data collected by the laser radar and image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include calibration objects; based on a random sampling consistency algorithm and a least squares method based on vertical distance, fitting and optimizing the point cloud data, and obtaining the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the calibration object in the laser radar coordinate system. system; determine the coordinate values ​​of the feature points of the calibration object in the image data in the camera coordinate system according to the checkerboard corner detection algorithm based on seed growth; perform color segmentation and straight line fitting on the image data to determine the characteristic straight line equation of the calibration object in the camera coordinate system; determine the error function of the intersection matching constraint based on the coordinate values ​​of the feature points of the calibration object in the lidar coordinate system and the coordinate values ​​of the feature points of the calibration object in the camera coordinate system; determine the error function of the reprojection constraint based on the characteristic straight line equation of the calibration object in the lidar coordinate system and the characteristic straight line equation of the calibration object in the camera coordinate system; determine the external parameter registration result of the lidar and camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through the iterative closest point registration algorithm.

[0145] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the external parameter alignment and inter-frame loading method based on the laser radar and the camera provided by the above-mentioned methods, the method comprising: acquiring point cloud data collected by the laser radar and image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include calibration objects; fitting and optimizing the point cloud data based on a random sampling consistency algorithm and a least squares method based on vertical distance to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system; and calculating the feature point coordinate values ​​of the calibration object in the laser radar coordinate system based on a least squares method based on a random sampling consistency algorithm and a least squares method based on a vertical distance. The seed-grown checkerboard corner detection algorithm determines the coordinate values ​​of the feature points of the calibration object in the image data in the camera coordinate system; performs color segmentation and straight line fitting on the image data to determine the characteristic straight line equation of the calibration object in the camera coordinate system; based on the coordinate values ​​of the feature points of the calibration object in the lidar coordinate system and the coordinate values ​​of the feature points of the calibration object in the camera coordinate system, the error function of the intersection matching constraint is determined; based on the characteristic straight line equation of the calibration object in the lidar coordinate system and the characteristic straight line equation of the calibration object in the camera coordinate system, the error function of the reprojection constraint is determined; the external parameter registration result of the lidar and camera is determined through the iterative closest point registration algorithm based on the error function of the intersection matching constraint and the error function of the reprojection constraint.

[0146] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0147] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0148] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for external parameter registration and inter-frame loading based on laser radar and camera, characterized in that: The laser radar and the camera have the same calibration object, and the method includes: Acquire the point cloud data collected by the laser radar and the image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include the calibration object; Based on the random sampling consistency algorithm and the least square method based on the vertical distance, the point cloud data is fitted and optimized to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system; Determine the coordinate values ​​of the feature points of the calibration object in the image data in the camera coordinate system according to a checkerboard corner point detection algorithm based on seed growth; Performing color segmentation and straight line fitting on the image data to determine a characteristic straight line equation of the calibration object in the camera coordinate system; Determine an error function of an intersection matching constraint based on the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature point coordinate values ​​of the calibration object in the camera coordinate system; Determine an error function of a reprojection constraint based on a characteristic straight line equation of the calibration object in the laser radar coordinate system and a characteristic straight line equation of the calibration object in the camera coordinate system; The external parameter registration result of the laser radar and the camera is determined by an iterative closest point registration algorithm based on the error function of the intersection matching constraint and the error function of the reprojection constraint.

2. The method for external parameter registration and inter-frame loading based on laser radar and camera according to claim 1, characterized in that: The calibration object includes: three pyramids with checkerboard patterns on the sides and a two-dimensional checkerboard calibration plate.

3. The method for external parameter registration and inter-frame loading based on laser radar and camera according to claim 2 is characterized in that: The point cloud data is fitted and optimized based on the random sampling consistency algorithm and the least square method based on the vertical distance to obtain the feature point coordinate values ​​of the calibration object in the laser radar coordinate system and the feature straight line equation of the calibration object in the laser radar coordinate system, including: Iteratively segmenting the point cloud data of the calibration object based on a random sampling consistency algorithm to obtain initial values ​​of fitting plane equations of four planes; The sum of the squares of the distances from the origin to the four planes is used as the objective function of the least squares method, and the initial values ​​of the fitted plane equations of the four planes are optimized to obtain the plane equation parameters of the four planes; Conditional filtering is performed based on the sum of the squares of the distances of all the inner points of the four planes in the height direction to determine the plane equation parameters of the two-dimensional chessboard calibration plate plane in the four planes; Based on the intersection operation of the plane equation parameters of the four planes and the plane equation parameters of the two-dimensional chessboard calibration plate plane, the feature point coordinate values ​​of the four intersection points of the four planes and the feature straight line equations of the three edges of the pyramid are determined.

4. The method for external parameter registration and inter-frame loading based on laser radar and camera according to claim 1, characterized in that: After determining the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint by using the iterative closest point registration algorithm, the method further includes: Performing a weighted summation on the error function of the intersection matching constraint and the error function of the reprojection constraint to obtain a total error function; Performing iterative closest point registration on the point cloud data based on the total error function to obtain a spatial variation matrix between all adjacent frames; The spatial variation matrix between all adjacent frames is aligned with the spatial variation matrix of the first frame to obtain a global feature point cloud of the laser radar.

5. The method for external parameter registration and inter-frame loading based on laser radar and camera according to claim 4 is characterized in that: After aligning the spatial variation matrix between all adjacent frames with the spatial variation matrix of the first frame to obtain the global feature point cloud of the laser radar, the method further includes: Based on the global feature point cloud, each frame point cloud in the point cloud data is registered and aligned to perform loop closure optimization on each frame point cloud.

6. The method for external parameter registration and inter-frame loading based on laser radar and camera according to claim 1, characterized in that: After determining the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint by using the iterative closest point registration algorithm, the method further includes: The point cloud data is mapped to the image data based on the external parameter registration result to obtain a colored three-dimensional point cloud scene reconstruction result.

7. An external parameter registration and inter-frame loading device based on laser radar and camera, characterized in that: The laser radar and the camera have the same calibration object, and the device includes: an acquisition module, used to acquire the point cloud data collected by the laser radar and the image data collected by the camera, wherein the point cloud data and the image data are scanned at the same time, and both the point cloud data and the image data include the calibration object; A fitting and optimization module, used to fit and optimize the point cloud data based on a random sampling consistency algorithm and a least square method based on vertical distance, to obtain the coordinate values ​​of the feature points of the calibration object in the laser radar coordinate system and the characteristic straight line equation of the calibration object in the laser radar coordinate system; A first determination module is used to determine the coordinate values ​​of the feature points of the calibration object in the image data in the camera coordinate system according to a checkerboard corner point detection algorithm based on seed growth; A second determination module is used to perform color segmentation and straight line fitting on the image data to determine a characteristic straight line equation of the calibration object in the camera coordinate system; A matching constraint module, used to determine an error function of an intersection matching constraint based on the coordinate values ​​of the feature points of the calibration object in the laser radar coordinate system and the coordinate values ​​of the feature points of the calibration object in the camera coordinate system; A projection constraint module, used to determine an error function of a reprojection constraint based on a characteristic straight line equation of the calibration object in the laser radar coordinate system and a characteristic straight line equation of the calibration object in the camera coordinate system; The external parameter configuration module is used to determine the external parameter registration result of the laser radar and the camera based on the error function of the intersection matching constraint and the error function of the reprojection constraint through an iterative closest point registration algorithm.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the external parameter alignment and inter-frame loading method based on laser radar and camera is implemented as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the external parameter alignment and inter-frame loading method based on laser radar and camera as described in any one of claims 1 to 6 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the external parameter registration and inter-frame loading method based on laser radar and camera as described in any one of claims 1 to 6 is implemented.

Images