Laser radar-camera automatic calibration method based on boundary features

Abstract

The invention relates to the technical field of multi-sensor calibration, and particularly discloses a laser radar-camera external parameter calibration method based on boundary features, which comprises the following steps: taking a checkerboard calibration plate as a calibration target, and automatically extracting a complete point cloud and an image of the calibration plate; a theoretical boundary point cloud constructed based on the real size of the calibration plate is aligned to the extracted actual scanning boundary, and coarse calibration is carried out according to the center point of the calibration plate image and the center point of the corresponding theoretical boundary point cloud; and performing external parameter optimization by using a boundary re-projection error and a normal vector consistency error of the calibration plate point cloud and the image in a fine calibration stage.

Description

Technical Field

[0001] This invention relates to the field of multi-sensor calibration technology, specifically to a method for extrinsic parameter calibration of lidar and cameras, and in particular to a method for achieving high-precision extrinsic parameter calibration based on sparse point clouds. Background Technology

[0002] In recent years, LiDAR-camera fusion perception has attracted widespread attention in fields such as autonomous driving, robotics, and 3D reconstruction, and is one of the research hotspots in the field of 3D vision. LiDAR can provide high-precision 3D point clouds, but lacks texture and color information, and sparse point clouds may lead to indistinct local features; cameras can capture rich visual semantic information, but are susceptible to the influence of external environments such as lighting, and depth measurement relies on complex calculations. Fusion of the two can achieve more comprehensive environmental perception in multiple scenarios. Accurate extrinsic parameter calibration is an important prerequisite for LiDAR-camera fusion perception. Extrinsic parameter calibration refers to the process of estimating the rigid transformation parameters between LiDAR and camera through mathematical modeling and optimization methods, thereby aligning their coordinate systems. This process is the basis for accurately projecting 3D point clouds onto 2D images; therefore, obtaining high-precision and robust extrinsic parameters has become a challenge for LiDAR-camera fusion.

[0003] Existing LiDAR-camera extrinsic parameter calibration methods are mainly divided into two categories: target-based methods and target-free methods. Target-based methods, such as those described in patents (publication number: CN119540370A, title: LiDAR and Camera Extrinsic Parameter Calibration Method, System and Storage Medium) and (publication number: CN120125672A, title: A LiDAR-Camera Fusion Calibration Method and System), are limited in accuracy by point cloud density. Accuracy decreases or even fails in sparse point cloud conditions. Target-based methods, such as those described in patent (publication number: CN120163884A, title: A Robot Hand-Eye Calibration Method Based on Joint Calibration of LiDAR and Camera), design their own calibration objects to optimize the calibration process for specific needs. However, the fabrication of these self-designed calibration objects is complex, and the accuracy of the self-made calibration objects directly affects the accuracy of the extrinsic parameter calibration. Targetless methods, such as the invention patent (publication number: CN117808895A, title: Targetless extrinsic parameter calibration method and related device for lidar and camera), have specific environmental requirements and must include elements such as cars and lane lines; targetless methods, such as the invention patent (publication number: CN119478059A, title: A targetless environment lidar and camera extrinsic parameter calibration method), have larger errors in some environments without rich textures or repetitive structures and require dense point clouds.

[0004] In summary, current calibration methods cannot perform high-precision lidar-camera extrinsic parameter calibration based on sparse point clouds without relying on specific environments, while dense point clouds are costly to acquire and complex to process. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, this invention proposes a lidar-camera extrinsic parameter calibration method based on boundary features.

[0006] This invention uses a checkerboard calibration board as the calibration target, automatically extracts the complete point cloud and image of the calibration board, aligns the theoretical boundary point cloud constructed based on the actual size of the calibration board to the extracted actual scanning boundary, and performs coarse calibration based on the center point of the calibration board image and the corresponding center point of the theoretical boundary point cloud. Then, in the fine calibration stage, the extrinsic parameters are optimized using the boundary reprojection error and the consistency error of the normal vector between the calibration board point cloud and the image.

[0007] The technical solution of this invention is as follows:

[0008] A lidar-camera extrinsic parameter calibration method based on boundary features includes the following steps:

[0009] Step 1: Obtain the point cloud data and image data required for external parameter calibration. Specifically, install and fix the lidar and camera, requiring them to have a common field of view. Place a rigid checkerboard calibration plate of size w×h within the common field of view, so that the calibration plate is within the effective detection range of both. During data acquisition, synchronize the timestamps of the lidar and camera through a switch, scan the calibration plate multiple times to obtain the corresponding point cloud and image. During each scan, the calibration plate needs to be in a different area of ​​the field of view, and the scanning should be performed with the calibration plate tilted.

[0010] Step 2: Preprocess the acquired point cloud and image data separately. The specific steps are as follows:

[0011] Step 2.1: Perform calibration plate extraction and ray projection operations on the point cloud data:

[0012] Step 2.1.1: Filter out environmental points in the point cloud that are irrelevant to the calibration plate area based on a spatial threshold. Fit the ground plane to the filtered point cloud and then filter out the ground point cloud to obtain point cloud P. init Further analysis of the point cloud P including the calibration board init A three-dimensional plane fitting is performed to obtain plane α. According to equation (1), points within a given distance d near plane α are retained as points of the calibration plate, thus obtaining the point cloud P of the calibration plate. b :

[0013]

[0014] Where (x,y,z) is the point cloud P initThe three-dimensional coordinates of a point in the plane are given by A, B, C, and D, which are parameters of the plane α.

[0015] Step 2.1.2: Convert the point cloud P, which includes fluctuation points and noise points, into a single point cloud. b By projecting the ray onto the fitting plane α, the three-dimensional planar point cloud P of the calibration plate is obtained. r As shown in equation (2):

[0016] Step 2.2: Preprocessing operations for image data, including distortion correction and calibration plate extraction, are as follows: Step 2.2.1: Given the camera's intrinsic parameters K and distortion coefficients dist, the image is distorted using OpenCV's undistort function based on K and dist, resulting in a distortion-free image I. u ;

[0017] Step 2.2.2: Obtain the chessboard bounding box by calling OpenCV's findChessboardCorners function, and expand it outward to obtain a new bounding box that includes the calibration board boundary. Perform edge detection and line detection within the new bounding box to obtain a set of all candidate boundary line segments of the calibration board. Where k is the number of all detected line segments;

[0018] Step 2.2.3: For each line segment in Represents line segment L i starting point, Represents line segment L i The endpoint is calculated using equation (3) to determine its unit direction vector v. i :

[0019]

[0020] According to v i By using clustering methods Divided into two clusters of G v and G h These are respectively used as line segments in the two main directions of the calibration plate;

[0021] Step 2.2.4: For G h Select the first line segment of the cluster and calculate its normal vector n according to equation (4). l :

[0022] For G h For each line segment within the range, calculate all center points c according to equation (5). i The projection CP onto the normal vector, where j represents the number of line segments in the cluster:

[0023]

[0024] Select the two line segments with the largest and smallest projection values ​​as the two outermost line segments L of this cluster. out1 and L out2 As shown in equation (6):

[0025]

[0026] Similarly, according to step 2.2.4, for G... v The two outermost line segments can also be obtained, along with G. h The outermost line segments together form the four boundary line segments L of the calibration plate. l L r L u L d ;

[0027] Step 2.2.5: For two line segments L a and L b Solve the system of linear equations with respect to t and s:

[0028]

[0029] If the coefficient matrix is ​​of full rank, then L a With L b The coordinates (x, y) of the intersection point are:

[0030] Find the four line segments L according to equations (7) and (8). r L l L u L d The intersection of two adjacent straight lines in the middle will give us the four corner points Cn1, Cn2, Cn3, and Cn4 of the calibration board. Then, we convert the four corner points in the envelope to I... u In the process, it is determined whether each pixel is within the quadrilateral formed by the four points. If not, the RGB values ​​of that pixel are set to (0,0,0), thus obtaining image I. m ;

[0031] Step 2.2.6: Obtain I by calling OpenCV's findContours function. m Contour = {(x1,y1),(x2,y2),…,(x...}}, ... n ,y n )}, used for subsequent calibration;

[0032] Step 3: Perform coarse calibration using the preprocessed data to obtain initial extrinsic parameters. The specific steps are as follows:

[0033] Step 3.1: Construct the theoretical boundary point cloud of the calibration board based on its actual dimensions, and align it with the actual scanned boundary point cloud. The specific steps are as follows:

[0034] Step 3.1.1: Apply the boundary extraction algorithm to extract the boundary point cloud of the calibration board, and obtain the actual scanned boundary point cloud P. tb Simultaneously, based on the actual dimensions of the calibration plate, a theoretical boundary point cloud P of size w×h is constructed. cb Step 3.1.2: For P cb With P tb Calculate the covariance matrix and perform eigenvalue decomposition to obtain the orthogonal basis V. cb and V tb , with V tb Using the principal direction as a reference, by matching the principal direction and combining it with centroid alignment, we obtain P. cb The transformation matrix to the target plane α is shown in equation (9), where μ cb μ tb P respectively cb With P tb Center of mass:

[0035] After transformation, we get P' cb =T Init ·P cb (After homogenization), make P tb With P' cb The two point clouds are on the same plane α, and preliminary alignment is performed based on the characteristics of the point clouds.

[0036] Step 3.1.3: Place P tb With P' cb According to the normal vector n of plane α p = (A, B, C) is rotated to a plane parallel to the XOY plane, and the rotation matrix is:

[0037] R P =I+sinωK r +(1-cosω)K r 2 (10)

[0038] in:

[0039]

[0040] Where, k r K is the unit vector along the axis of rotation. r It is an antisymmetric matrix;

[0041] Step 3.1.4: Since the two point clouds are already on the same plane, the transformation between them is only caused by the rotation angle θ and translation t around the Z-axis.x ,t y It was decided to use a global search algorithm to optimize the three parameters. The optimization of the transformation parameters is shown in Equation (12). The error function is defined as the square mean of the distances between each point in the rotated point cloud and its nearest neighbor in the target point cloud, where, This means finding the point q in the target point cloud that has the closest Euclidean distance to the transformed source point using a KD-tree search. j p i P' cb Point R in Z Let t be a rotation matrix. Z For translation vectors:

[0042] Step 3.1.5: After obtaining the results of the global search, a local optimization algorithm is used to further optimize them, similar to equation (12), to obtain the optimal parameters θ',t'. x ,t' y Then, construct the homogeneous transformation matrix T according to equation (13). Z :

[0043] Step 3.1.6: For P' cb Transform to obtain P” cb =T Z ·P' cb According to R P Rotate back to plane α, which is the actual plane where the calibration plate is located, and finally obtain the aligned theoretical boundary P. eb ;

[0044] Step 3.2: Estimate the camera pose using the corresponding point cloud and image center points. The specific steps are as follows:

[0045] Step 3.2.1: Perform the operations of Step 2 and Step 3.1 on the collected s (s≥4) sets of data to obtain the theoretical boundary point cloud set PEB={P eb1 ,…,P ebs} and the calibration plate image set IM = {I m1 ,…,I ms}, where the elements in the two sets have a one-to-one correspondence;

[0046] Step 3.2.2: Calculate the centroid C of the calibration board point cloud based on the theoretical boundary point cloud and the calibration board image boundary obtained in Step 2.2.6. L With image centroid C I We can obtain the set of point cloud center points CL = {C L1 ,…,C Ls} and the set of image center points CI = {C I1 ,…,C Is};

[0047] Step 3.2.3: Solve the PnP (Perspective-n-Point) problem based on CL and CI to obtain the coarsely calibrated extrinsic parameters, namely the rotation vector rvec and the translation vector tvec;

[0048] Step 4: Based on the coarse calibration, perform fine calibration. The specific steps are as follows:

[0049] Step 4.1: Calculate the normal vector of the calibration board point cloud and the normal vector of the calibration board in the image, respectively. The specific steps are as follows:

[0050] Step 4.1.1: Use the rvec and tvec obtained from the coarse calibration to transform the boundary point cloud set PEB, converting the lidar coordinate system to the camera coordinate system, and obtain PCAL = {P cal1 ,…,P cals}, as shown in equation (14):

[0051]

[0052] Wherein, Rodrigues(·) transforms the rotation vector into a Rodrigues rotation matrix, P ebi Let P be the homogeneous coordinate matrix of the point cloud in the lidar coordinate system. cali Let P be the homogeneous coordinate matrix of the point cloud in the camera coordinate system; Step 4.1.2: P cali normal vector n ei The normal vector of the plane can be obtained by the plane fitting algorithm; Step 4.1.3: The normal vector n of the calibration plate in the image set IM. ci The solution needs to be indirectly calculated using the PnP problem: In the world coordinate system containing the checkerboard (with the top-left corner of the checkerboard as the origin), establish a local coordinate system: the X-axis is to the right, the Y-axis is downward, and the Z-axis is perpendicular to the checkerboard plane and points towards the camera. Under this definition, the normal vector of the checkerboard in the world coordinate system is n. w =(0,0,1) T Then, the OpenCV `findChessboardCorners` function is used to detect the pixel corner coordinates of the chessboard in the image. Combined with the known 3D world coordinates of the corners, the rotation vector `r` from the world coordinate system to the camera coordinate system is solved using the PnP algorithm. ni According to equation (15), n w Transform to the camera coordinate system using a rotation matrix to obtain n ci :

[0053] n ci =Rodrigues(r ni )·n w (15)

[0054] Step 4.2: Refine the rotation and translation parameters. The specific steps are as follows:

[0055] Step 4.2.1: Represent the rotation using the quaternion q = [w, x, y, z], where quaternion q = w + xi + yj + zk (satisfying w 2 +x 2 +y 2 +z 2 =1) The corresponding rotation matrix R f It can be expressed by equation (16):

[0056] Step 4.2.2: According to equation (17), combine all P values ​​through the union operation. cali Merge into P merge In the middle, I mi The corresponding two-dimensional contour point set Contour i Merge into C merge middle:

[0057] Step 4.2.3: Perform point cloud P merge The point (x) in c ,y c ,z c ), and reproject it onto the image according to the reprojection function π(·), then obtain its pixel coordinates (u,v) in the image, as shown in equation (18):

[0058] The Euclidean distance from the reprojection point to the nearest point among the calibration board contour points is used as the error. The calibration board boundary point cloud is forced to be reprojected onto the corresponding boundary of the image. The boundary point reprojection error E edge As shown in equation (19):

[0059]

[0060] Among them, R f Let t be a rotation matrix. f Let η(·) be the translation vector in C. merge Find the nearest point to a given point, and introduce an error E. normal To ensure alignment, the normal vectors of the point cloud and the calibration board in the image are made parallel. The absolute value of the dot product of the normal vectors is used to measure the degree of parallelism, avoiding sensitivity issues related to the normal vector direction. The normal vector alignment error E... normal As shown in equation (20):

[0061] The complete error function E is shown in equation (21), where w e and w nWeights for the two errors:

[0062] E = w e ·E edge +w n ·E normal (twenty one)

[0063] An RMS-based dynamic error weighting method is adopted, w e It is always equal to 1.0. During optimization, w is dynamically adjusted according to equation (22) after each iteration. n :

[0064]

[0065] Where e represents the value of a single error term;

[0066] Step 4.2.4: Based on the error function E, optimize the rotation and translation parameters using a local optimization algorithm. After convergence, obtain the optimal rotation parameter q' and the optimal translation parameter t'. According to equation (16), the rotation matrix R' can be obtained from q'. Combining the coarse calibration results, the final transformation matrix T' obtained by fine calibration of the extrinsic parameters is:

[0067] The transformation matrix is ​​applied to the point cloud in the lidar coordinate system to obtain the coordinates of the point cloud in the camera coordinate system. The pixel coordinates of the reprojection point of the point in the image can be obtained by equation (18).

[0068] Compared with existing lidar-camera extrinsic parameter calibration methods, the advantages of this invention through the above method are as follows:

[0069] 1) In the data preprocessing step, a method for automatic extraction of the boundary of the complete calibration board image based on line segment direction vector clustering is proposed, which improves the degree of automation and avoids the errors caused by manual extraction;

[0070] 2) This method uses the theoretical point cloud boundary based on the calibration board size to replace the actual scanning calibration board boundary. It is still effective for sparse point clouds with blurred geometric features or point clouds that are partially occluded. It has lower requirements for lidar and reduces costs.

[0071] 3) In the optimization process of fine adjustment of extrinsic parameters, the boundary reprojection error and normal vector consistency error are included in the error function at the same time, which reduces the error caused by point cloud direction offset and improves accuracy;

[0072] 4) The implementation of this method does not depend on a specific environment and is still applicable in various scenarios such as indoors and outdoors. Attached Figure Description

[0073] Figure 1 The overall flowchart for calibration;

[0074] Figure 2 This is a schematic diagram of sensor installation in the embodiment of step 1 of the present invention;

[0075] Figure 3 This is a schematic diagram illustrating the data collection process in an example embodiment.

[0076] Figure 4 This is the point cloud extraction result of the calibration board in step 2.1.1 of the present invention;

[0077] Figure 5 This is the result of the ray projection and the three-dimensional planar point cloud of the calibration plate in step 2.1.2 of the present invention;

[0078] Figure 6 This is the result of extracting the calibration plate image in step 2.2 of the present invention;

[0079] Figure 7 This is a schematic diagram of extracting the calibration plate image in step 2.2 of the present invention;

[0080] Figure 8 This is a schematic diagram of aligning the theoretical boundary with the actual scanning boundary in step 3.1 of the present invention;

[0081] Figure 9 This is a schematic diagram of the normal vector consistency error in step 4.2.3 of the present invention;

[0082] Figure 10 This is a projection result diagram after the fine calibration is completed in step 4.2.4 of the present invention. Detailed Implementation

[0083] The specific implementation of the lidar-camera extrinsic parameter calibration method of the present invention will be described in detail below with reference to an embodiment. This embodiment uses a hybrid solid-state lidar (field of view 180°×40°) and an area array camera (field of view 56.5°×44.6°) as sensors to calibrate the rigid body transformation parameters from the lidar coordinate system to the camera coordinate system. The overall calibration process is as follows: Figure 1 As shown, the specific steps include the following:

[0084] Step 1: Obtain the point cloud data and image data required for extrinsic parameter calibration. Specifically, install and fix the LiDAR and camera, ensuring they share a common field of view. In this embodiment, the camera is mounted above the LiDAR. Figure 2 As shown in (a), both are mounted together on a 5.2-meter-high support frame, pointing downwards at a 45° angle. Figure 2As shown in (b), a rigid checkerboard calibration board of size w×h is placed within the common field of view, ensuring it is within the effective detection range of both the laser radar and the camera. During data acquisition, the timestamps of the laser radar and camera are synchronized via a switch, and the calibration board is scanned multiple times to obtain the corresponding point cloud and image. During each scan, the calibration board needs to be in different areas of the field of view, and the scan is performed with the calibration board tilted. In this embodiment, the calibration board size is 1.35m×1.01m, the number of corner points is 10×7, and the side length of each square is 0.12m. Scanning is performed in five areas: upper left, upper center, upper right, lower left, and lower center of the field of view. Some of the acquired data is shown below. Figure 3 As shown, where, Figure 3 (a) is an example of a captured image. Figure 3 (b) is an example of the collected point cloud;

[0085] Step 2: Preprocess the acquired point cloud and image data separately. The specific steps are as follows:

[0086] Step 2.1: Perform calibration plate extraction and ray projection operations on the point cloud data:

[0087] Step 2.1.1: Filter out environmental points in the point cloud that are unrelated to the calibration board area based on a spatial threshold. In this embodiment, points located between plane X = -2.2m and plane X = -3.0m are retained, such as... Figure 4 As shown in (a), the ground plane is fitted to the filtered point cloud and the ground point cloud is filtered out to obtain the point cloud P. init Further analysis of the point cloud P including the calibration board init A three-dimensional plane fitting is performed to obtain plane α. According to equation (1), points within a certain distance d near plane α are retained as points of the calibration plate, thus obtaining the point cloud P of the calibration plate. b :

[0088]

[0089] Where (x,y,z) is the point cloud P init The three-dimensional coordinates of a point in the plane are given by A, B, C, and D, which are parameters of plane α. In this embodiment, the plane fitting algorithm uses the RANSAC algorithm. Taking the calibration board point cloud at the lower center position as an example, point cloud P... init like Figure 4 As shown in (b), the parameters of plane α are -0.162752, -0.833781, -0.527562, and 3.40101, respectively, and the distance threshold d is 0.05m. The obtained P... b like Figure 4 As shown in (c);

[0090] Step 2.1.2: Convert the point cloud P, which includes fluctuation points and noise points, into a single point cloud. bBy projecting the ray onto the fitting plane α, the three-dimensional planar point cloud P of the calibration plate is obtained. r As shown in equation (2):

[0091] In this embodiment, taking the calibration board point cloud at the lower middle position as an example, the principle of ray projection is as follows: Figure 5 As shown in (a), the obtained three-dimensional planar point cloud is as follows: Figure 5 As shown in (b);

[0092] Step 2.2: Preprocessing operations for image data, including distortion correction and calibration plate extraction, are as follows: Step 2.2.1: Given the camera's intrinsic parameters K and distortion coefficients dist, the image is distorted using OpenCV's undistort function based on K and dist, resulting in a distortion-free image I. u In this embodiment, the intrinsic parameter K is [[1.35851612e+03,0.0,8.07148238e+02],[0.0,1.36499370e+03,5.96328265e+02],[0.0,0.0,1.0]], and the distortion coefficient dist is [-0.0627365,0.13863059,-0.00103595,-0.00185158,-0.0899572];

[0093] Step 2.2.2: Obtain the chessboard envelope by calling the OpenCV findChessboardCorners function, and expand it outwards to obtain a new envelope that includes the calibration board boundary. In this embodiment, the expansion is 20%, such as... Figure 6 As shown in (a), edge detection is performed within the new envelope (e.g., Figure 6 (as shown in (b)) and line detection are used to obtain a set of all candidate boundary line segments of the calibration board. Where k is the number of all detected line segments, such as Figure 6 As shown in (c), in this embodiment, Canny edge detection and Hough line detection are used. Taking the calibration plate in the lower middle position as an example, k = 29.

[0094] Step 2.2.3: For each line segment in Represents line segment L i starting point, Represents line segment L i In this embodiment, taking the calibration plate at the lower middle position as an example, the first three line segments are [(296,50),(441,223)], [(16,148),(118,250)], and [(314,16),(473,213)]. The unit direction vector v is calculated according to equation (3).i :

[0095] In this embodiment, taking the calibration board at the lower middle position as an example, the unit direction vectors of the first three line segments are (0.642360, 0.766402), (0.707106, 0.707106), and (0.628061, 0.778163), respectively. According to v... i By using clustering methods Divided into two clusters of G v and G h The two segments are used as line segments in the two main directions of the calibration board, as shown in the clustering diagram. Figure 7 As shown in (a), in this embodiment, the clustering method uses the K-Means clustering algorithm. Taking the calibration board in the lower middle position as an example, G v and G h There are 19 line segments and 10 line segments respectively. The clustering results are as follows: Figure 6 As shown in (d);

[0096] Step 2.2.4: For G h Select the first line segment of the cluster and calculate its normal vector n according to equation (4). l :

[0097] For G h For each line segment within the range, calculate all center points c according to equation (5). i The projection CP onto the normal vector, where j represents the number of line segments in the cluster:

[0098]

[0099] Select the two line segments with the largest and smallest projection values ​​as the two outermost line segments L of this cluster. out1 and L out2 As shown in equation (6):

[0100]

[0101] Select G h A schematic diagram of the two outermost line segments is shown below. Figure 7 As shown in (b), similarly, according to step 2.2.4, for G... v The two outermost line segments can also be obtained, along with G. h The outermost line segments together form the four boundary line segments L of the calibration plate. l L r L u L d In this embodiment, taking the calibration plate in the lower middle position as an example, G v and G h nl The values ​​are (-0.76640259, 0.64236055) and (0.30858249, 0.95119758) respectively, G v out1 and out2 are 1 and 13 respectively, G h out1 and out2 are 8 and 5 respectively, resulting in L. r L l L u L d The values ​​are [(315,16),(473,211)],[(16,148),(118,250)],[(15,150),(330,23)],[(175,305),(473,214)];

[0102] Step 2.2.5: For two line segments L a and L b Solve the system of linear equations with respect to t and s:

[0103]

[0104] If the coefficient matrix is ​​of full rank, then L a With L b The coordinates (x, y) of the intersection point are:

[0105]

[0106] Find the four line segments L according to equations (7) and (8). r L l L u L d The intersection of two adjacent straight lines gives the four corner points Cn1, Cn2, Cn3, and Cn4 of the calibration plate, as illustrated in the diagram below. Figure 7 As shown in (c), in this embodiment, taking the calibration plate at the lower center position as an example, Cn1, Cn2, Cn3, and Cn4 are (322.96873274, 25.83482839), (17.13800905, 149.13800905), (173.46786632, 305.46786632), and (474.94862598, 213.40494978), respectively. Figure 6 As shown in (e), the four corner points of the envelope are transformed to I. u In the process, it is determined whether each pixel is within the quadrilateral formed by the four points. If not, the RGB values ​​of that pixel are set to (0,0,0), thus obtaining image I. m The result is as follows Figure 6 As shown in (f);

[0107] Step 2.2.6: Obtain I by calling OpenCV's findContours function. m Contour = {(x1,y1),(x2,y2),…,(x...}}, ... n ,y n The data is used for subsequent calibration. In this embodiment, taking the calibration plate in the lower middle position as an example, the first three contour points of Contour are (904, 846), (903, 847), and (902, 847). Step 3: Perform coarse calibration using the preprocessed data to obtain the initial extrinsic parameters. The specific steps are as follows:

[0108] Step 3.1: Construct the theoretical boundary point cloud of the calibration board based on its actual dimensions, and align it with the actual scanned boundary point cloud. The specific steps are as follows:

[0109] Step 3.1.1: Apply the boundary extraction algorithm to extract the boundary point cloud of the calibration board, and obtain the actual scanned boundary point cloud P. tb Simultaneously, based on the actual dimensions of the calibration plate, a theoretical boundary point cloud P of size w×h is constructed. cb ,like Figure 8 As shown in (a), in this embodiment, the boundary extraction algorithm adopts the Alpha Shapes algorithm, P cb Its dimensions are 1.35m × 1.01m;

[0110] Step 3.1.2: For P cb With P tb Calculate the covariance matrix and perform eigenvalue decomposition to obtain the orthogonal basis V. cb and V tb , with V tb Using the principal direction as a reference, by matching the principal direction and combining it with centroid alignment, we obtain P. cb The transformation matrix to the target plane α is shown in equation (9), where μ cb μ tb P respectively cb With P tb Center of mass:

[0111] In this embodiment, taking the calibration plate in the lower middle position as an example, T Init =[[-0.735604,0.662563,-0.141059,-0.120749],[-0.267314,-0.475248,-0.838262,5.40954],[0.622439,0.578922,-0.526706,-2.07049],[0,0,0,1]], after transformation, we get P' cb =T Init·P cb (After homogenization), make P tb With P' cb The two point clouds are on the same plane α, and preliminary alignment is performed based on the characteristics of the point clouds.

[0112] Step 3.1.3: Place P tb With P' cb According to the normal vector n of plane α p = (A, B, C) is rotated to a plane parallel to the XOY plane, and the rotation matrix is:

[0113] R P =I+sinωK r +(1-cosω)K r 2 (10)

[0114] in:

[0115] In this embodiment, taking the calibration plate in the lower middle position as an example, n p = (-0.162752, -0.833781, -0.527562), rotation matrix R p =[[0.95796,-0.249832,0.141058],[-0.249832,-0.484668,0.838261],[-0.141058,-0.838261,-0.526708]];

[0116] Step 3.1.4: Since the two point clouds are already on the same plane, the transformation between them is only caused by the rotation angle θ and translation t around the Z-axis. x ,t y Decision, such as Figure 8 As shown in (b), a global search algorithm is used to optimize the three parameters. The optimization of the transformation parameters is shown in Equation (12). The error function is defined as the mean square of the distances between each point in the rotated point cloud and its nearest neighbor in the target point cloud:

[0117]

[0118] in, This means finding the point q in the target point cloud that has the closest Euclidean distance to the transformed source point using a KD-tree search. j p i P' cb Point R in Z Let t be a rotation matrix. Z The diagram illustrating the definition of the error function for the translation vector is shown below. Figure 8As shown in (c), in this embodiment, the global search algorithm adopts the Particle Swarm Optimization (PSO) algorithm, where the number of particles is set to 80, and the inertia weight w of the algorithm is... i During the iteration process, the coefficient c1 decreases linearly from 0.9 to 0.4, the cognitive coefficient c1 decreases from 2.5 to 0.5, and the social coefficient c2 increases from 0.5 to 2.5. The initial number of iterations is set to a maximum value, and iteration stops after convergence. Taking the calibration board in the lower middle position as an example, the search angle range is [-90°, 90°], and the search translation range is [-2.0m, 2.0m]. The optimized R is obtained as follows. Z The expression is [[0.979756,0.200191,0],[-0.200191,0.979756,0],[0,0,1]], t Z The values ​​are [0.802194, -0.421475, 0]. T ;

[0119] Step 3.1.5: After obtaining the results of the global search, a local optimization algorithm is used to further optimize them, similar to equation (12), to obtain the optimal parameters θ',t'. x ,t' y Then, construct the homogeneous transformation matrix T according to equation (13). Z :

[0120] In this embodiment, the local optimization algorithm uses the Levenberg-Marquardt (LM) algorithm. Taking the calibration board in the lower middle position as an example, T Z =[[0.976717,0.214529,0,0.744553],[-0.214529,0.976717,0,-0.390395],[0,0,1,0],[0,0,0,1]];

[0121] Step 3.1.6: For P' cb Transform to obtain P” cb =T Z ·P' cb According to R P Rotate back to plane α, which is the actual plane where the calibration plate is located, and finally obtain the aligned theoretical boundary P. eb In this embodiment, taking the calibration plate in the lower middle position as an example, P eb like Figure 8 As shown in (d);

[0122] Step 3.2: Estimate the camera pose using the corresponding point cloud and image center points. The specific steps are as follows:

[0123] Step 3.2.1: Perform the operations of Step 2 and Step 3.1 on the collected s (s≥4) sets of data to obtain the theoretical boundary point cloud set PEB={P eb1 ,…,P ebs} and the calibration plate image set IM = {I m1 ,…,I ms}, where the elements in the two sets have a one-to-one correspondence; in this embodiment, s = 5;

[0124] Step 3.2.2: Calculate the centroid C of the calibration board point cloud based on the theoretical boundary point cloud and the calibration board image boundary obtained in Step 2.2.6. L With image centroid C I We can obtain the set of point cloud center points CL = {C L1 ,…,C Ls} and the set of image center points CI = {C I1 ,…,C Is In this embodiment, CL = {(-0.0259643,4.94552,-1.35737),(-2.12471,4.99015,-1.45348),(-0.0997304,8.06285,1.09545),(-1.7565,7.88615,0.92054),(1.71084,7.91932,1.03249)}, CI = {(834.279,991.819),(266,1007.37),(810,430.455),(532.294,455.227),(1116.64,439.914)};

[0125] Step 3.2.3: Solve the PnP (Perspective-n-Point) problem based on CL and CI to obtain the coarsely calibrated extrinsic parameters, namely the rotation vector rvec and the translation vector tvec. In this embodiment, the PnP problem is solved using the EPnP (Efficient Perspective-n-Point) algorithm, resulting in rvec = [1.56793606167, 0.00229381054120, 0.0120740275567] and tvec = [0.165043685887, 0.0827291633852, 0.0951883789965].

[0126] Step 4: Based on the coarse calibration, perform fine calibration. The specific steps are as follows:

[0127] Step 4.1: Calculate the normal vector of the calibration board point cloud and the normal vector of the calibration board in the image, respectively. The specific steps are as follows:

[0128] Step 4.1.1: Use the rvec and tvec obtained from the coarse calibration to transform the boundary point cloud set PEB, converting the lidar coordinate system to the camera coordinate system, and obtain PCAL = {P cal1 ,…,P cals}, as shown in equation (14):

[0129]

[0130] Wherein, Rodrigues(·) transforms the rotation vector into a Rodrigues rotation matrix, P ebi Let P be the homogeneous coordinate matrix of the point cloud in the lidar coordinate system. cali Let be the homogeneous coordinate matrix of the point cloud in the camera coordinate system. In this embodiment, the transformation matrix is ​​[[0.99993874,-0.00624157,0.00914136,0.16504369],[0.00915907,0.00281423,-0.99995409,0.08272916],[0.00621556,0.99997656,0.00287122,0.09518838],[0,0,0,1]].

[0131] Step 4.1.2: P cali normal vector n ei The normal vector of the plane can be obtained by the plane fitting algorithm. In this embodiment, the plane fitting algorithm adopts the RANSAC algorithm, and the normal vector n ei The values ​​are (0.14063, -0.523038, 0.840627), (0.089979, -0.578645, 0.810601), (0.00665, -0.519807, 0.854258), (-0.00816884, -0.528834, 0.848686), and (0.0789003, -0.505999, 0.858918), respectively; Step 4.1.3: The normal vector n of the calibration board in the image set IM. ci The solution needs to be indirectly calculated using the PnP problem: In the world coordinate system containing the checkerboard (with the top-left corner of the checkerboard as the origin), establish a local coordinate system: the X-axis is to the right, the Y-axis is downward, and the Z-axis is perpendicular to the checkerboard plane and points towards the camera. Under this definition, the normal vector of the checkerboard in the world coordinate system is n. w =(0,0,1) T Then, the OpenCV `findChessboardCorners` function is used to detect the pixel corner coordinates of the chessboard in the image. Combined with the known 3D world coordinates of the corners, the rotation vector `r` from the world coordinate system to the camera coordinate system is solved using the PnP algorithm. niAccording to equation (15), n w Transform to the camera coordinate system using a rotation matrix to obtain n ci :

[0132] n ci =Rodrigues(r ni )·n w (15)

[0133] In this embodiment, the normal vector n ci The values ​​are (-0.140928, 0.555746, -0.819319), (-0.099195, 0.580313, -0.808329), (-0.023583, 0.535641, -0.844115), (-0.014166, 0.545632, -0.837904), and (-0.077845, 0.527702, -0.845854), respectively.

[0134] Step 4.2: Refine the rotation and translation parameters. The specific steps are as follows:

[0135] Step 4.2.1: Represent the rotation using the quaternion q = [w, x, y, z], where quaternion q = w + xi + yj + zk (satisfying w 2 +x 2 +y 2 +z 2 =1) The corresponding rotation matrix R f It can be expressed by equation (16):

[0136] In this embodiment, the values ​​of q in the first three iterations of the optimization process are [0.999997, 0.0019377, 0.00152833, 0.000323981], [0.999994, 0.00242238, 0.00227404, 0.000533656], and [0.999993, 0.00251463, 0.00269473, 0.00067819], respectively;

[0137] Step 4.2.2: According to equation (17), combine all P values ​​through the union operation. cali Merge into P merge In the middle, I mi The corresponding two-dimensional contour point set Contour i Merge into C merge In this embodiment, s = 5;

[0138] Step 4.2.3: Perform point cloud P merge The point (x) inc ,y c ,z c ), and reproject it onto the image according to the reprojection function π(·), then obtain its pixel coordinates (u,v) in the image, as shown in equation (18):

[0139] The Euclidean distance from the reprojection point to the nearest point among the calibration board contour points is used as the error. The calibration board boundary point cloud is forced to be reprojected onto the corresponding boundary of the image. The boundary point reprojection error E edge As shown in equation (19):

[0140]

[0141] Among them, R f Let t be a rotation matrix. f Let η(·) be the translation vector in C. merge Find the nearest point to a given point, and introduce an error E. normal The normal vectors of the point cloud and the calibration plate in the image are parallel to force them to maintain the same direction. A schematic diagram of the normal vector consistency error is shown below. Figure 9 As shown, the absolute value of the dot product of the normal vectors is used to measure the degree of parallelism to avoid the problem of sensitivity to the direction of the normal vector. The normal vector alignment error E normal As shown in equation (20):

[0142] The complete error function E is shown in equation (21), where w e and w n Weights for the two errors:

[0143] E = w e ·E edge +w n ·E normal (twenty one)

[0144] An RMS-based dynamic error weighting method is adopted, w e It is always equal to 1.0. During optimization, w is dynamically adjusted according to equation (22) after each iteration. n :

[0145]

[0146] Where e represents the value of a single error term, and in this embodiment, w n The values ​​for the first three iterations during the optimization process were 8537.72, 8158.11, and 7979.95, respectively.

[0147] Step 4.2.4: Based on the error function E, optimize the rotation and translation parameters using a local optimization algorithm. After convergence, obtain the optimal rotation parameter q' and the optimal translation parameter t'. According to equation (16), the rotation matrix R' can be obtained from q'. Combining the coarse calibration results, the final transformation matrix T' obtained by fine calibration of the extrinsic parameters is:

[0148] The transformation matrix is ​​applied to the point cloud in the lidar coordinate system to obtain the coordinates of the point cloud in the camera coordinate system. The reprojection pixel coordinates of the point in the image can be obtained by equation (18). In this embodiment, the local optimization algorithm adopts the LM algorithm, and the optimized values ​​are q'=[0.999989,0.002261,0.004054,0.000482], t'=[-0.076242,0.063204,0.026197], T' =[[0.9999387,-0.006241574,0.009141359,0.1650437],[0.009159065,0.00281423,-0.9999541,0.08272916],[0.006215562,0.9999766,0.002871224,0.09518838],[0,0,0,1]], the reprojection result is as follows Figure 10 As shown, compared with existing similar methods at home and abroad, the present invention has the smallest calibration rotation error and translation error. The calibration results of each method are shown in Table 1. In this table, "failure" indicates that the method cannot obtain calibration results under this condition.

[0149] Table 1

[0150]

[0151] The embodiments described in this specification are merely examples of implementations of the inventive concept. The scope of protection of this invention should not be considered as limited to the specific forms described in the embodiments. The scope of protection of this invention also extends to equivalent technical means that can be conceived by those skilled in the art based on the inventive concept.

Claims

1. A method for automatic calibration of a lidar-camera system based on boundary features, characterized in that, Includes the following steps: Step 1: Obtain the point cloud data and image data required for external parameter calibration. Specifically, install and fix the lidar and camera, requiring them to have a common field of view. Place a rigid checkerboard calibration plate of size w×h within the common field of view, so that the calibration plate is within the effective detection range of both. During data acquisition, synchronize the timestamps of the lidar and camera through a switch, scan the calibration plate multiple times to obtain the corresponding point cloud and image. During each scan, the calibration plate needs to be in a different area of ​​the field of view, and the scanning should be performed with the calibration plate tilted. Step 2: Preprocess the acquired point cloud and image data respectively: including calibration plate extraction and ray projection operations on the point cloud data, and distortion correction and calibration plate extraction preprocessing operations on the image data. Step 3: Perform coarse calibration using preprocessed data to obtain initial extrinsic parameters: First, construct the theoretical boundary point cloud of the calibration board based on its actual size and align it with the actual scanned boundary point cloud; then, estimate the camera pose using the corresponding point cloud and image center point. Step 4: Based on the coarse calibration, perform fine calibration: First, calculate the normal vector of the calibration board point cloud and the normal vector of the calibration board in the image; then refine and adjust the rotation and translation parameters.

2. The automatic calibration method for lidar-camera based on boundary features according to claim 1, characterized in that, The specific steps of step 2 are as follows: Step 2.1: Perform calibration plate extraction and ray projection operations on the point cloud data: Step 2.1.1: Filter out environmental points in the point cloud that are irrelevant to the calibration plate area based on a spatial threshold. Fit the ground plane to the filtered point cloud and then filter out the ground point cloud to obtain point cloud P. init For the point cloud P containing the calibration board init A three-dimensional plane fitting is performed to obtain plane α. According to equation (1), points within a given distance d near plane α are retained as points of the calibration plate, thus obtaining the point cloud P of the calibration plate. b : Where (x,y,z) is the point cloud P init The three-dimensional coordinates of a point in the plane are given by A, B, C, and D, which are parameters of the plane α. Step 2.1.2: Convert the point cloud P, which includes fluctuation points and noise points, into a single point cloud. b By projecting the ray onto the fitting plane α, the three-dimensional planar point cloud P of the calibration plate is obtained. r As shown in equation (2): Step 2.2: Perform distortion correction and calibration plate extraction preprocessing on the image data, as follows: Step 2.2.1: Given the camera's intrinsic parameters K and distortion coefficients dist, perform distortion correction on the image based on K and dist to obtain a distortion-free image I. u ; Step 2.2.2: Obtain the checkerboard-patterned bounding box and expand it outwards to obtain a new bounding box that includes the calibration board boundary. Perform edge detection and line detection within the new bounding box to obtain a set of all candidate boundary line segments of the calibration board. Where k is the number of all detected line segments; Step 2.2.3: For each line segment in Represents line segment L i starting point, Represents line segment L i The endpoint is calculated using equation (3) to determine its unit direction vector v. i : According to v i By using clustering methods Divided into two clusters of G v and G h These are respectively used as line segments in the two main directions of the calibration plate; Step 2.2.4: For G h Select the first line segment of the cluster and calculate its normal vector n according to equation (4). l : For G h For each line segment within the range, calculate all center points c according to equation (5). i The projection CP onto the normal vector, where j represents the number of line segments in the cluster: Select the two line segments with the largest and smallest projection values ​​as the two outermost line segments L of this cluster. out1 and L out2 As shown in equation (6): Similarly, according to step 2.2.4, for G... v The two outermost line segments can also be obtained, along with G. h The outermost line segments together form the four boundary line segments L of the calibration plate. l L r L u L d ; Step 2.2.5: For two line segments L a and L b Solve the system of linear equations with respect to t and s: If the coefficient matrix is ​​of full rank, then L a With L b The coordinates (x, y) of the intersection point are: Find the four line segments L according to equations (7) and (8). r L l L u L d The intersection of two adjacent straight lines in the middle yields the four corner points Cn1, Cn2, Cn3, and Cn4 of the calibration board. These four corner points within the envelope are then converted to I... u In the process, it is determined whether each pixel is within the quadrilateral formed by the four points. If not, the RGB values ​​of that pixel are set to (0,0,0), thus obtaining image I. m ; Step 2.2.6: Acquire Image I m Contour = {(x1,y1),(x2,y2),…,(x...}}, ... n ,y n )}, used for subsequent calibration.

3. The automatic calibration method for lidar-camera based on boundary features according to claim 2, characterized in that, The specific steps of step 3 are as follows: Step 3.1: Construct the theoretical boundary point cloud of the calibration board based on its actual dimensions, and align it with the actual scanned boundary point cloud, as follows: Step 3.1.1: Apply the boundary extraction algorithm to extract the boundary point cloud of the calibration board, and obtain the actual scanned boundary point cloud P. tb Simultaneously, based on the actual dimensions of the calibration plate, a theoretical boundary point cloud P of size w×h is constructed. cb ; Step 3.1.2: For P cb With P tb Calculate the covariance matrix and perform eigenvalue decomposition to obtain the orthogonal basis V. cb and V tb , with V tb Using the principal direction as a reference, by matching the principal direction and combining it with centroid alignment, we obtain P. cb The transformation matrix to the target plane α is shown in equation (9), where μ cb μ tb P respectively cb With P tb Center of mass: After transformation, we get P' cb =T Init ·P cb , making P tb With P' cb The two point clouds are on the same plane α, and preliminary alignment is performed based on the characteristics of the point clouds. Step 3.1.3: Place P tb With P' cb According to the normal vector n of plane α p = (A, B, C) is rotated to a plane parallel to the XOY plane, and the rotation matrix is: R P =I+sinωK r +(1-cosω)K r 2 (10) in: Where, k r K is the unit vector along the axis of rotation. r It is an antisymmetric matrix; Step 3.1.4: Since the two point clouds are already on the same plane, the transformation between them is only caused by the rotation angle θ and translation t around the Z-axis. x ,t y It was decided to use a global search algorithm to optimize the three parameters. The optimization of the transformation parameters is shown in Equation (12). The error function is defined as the square mean of the distances between each point in the rotated point cloud and its nearest neighbor in the target point cloud, where, This means finding the point q in the target point cloud that has the closest Euclidean distance to the transformed source point using a KD-tree search. j p i P' cb Point R in Z Let t be a rotation matrix. Z For translation vectors: Step 3.1.5: After obtaining the results of the global search, a local optimization algorithm is used to further optimize them to obtain the optimal parameters θ',t'. x ,t' y Then, construct the homogeneous transformation matrix T according to equation (13). Z : Step 3.1.6: For P' cb Transform to obtain P” cb =T Z ·P' cb According to R P Rotate back to plane α, which is the actual plane where the calibration plate is located, and finally obtain the aligned theoretical boundary P. eb ; Step 3.2: Estimate the camera pose using the corresponding point cloud and image center points, as follows: Step 3.2.1: Perform the operations of Step 2 and Step 3.1 on the collected s sets of data to obtain the theoretical boundary point cloud set PEB = {P eb1 ,…,P ebs } and the calibration plate image set IM = {I m1 ,…,I ms }, where the elements in the two sets have a one-to-one correspondence; Step 3.2.2: Calculate the centroid C of the calibration plate point cloud based on the theoretical boundary point cloud set and the calibration plate contour point coordinate set obtained in Step 2.2.

6. L With image centroid C I The set of point cloud center points CL = {C} is obtained. L1 ,…,C Ls } and the set of image center points CI = {C I1 ,…,C Is }; Step 3.2.3: Solve the PnP problem based on CL and CI to obtain the coarsely calibrated extrinsic parameters, namely the rotation vector rvec and the translation vector tvec.

4. The automatic calibration method for lidar-camera based on boundary features according to claim 1, characterized in that, The specific steps of step 4 are as follows: Step 4.1: Calculate the normal vector of the calibration board point cloud and the normal vector of the calibration board in the image, as follows: Step 4.1.1: Use the rvec and tvec obtained from the coarse calibration to transform the boundary point cloud set PEB, converting the lidar coordinate system to the camera coordinate system, and obtain PCAL = {P cal1 ,…,P cals }, as shown in equation (14): Wherein, Rodrigues(·) transforms the rotation vector into a Rodrigues rotation matrix, P ebi Let P be the homogeneous coordinate matrix of the point cloud in the lidar coordinate system. cali Let be the homogeneous coordinate matrix of the point cloud in the camera coordinate system; Step 4.1.2: P cali normal vector n ei The normal vector of the plane is obtained by deriving it from the plane fitting algorithm; Step 4.1.3: The normal vector n of the calibration plate in the image set IM ci The solution needs to be indirectly calculated using the PnP problem: In the world coordinate system containing the checkerboard, establish a local coordinate system: the X-axis is to the right, the Y-axis is downward, and the Z-axis is perpendicular to the checkerboard plane and points towards the camera. The normal vector of the checkerboard in the world coordinate system is n. w =(0,0,1) T Then, the pixel corner coordinates of the checkerboard pattern are detected in the image, and combined with the known 3D world coordinates of the corner points, the rotation vector r from the world coordinate system to the camera coordinate system is solved using the PnP algorithm. ni According to equation (15), n w Transform to the camera coordinate system using a rotation matrix to obtain n ci : n ci =Rodrigues(r ni )·n w (15) Step 4.2: Refine the rotation and translation parameters as follows: Step 4.2.1: Represent the rotation using the quaternion q = [w, x, y, z], and the rotation matrix R corresponding to the quaternion q = w + xi + yj + zk. f This can be expressed by equation (16), where: w satisfies 2 +x 2 +y 2 +z 2 =1; Step 4.2.2: According to equation (17), combine all P values ​​through the union operation. cali Merge into P merge In the middle, I mi The corresponding two-dimensional contour point set Contour i Merge into C merge middle: Step 4.2.3: Perform point cloud P merge The point (x) in c ,y c ,z c ), and reproject it onto the image according to the reprojection function π(·), then obtain its pixel coordinates (u,v) in the image, as shown in equation (18): The Euclidean distance from the reprojection point to the nearest point among the calibration board contour points is used as the error. The calibration board boundary point cloud is forced to be reprojected onto the corresponding boundary of the image. The boundary point reprojection error E edge As shown in equation (19): Among them, R f Let t be a rotation matrix. f Let η(·) be the translation vector in C. merge Find the nearest point to a given point, and introduce an error E. normal To ensure alignment, the normal vectors of the point cloud and the calibration board in the image are made parallel. The absolute value of the dot product of the normal vectors is used to measure the degree of parallelism, avoiding sensitivity issues related to the normal vector direction. The normal vector alignment error E... normal As shown in equation (20): The complete error function E is shown in equation (21), where w e and w n Weights for the two errors: E=w e ·HAVE BEEN edge +w n ·HAVE BEEN normal (21) An RMS-based dynamic error weighting method is adopted, w e It is always equal to 1.

0. During optimization, w is dynamically adjusted according to equation (22) after each iteration. n : Where e represents the value of a single error term, and N represents the number of error terms e; Step 4.2.4: Based on the error function E, optimize the rotation and translation parameters using a local optimization algorithm. After convergence, obtain the optimal rotation parameter q' and the optimal translation parameter t'. According to equation (16), obtain the rotation matrix R' from q'. Combining the coarse calibration results, the final transformation matrix T' obtained by fine calibration of the extrinsic parameters is: The transformation matrix is ​​applied to the point cloud in the lidar coordinate system to obtain the coordinates of the point cloud in the camera coordinate system. The pixel coordinates of the reprojection point of the point in the image can be obtained by equation (18).

Images