Method for registering external parameters of lidar and camera and interframe loading method

Abstract

The application provides a laser radar and camera-based external parameter registration and interframe loading method, which comprises the following steps: acquiring point cloud data collected by a laser radar and image data collected by a camera, fitting and optimizing the point cloud data to obtain feature point coordinate values of a calibration object in a laser radar coordinate system and a feature straight line equation of the calibration object in the laser radar coordinate system; determining feature point coordinate values of the calibration object in a camera coordinate system in the image data; determining a feature straight line equation of the calibration object in the camera coordinate system; determining an intersection point matching constraint error function based on the feature point coordinate values in the laser radar coordinate system and in the camera coordinate system; determining a re-projection constraint error function based on the feature straight line equations in the laser radar coordinate system and in the camera coordinate system; and determining a laser radar and camera external parameter registration result based on the intersection point matching constraint error function and the re-projection constraint error function through an iterative closest point registration algorithm.

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 using 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, 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 fusing and using multiple different sensors, there is a technical problem that 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 related technologies that external parameters cannot be registered when multiple different sensors are integrated and used, and to achieve high-precision external parameter registration results.

[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 includes the following steps.

[0007] Obtain 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 squares method based on the vertical distance, fit and optimize the point cloud data 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 feature point coordinate values ​​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; perform color correction on the image data. 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; through an iterative nearest point alignment algorithm, the external parameter alignment 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.

[0008] According to a method for extrinsic 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 on their sides and a two-dimensional checkerboard calibration plate.

[0009] According to the present invention, a method for external parameter registration and inter-frame loading based on laser radar and camera is provided. The method fits and optimizes 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 characteristic 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 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; performing conditional filtering based on the sum of the squares of the distances of all internal points of the four planes in the height direction to determine the plane equation parameters of the two-dimensional checkerboard 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 checkerboard calibration plate plane to determine the feature 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.

[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 further 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 registration and inter-frame loading device based on laser radar and camera, comprising the following modules: an acquisition module for 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 the calibration object; a fitting and optimization module for 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; a first determination module for determining the feature point coordinate values ​​of the calibration object in the image data in the camera coordinate system according to a checkerboard corner detection algorithm based on seed growth. point coordinate values; 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 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; 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 nearest 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 runnable on the processor. When the processor executes the program, it implements any of the above-described external parameter registration and inter-frame loading methods based on laser radar and camera.

[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, it implements any of the above-mentioned external parameter alignment and inter-frame loading methods based on laser radar and camera.

[0016] The present invention also provides a computer program product, comprising a computer program, which, when executed by a processor, implements any of the above-described methods for external parameter registration and inter-frame loading 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 by 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 feature line equations 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 line fitting to determine the feature line equations of the calibration object in the camera coordinate system, thereby further improving the accuracy of the feature points and feature line equations 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 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 aligned 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 any 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 This is a schematic diagram of corresponding frame extraction and interpolation provided by the present invention.

[0023] Figure 5 This is a flowchart of RANSAC single feature plane segmentation and fitting provided by the present invention.

[0024] Figure 6Schematic diagram of the sub-pixel corner detection principle provided by the present invention.

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

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

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

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

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

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

[0031] Figure 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] Figure 14 It is a schematic diagram of the physical structure of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0033] To make the objectives, technical solutions, and advantages of the present invention more clear, the technical solutions of the present invention will be clearly and completely described below in conjunction with the accompanying drawings. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0034] With the continuous advancement of digital technology, people are increasingly experiencing the convenience brought by 3D reconstruction technology in their daily lives. They can now tour tourist attractions online, preview home renovations, and more on their mobile phones. This kind of virtual reality is inseparable from the support of 3D scene reconstruction technology. 3D reconstruction technology has unknowingly become part of every aspect of our lives. It not only plays an indispensable role in intelligent driving, robotics, the metaverse, and game development, but is also increasingly permeating architectural design, such as virtual attractions and online renovations, and cultural heritage preservation. Furthermore, 3D reconstruction technology has become an indispensable component in key fields such as healthcare and the military. 3D reconstruction technology provides a highly accurate reproduction of real scenes, enabling a more realistic, efficient, and convenient experience.

[0035] In simple terms, 3D reconstruction technology uses machines to replace the human eye, observing the surrounding environment and storing it as 3D data that more closely resembles the real world, meeting various human needs. Evaluation of 3D reconstruction requires comprehensive consideration of its accuracy, authenticity, and real-time performance. 3D reconstruction relies on a variety of sensors that replace the human eye in perceiving the world, including traditional cameras, global positioning systems, and the increasingly popular LiDAR (LiDAR). With the continuous advancement of sensors and related technologies, most modern environmental perception systems are no longer limited to a single sensor, but instead combine multiple sensors to form a combined sensor system based on specific needs. Cameras, as a long-established sensor, despite having many limitations compared to newer sensors, remain a staple in combined sensor systems due to their speed, convenience, color information, low cost, and technological maturity. LiDAR, meanwhile, has gradually gained traction with the rise of intelligent driving. Its advantages, such as high precision, strong environmental adaptability, and long-range sensing, have led to its application in a growing number of technological fields. As two currently mainstream sensors, both capable of performing their own functions, combined perception using LiDAR and cameras is becoming an increasingly popular choice. Therefore, this paper combines LiDAR and cameras 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 richer, multi-dimensional information for people to perceive the environment, the problem of external parameter calibration for multi-sensor fusion must first be solved. 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 3D reconstruction, it is also necessary to consider the registration problem between different frames of data from the same sensor. Although a large number of research methods have been developed for these two key issues, an absolutely mainstream or unified solution has not yet been formed. In most cases, the two are regarded as two independent issues that cannot provide convenience for each other.

[0037] A comprehensive solution that can be used for both sensor calibration and 3D reconstruction registration problems can not only significantly improve the portability of operations, but also simplify 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 multi-frame data is used as a benchmark, and the point clouds of each frame are aligned, thereby achieving 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 point cloud is projected into the corresponding image frame, and bilinear interpolation is used to obtain the color information of each point cloud, completing the colored point cloud three-dimensional scene reconstruction task.

[0039] refer to Figure 1 , Figure 1 This is a schematic diagram of the overall framework of the external parameter registration and inter-frame loading method based on the laser radar and camera provided by the present invention, such as Figure 1 Said method comprises the following aspects:

[0040] To bridge the extrinsic calibration problem of lidar and cameras with the joint 3D scene reconstruction problem, a calibration object system was designed that can be used for both sensor calibration and 3D reconstruction registration. The system consists of a pyramid calibration object with a checkerboard pattern on three sides and a 2D checkerboard calibration plate. Scanning data can be obtained by a single person, and simply rotating the scanning system 90 degrees converts the extrinsic calibration scan into a 3D scene reconstruction scan. Furthermore, sharing the same calibration object system allows for the use of some feature point extraction and registration methods for both extrinsic calibration and 3D reconstruction, reducing algorithmic complexity.

[0041] To improve the accuracy and robustness of lidar and camera extrinsic calibration, various strategies and methods are employed. In addition to using the intersection of four checkerboard planes as feature points for registration, the reprojection error of 3D edges into the image coordinate system is also introduced as an additional constraint. During the feature extraction phase, the RANSAC algorithm is combined with a least-squares method based on perpendicular distances to accurately fit the point cloud plane. The RANSAC algorithm is used to filter reliable point cloud samples from the raw data, while the least-squares method based on perpendicular distances performs a more precise fit on these samples, resulting in a more accurate plane equation. This approach not only improves feature extraction accuracy but also enhances robustness, providing more accurate and stable features for subsequent registration. The feature registration process is divided into three stages. First, a coarse registration stage based on the least-squares method provides an initial, roughly correct alignment for the entire registration process. The feature plane fit is then optimized using the geometric constraint of the three sides of the pyramid being perpendicular to each other. Finally, the ICP registration algorithm is used to accurately solve the lidar and camera extrinsic parameter matrices.

[0042] In joint 3D scene reconstruction using LiDAR and cameras, we first address the cumulative errors caused by inter-frame registration and the random errors inherent in a single set of data. We then introduce a global optimization method based on loop closure detection. Using a global point cloud generated from multi-frame registration, we perform global loop closure optimization on each frame. This not only improves calibration and reconstruction accuracy but also ensures global scene consistency. Finally, the point cloud is mapped onto an image using an extrinsic matrix to obtain its color information, resulting in a high-precision, colored 3D point cloud reconstruction. This not only makes the reconstruction more vivid and realistic, but also provides rich information for potential applications such as environmental perception and object recognition.

[0043] 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 as an example of executing the external parameter alignment and inter-frame loading method based on laser radar and camera in this embodiment.

[0044] Figure 2 This is a flow chart of the external parameter registration and inter-frame loading method based on the laser radar and camera provided by the present invention, such as Figure 2 As shown, the method includes the following:

[0045] Step 201 : 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.

[0046] In an 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.

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

[0048] 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 based on the coordinate values ​​of the point cloud for subsequent operations.

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

[0050] The extrinsic 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, thereby achieving 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 will always be more or less errors. You can refer to Figure 4 .

[0051] Since the LiDAR frame rate is lower than the camera frame rate, it is more accurate to find matching image frames using LiDAR point cloud frames. The method is divided into the following steps:

[0052] (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 frames by checkerboard calibration. 、 and 、 Solve it.

[0053] (2) Calculate the time interpolation factor by the time difference between two image frames Interpolate the rotation matrix and translation matrix. The translation matrix interpolation can be easily obtained using linear interpolation. The formula is as follows:

[0054]

[0055]

[0056] in and 、 are the timestamps of the lidar point cloud frame and its two adjacent image frames, The linear interpolation result of the image frame translation matrix.

[0057] (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 universal lock problems 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:

[0058]

[0059] in, is the real part (or scalar part) of the quaternion, is the imaginary part (or vector portion) of the quaternion.

[0060]

[0061] 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:

[0062]

[0063] 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 after interpolation of the image frame .

[0064] In step 202 , the point cloud data is fitted and optimized based on a random sampling consistency algorithm and a least squares method based on vertical distance to obtain the coordinate values ​​of the feature points of the calibration object in the lidar coordinate system and the characteristic straight line equation of the calibration object in the lidar coordinate system.

[0065] According to the external parameter registration 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.

[0066] According to the external parameter registration and inter-frame loading method based on the 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 squares method based on the vertical distance, and 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 are obtained, including:

[0067] 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;

[0068] 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 fitting plane equations of the four planes are optimized to obtain the plane equation parameters of the four planes;

[0069] Conditional filtering is performed based on the sum of the squares of the distances of all the inner points in the four planes in the height direction to determine the plane equation parameters of the two-dimensional checkerboard calibration plate plane in the four planes;

[0070] 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 coordinate values ​​of the feature points of the four intersection points of the four planes and the characteristic straight line equations of the three ridges of the pyramid are determined.

[0071] refer to Figure 5 , Figure 5 This is a flowchart of RANSAC single feature plane segmentation and fitting provided by the present invention.

[0072] The steps for iteratively splitting the four planes are as follows:

[0073] Step 1: Randomly select three points from the calibration object point cloud to form a plane equation.

[0074] 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, and 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.

[0075] Step 3: 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 n' of interior points of the new plane equation C'. If the number n' of interior points of the new plane is greater than the number n of interior points of the pre-selected plane, update the pre-selected plane to the new plane C' and update the set of interior points of the pre-selected plane.

[0076] 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.

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

[0078] 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 value of the number of interior points of the valid plane It should be noted that if the distance threshold of the plane point is If it is set too small, the RANSAC algorithm will distinguish between inliers and outliers more strictly, and points will only be considered inliers when the distance to the fitting plane is very close.

[0079] Since the accuracy of the RANSAC algorithm is highly dependent on parameter settings, and its randomness can 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, this paper sets relatively conservative parameters for RANSAC, only segments the point cloud of 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.

[0080] For the traditional ordinary least squares method based on z-axis distance residual, the general expression of the plane equation is:

[0081] Ax+By+Cz+D=0

[0082] Where A, B, and C are the components of the plane normal vector, and D is the offset of the plane, which can be transformed into:

[0083]

[0084]

[0085] So, using all n point coordinates ( In the equation, x and y 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:

[0086]

[0087] Use the least squares method to find the plane parameters that minimize the loss function 、 and After this, the expression for the plane equation can be obtained. However, this method's loss function assumes that the x and y values ​​of the point are accurate, while the coordinates of the actual midpoint contain errors on all three axes. Therefore, the results of this method are not sufficiently accurate and robust. Furthermore, using only the z-value residual as the loss function makes the results susceptible to the influence of the plane angle. As the angle between the plane to be fitted and the xy plane increases, the overall z-value residual also increases, thus affecting the fitting accuracy.

[0088] Therefore, in this 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. The method is as follows:

[0089] For a plane equation, (A, B, C) represents the normal vector of the plane, (x, y, z) is the coordinate of a point in space, and D represents the distance from the plane to the origin. It can be expressed as:

[0090]

[0091] Furthermore, every plane equation can be written in terms of a unit normal vector, satisfying:

[0092]

[0093] Therefore, the three parameters a, b, and c are introduced to establish a relationship with the plane equation parameters A, B, C, and D:

[0094]

[0095] The point-to-plane distance formula can be expressed as:

[0096]

[0097] The 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 in and find the values ​​of parameters a, b, and c when minimizing the objective function, namely:

[0098]

[0099] 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. Then, the plane equation parameters A, B, C, and D are solved in reverse.

[0100] For the four plane equations obtained after fitting and optimization, since the angle between the 2D checkerboard calibration plane and the ground is the smallest, conditional filtering is first used to separate the 2D checkerboard calibration plane from the four planes based on the sum of the squared height distances of all interior points of each plane. Then, through plane intersection, the equations of the four intersection points and the three edge lines of the pyramid are solved, completing the extraction of feature points and feature lines from the lidar point cloud.

[0101] Step 203 : determining 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.

[0102] To extract the checkerboard plane in an image, we first need to detect the black and white checkerboard corners in the image. The traditional checkerboard corner detection algorithm steps are as follows:

[0103] First, the image is binarized into black and white using the average adaptive thresholding method to highlight the rectangular structure of the checkerboard.

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

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

[0106] 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.

[0107] 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. 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.

[0108] Although this checkerboard corner detection method is simple to implement and fast, it has poor stability and environmental adaptability. It requires the number of checkerboard rows and columns to be given in advance, and it is easy to fail detection when the checkerboard is tilted at a large angle.

[0109] The checkerboard corner detection algorithm based on seed growth can be applied to the case of detecting multiple checkerboards at a time. The method steps of the embodiment of the present invention are as follows:

[0110] Step 1: Roughly extract the corner positions by using Gaussian filtering and non-maximum suppression method.

[0111] 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 checkerboard corners.

[0112] Then the grayscale image is convolved using the Gaussian filter kernel. The pixels of the four result images are averaged to create the average image. In the average image, the values ​​of flat areas remain essentially unchanged after convolution, while different convolution kernels produce different values ​​in the direction of the edge at corners or edges. Therefore, the difference image obtained by subtracting the four result images from their average image can highlight the approximate location range of the corners.

[0113] Finally, the non-maximum suppression algorithm is used to find the local maximum within each corner point's local region. The algorithm then checks whether any of the local maximum's neighbors have a larger value than the local maximum. If so, the algorithm marks the local maximum as a failure. Boundary detection conditions must also be set to ensure that the local maximum is not a local maximum on a boundary. If the local maximum is not marked as a failure and its value is greater than a pre-set threshold, the location of the local maximum is marked as a candidate corner point.

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

[0115] refer to Figure 6 , Figure 6 Schematic diagram of the sub-pixel corner detection principle provided by the present invention.

[0116] 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. It 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 the points located in the flat area of ​​the black and white grid are e.g. 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 vectors and 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 vector of all points in its neighborhood is With vector The dot product is 0, that is:

[0117]

[0118] 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. But it is different from the ideal corner point c in that for the point at the intersection of the black and white grid and , since its gradient vector 、 and 、 Not perpendicular, so the dot product is not 0, that is:

[0119]

[0120] Therefore, in order to convert the actual pixel-level corner Approaching the ideal corner point c, choose to use Neighborhood point set ), the sum 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:

[0121]

[0122] Step 3: Checkerboard corner seed growth.

[0123] For a seed corner point, we first extract nine corner points in its neighborhood as the initial 2*2 checkerboard, and then gradually grow from the edge to the surrounding areas. During the growth process, we predict the third corner point by using two adjacent corner points. The distance between the predicted point and the nearest corner point is used as an important basis for evaluation. The score is as follows:

[0124]

[0125] in is based on and Predicted growth corners, It's 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 checkerboard for subsequent comparison of overlapping checkerboards.

[0126] After detecting the complete checkerboard, the edge corners of the checkerboard are marked so that they can be avoided when selecting seed corners, reducing the number of iterations and the amount of computation. Furthermore, in actual checkerboard growth, two checkerboards may become stuck together. Since the number and size of the checkerboards in the image are known, these stuck checkerboards can be identified by detecting the number and size of the checkerboards and then separated based on the known checkerboard size.

[0127] After obtaining the pixel coordinates of the checkerboard corners, the direct linear transformation (DLT) algorithm is used to solve the pose transformation relationship between each checkerboard coordinate system and the camera coordinate system. For each checkerboard, set its own coordinate system 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.

[0128] The same point in pixel coordinates With the checkerboard coordinate system The coordinate transformation relationship between them is as follows:

[0129]

[0130] The 3*4 matrix is is the intrinsic parameter matrix of the camera, and are the rotation matrix and translation matrix between the checkerboard coordinate system and the camera coordinate system.

[0131] 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 checkerboard coordinate system of the checkerboard corner point:

[0132]

[0133] Right now:

[0134]

[0135] in and , respectively represent the coordinates of the chessboard corner points in the chessboard coordinate system and the camera coordinate system, M is the intrinsic parameter matrix of the camera, 、 、 is the rotation matrix between the checkerboard coordinate system and the camera coordinate system The weight, is the translation matrix between the checkerboard coordinate system and the camera coordinate system, is the scale factor.

[0136] Define the homography matrix H:

[0137]

[0138] but:

[0139]

[0140] Substituting each set of coordinates of the corner points into the equation:

[0141]

[0142] 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 , combining it with the inherent constraint properties of the rotation vector, referring to the following formula, the rotation matrix between the checkerboard coordinate system and the camera three-dimensional coordinate system can be decomposed from H and translation matrices .

[0143]

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

[0145] Step 204 : Perform color segmentation and line fitting on the image data to determine the characteristic line equation of the calibration object in the camera coordinate system.

[0146] Set the three edges of the pyramid to red, and extract the equations of the three edges in the image through color segmentation and line fitting. The specific steps are as follows:

[0147] Color segmentation: Convert the image's color space from RGB to HSV, and use conditional filtering to extract the red areas in the image, leaving the rest of the image black.

[0148] The RANSAC algorithm is used to extract the equations of three edge lines. First, two red pixels are randomly selected and fitted to a line equation. The number of red pixels passing through the line equation is determined to be greater than a threshold value n. If so, the line is updated to the preselected line, and all points whose distance from the line is less than a threshold value d are marked as inliers. Next, two more pixels are randomly selected from the inliers of the preselected line to fit a new line equation. If the inliers of the new line equation are greater than the preselected line, it is updated to the preselected line. Otherwise, the algorithm continues to iterate until the specified number of times has passed. Finally, the fitted line equation and its inliers are saved, and all inliers of the line are set to black. The same method is then used to extract two more lines, completing the extraction of the pyramid feature edges in the image.

[0149] Step 205 : Determine 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.

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

[0151] The coordinate transformation relationship of the same point in the laser radar and camera coordinate systems is as follows:

[0152]

[0153] 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.

[0154] 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, namely the vertex of the pyramid tip 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:

[0155] (1) Calculate the centroid of two point sets C and L:

[0156]

[0157] in, represents the centroid of the point set L, represents the centroid of the point set C, represents the intersection of the four checkerboard planes in the camera coordinate system, Represents the intersection of four checkerboard planes in the lidar coordinate system.

[0158] (2) Point set centralization. Subtract the centroid of the point set from the points in the point set to obtain the decentralized point set. and :

[0159]

[0160] (3) For the matrix Using singular value decomposition (SVD), the initial rotation matrix can be solved , and then solve the translation matrix .

[0161]

[0162]

[0163] (4) Distance residual calculation. Calculation point set All points in and The residual sum of squares of the distance between the transformed and corresponding points in point set C is:

[0164]

[0165] (5) By calculating the correspondence between two possible point sets and , compare the distance residual sum of squares under the two point set correspondences. The set of data with the lower distance residual sum of squares is the correct point set correspondence, and its corresponding rotation and translation matrix is and are the initial external parameter estimates.

[0166] The degree of dispersion of LiDAR point cloud data in the directions parallel to and perpendicular to the laser beam varies, and the scanning direction of the calibration object plane is different. As a result, the number of in-plane points in the several planes varies, resulting in different fitting errors for each plane. Therefore, the geometric constraint relationship of the three sides of the pyramid being perpendicular to each other is used to optimize the equations of the three LiDAR point cloud planes.

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

[0168] 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 between the three edges of the pyramid after mapping the initial external parameters.

[0169] The specific steps for plane correction are as follows:

[0170] (1) Determine the plane error sorting. 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 the 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 .

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

[0172]

[0173] (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, e, is 0.

[0174]

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

[0176] refer to Figure 9 , Figure 9 This is a schematic diagram of loop detection provided by the present invention.

[0177] 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: Figure 9 , select points with equal distance on the extension lines of the three edges of the pyramid.

[0178] The ICP registration algorithm mainly uses the idea of ​​iterative optimization, taking the sum of the squared distances between the two sets of feature points as the error function. By adjusting the correspondence between the points in the two point sets, the rigid pose transformation between the two point sets is continuously optimized to achieve the maximum matching result. In order to reduce the impact of the cumulative 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, an error constraint on the reprojection angle of the intersection of the pyramid point cloud planes to the image coordinate system is introduced. The specific registration steps are as follows:

[0179] Establish an error function. The error function includes two constraints: the feature plane intersection matching constraint and the pyramid three edge reprojection constraint.

[0180] Among them, the feature plane intersection matching constraints include:

[0181] The feature point clouds in the laser radar and camera coordinate systems are defined as follows: 、 , first find the lidar feature point cloud Each point in In the camera feature point cloud The nearest point in is used as the matching point. Then the external parameter estimation matrix between the matching points is calculated by the formula and The sum of squares of the distance residual after mapping is used as the error function of this part:

[0182]

[0183] Step 206 : 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.

[0184] The reprojection constraints of the three pyramid edges include:

[0185] Through feature extraction, the straight line function of the three ridges of the pyramid in the laser radar coordinate system and the straight line parameters in the two-dimensional image are obtained. The ridge equation in the laser radar coordinate system is projected into the two-dimensional image, and its slope is recorded as , the slope of the ridge in the two-dimensional image is recorded as , the sum of the squares of the differences between the two slopes is used as the error function of the constraint, where j is the sequence number of the three edges:

[0186]

[0187] Finally, the two error functions are weighted to obtain the final total error function:

[0188]

[0189] Step 207 , determining the external parameter registration result between the lidar 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.

[0190] By combining the two points in the above content and Continuous adjustment of the nearest neighbor points in the image to iteratively optimize the rotation matrix between the two sensor coordinate systems and translation matrices , until the target number of iterations is reached or the error L reaches the pre-set threshold. Finally, the optimized high-precision external parameter matrix T is obtained:

[0191]

[0192] Because single-set registration results are unstable and limited, and because lidar point cloud frames and corresponding camera image frames are matched using timestamp interpolation, to improve registration robustness, multiple sets of lidar and camera frames are used to solve for global extrinsic parameters. The basic idea of ​​this method is as follows: First, based on multi-frame point cloud registration and multi-frame image data registration, a global lidar feature point cloud and a global image feature point cloud are solved, respectively. Next, these 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 relies on the inter-frame registration of all adjacent frames, cumulative errors will occur, affecting the accuracy of the global feature point cloud. To address this, loop closure detection is introduced. Using the global feature point cloud as a benchmark, loop closure optimization is performed on all single-frame point clouds to reduce the cumulative error. Finally, the loop-closed optimized lidar and camera global feature point clouds are registered to solve for accurate and robust global extrinsic 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.

[0193] According to the method for extrinsic parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, after determining the extrinsic parameter registration result of the laser radar and the camera by using an iterative closest point registration algorithm based on an error function of an intersection matching constraint and an error function of a reprojection constraint, the method further includes:

[0194] 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;

[0195] Perform iterative closest point registration on the point cloud data based on the total error function to obtain the spatial change matrix between all adjacent frames;

[0196] The spatial variation matrices between all adjacent frames are aligned with the spatial variation matrix of the first frame to obtain the global feature point cloud of the lidar.

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

[0198] 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. Figure 10 Since the LiDAR frame rate is 10 frames per second, and the camera data is obtained by interpolating the timestamps of the LiDAR point cloud frames to obtain the corresponding camera frames, the spatial transformation between adjacent frames is very small. Therefore, when using the ICP algorithm to align adjacent frames, there is no need to calculate the initial value, and the solution can be directly performed.

[0199] 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 , obtain the global point cloud of the lidar Similarly, solve the global point cloud of the camera.

[0200] According to the method for extrinsic parameter registration and inter-frame loading based on a laser radar and a camera provided by the present invention, after aligning the spatial variation matrices 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:

[0201] Based on the global feature point cloud, each frame of the point cloud data is registered and aligned to perform loop closure optimization on each frame of the point cloud.

[0202] 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 distance The further away the frame, the more aligned it is. When , the errors of the spatial transformation matrices of all previous adjacent frames will be accumulated, making the error larger and larger. In order to reduce the impact of the accumulated error on frames that are farther away, the idea of ​​loop detection is adopted.

[0203] Specifically, because the global point cloud is derived from the combined solution of all frame point clouds, it reflects the overall characteristics of all frame point cloud data. Therefore, using the global point cloud as a benchmark, each frame point cloud is registered and optimized. This effectively reduces cumulative errors and improves the accuracy and stability of subsequent overall registration.

[0204] refer to Figure 11 , Figure 11 This is a schematic diagram of loop optimization provided by the present invention.

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

[0206] like Figure 11 As shown, 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 pose transformation, so as to reduce the cumulative error. Figure 12 As shown in Figure 3, the global point cloud obtained is registered with each frame point cloud to optimize the cumulative error.

[0207] 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.

[0208] When solving the global extrinsic parameter matrix, the ICP algorithm is used to accurately align the lidar and camera global point clouds after global loop closure optimization. To ensure a more accurate starting point for the ICP algorithm, the registration result between the first frame of the lidar feature point cloud and its corresponding timestamp camera feature point cloud is selected as the initial value, providing strong support for subsequent iterative optimization.

[0209] 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 external parameters. Then use the global point cloud composed of lidar and camera and The feature points of all frames constitute the global feature point cloud, which is recorded as and , the ICP algorithm is used to perform global registration on the two. The specific registration steps are as follows:

[0210] 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 in as corresponding points.

[0211] 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 cloud of the lidar and the camera contains 10n points respectively. The correspondence between these 10n points has been selected in (1). Substitute these 10 groups 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.

[0212]

[0213] Step 3: The result obtained in step 2 and Acting on 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.

[0214] 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 .

[0215] Since lidar point clouds have the advantages of high precision and being less susceptible to environmental influences, the surrounding scenes are mainly reconstructed using lidar point cloud data. Then, for each frame of lidar point cloud, its corresponding camera image frame is found, and all points are projected into the image using the previously solved external parameters of the lidar and camera to obtain the color information of the points, thereby completing the high-precision lidar and camera colored point cloud scene reconstruction task.

[0216] For data scanning in the joint scene reconstruction of the lidar and camera, a similar scanning method is used as in the calibration module, 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 calibration object system is fully included in the image, while in the reconstruction task, the camera is facing sideways to provide color information of the surrounding scene for reconstruction. The specific steps include:

[0217] Step 1: LiDAR multi-frame point cloud registration.

[0218] For multi-frame point clouds from LiDAR, a point cloud feature extraction method is used. The four feature plane point clouds are segmented and roughly fitted using the Random Sampling Consensus (RANSAC) algorithm. The point cloud planes are then accurately fitted using the least squares method based on the vertical distance residual and the unit normal vector constraint of the plane equation. The equations of the four feature planes are obtained. 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. Alignment uses the ICP algorithm, which follows the same principle as the LiDAR global point cloud solution. After alignment, all frames are aligned to the first frame to obtain a global point cloud. A global loop closure optimization method is then used to optimize the point clouds of each frame with the help of the global point cloud to reduce the impact of the cumulative error caused by the registration between adjacent frames.

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

[0220] 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.

[0221]

[0222] in is the coordinate of the point in the lidar 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 lidar and camera after global optimization, that is, the rotation matrix and translation matrix between the two sensor coordinate systems.

[0223] Since the (u, v) coordinates are not integers, interpolation is required to determine the color information of the point. By performing linear interpolation on the RGB color values ​​in two directions, the color values ​​of the non-integer coordinates (u, v) are obtained, thereby obtaining the color information of the 3D point cloud. The specific steps are as follows:

[0224] First, mark the four integer coordinates closest to (u,v) as 、 、 、 , perform linear interpolation in the u direction:

[0225]

[0226] Then perform linear interpolation in the v direction:

[0227]

[0228] Where C represents the color value, is the final bilinear interpolation result.

[0229] Step 3: Downsample the colored point cloud 3D scene reconstruction results.

[0230] Given that the data from scanning and reconstructing the surrounding scene includes a large number of LiDAR point cloud frames, with each frame containing over 60,000 points, the final color point cloud scene reconstruction contains millions of points, making the reconstruction too dense and redundant. To reduce the number of point clouds without significantly affecting the reconstruction quality of the scene, point cloud voxel filtering was selected as a point cloud downsampling method. This method preserves the characteristics and shape of the point cloud while maintaining high computational speed. At an appropriate voxel size, it can achieve high-quality point cloud downsampling.

[0231] First, the point cloud space is divided into voxel grids of fixed size according to the maximum and minimum values ​​of all points in the point cloud in the three-axis directions. 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.

[0232]

[0233] 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.

[0234] 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.

[0235] The following describes the external parameter alignment and inter-frame loading device based on laser radar and camera provided by the present invention. 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.

[0236] refer to Figure 13 , Figure 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.

[0237] An acquisition module 1301 is configured to acquire point cloud data collected by a laser radar and image data collected by a 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;

[0238] 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, and obtain the coordinate values ​​of the characteristic points of the calibration object in the laser radar coordinate system and the characteristic line equation of the calibration object in the laser radar coordinate system;

[0239] A first determination module 1303 is configured 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 detection algorithm based on seed growth;

[0240] The second determination module 1304 is used to perform color segmentation and line fitting on the image data to determine the characteristic line equation of the calibration object in the camera coordinate system;

[0241] A matching constraint module 1305 is configured 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;

[0242] The projection constraint module 1306 is 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;

[0243] 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 the iterative closest point registration algorithm.

[0244] 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.

[0245] Figure 14 This is a schematic diagram of the physical structure of the electronic device provided by the present invention, such as Figure 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 via the communications bus 1440. The processor 1410 may call the logic instructions in the memory 1430 to execute an external parameter registration and inter-frame loading method based on the laser radar and the camera, the method comprising: 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; 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; determining the image corner point based on a checkerboard corner detection algorithm based on seed growth. The feature point coordinate values ​​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; based on the feature point coordinate values ​​of the calibration object in the lidar coordinate system and the feature point coordinate values ​​of the calibration object in the camera coordinate system, determine the error function of the intersection matching 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 error function of the reprojection constraint; determine the external parameter alignment 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 alignment algorithm.

[0246] Furthermore, the logic instructions in the aforementioned memory 1430 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product, stored in a storage medium, includes instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0247] On the other hand, the present invention also provides a computer program product, which includes a computer program, which 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 the laser radar and camera provided by the above methods, and 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. The characteristic straight line equation of the calibration object in the camera coordinate system is determined according to the checkerboard corner detection algorithm based on seed growth; the characteristic point coordinate values ​​of the calibration object in the image data in the camera coordinate system are determined; the image data is color segmented and straight line fitting is performed 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, 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 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.

[0248] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which is implemented by a processor to execute the external parameter alignment and inter-frame loading method based on the laser radar and camera provided by the above methods, the method comprising: 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; 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 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; according to the method based on A seed-grown checkerboard corner detection algorithm is used to determine the feature point coordinate values ​​of the calibration object in the image data in the camera coordinate system; color segmentation and straight line fitting are performed on the image data to determine the feature line equation of the calibration object in the camera coordinate system; based on the feature point coordinate values ​​of the calibration object in the lidar coordinate system and the feature point coordinate values ​​of the calibration object in the camera coordinate system, the error function of the intersection matching constraint is determined; 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, the error function of the reprojection constraint is determined; the external parameter alignment result of the lidar and camera is determined based on the error function of the intersection matching constraint and the error function of the reprojection constraint through the iterative closest point alignment algorithm.

[0249] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0250] Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using software plus a necessary general-purpose hardware platform, or of course, hardware. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for causing a computer device (such as a personal computer, server, or network device) to execute the methods described in each embodiment or certain portions of the embodiments.

[0251] 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 various 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 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 the calibration object; Based on the random sampling consistency algorithm and the least squares method based on the vertical distance, the point cloud data is fitted and optimized 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; Determining the coordinate values ​​of feature points of the calibration object in the image data in a 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; Determining 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; Determining a reprojection constraint error function based on a characteristic straight line equation of the calibration object in the lidar coordinate system and a characteristic straight line equation of the calibration object in the camera coordinate system; An 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 their 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, characterized in that: The point cloud data is fitted and optimized based on the random sampling consistency algorithm and the least squares 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; Perform conditional filtering based on the sum of the squares of the distances of all the interior points of the four planes in the height direction, and determine the plane equation parameters of the two-dimensional checkerboard calibration plate plane in the four planes; Based on the plane equation parameters of the four planes and the plane equation parameters of the two-dimensional checkerboard calibration plate plane, an intersection operation is performed 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 ridges of the pyramid.

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 extrinsic parameter registration result between 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 matrices between all adjacent frames are 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, 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 of point cloud in the point cloud data is registered and aligned to perform loop closure optimization on each frame of 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 extrinsic parameter registration result between 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. A device 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 device includes: an acquisition module, configured 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 the calibration object; A fitting and optimization module is 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, 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 is configured to determine the coordinate values ​​of feature points of the calibration object in the image data in a camera coordinate system according to a checkerboard corner detection algorithm based on seed growth; A second determination module is configured 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, configured 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, configured to determine a reprojection constraint error function based on a characteristic straight line equation of the calibration object in the lidar coordinate system and a characteristic straight line equation of the calibration object in the camera coordinate system; An 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 registration and inter-frame loading method based on laser radar and camera as described in any one of claims 1 to 6 is implemented.

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 registration 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